Skip to content

Commit

Permalink
[pre-commit.ci] auto fixes from pre-commit.com hooks
Browse files Browse the repository at this point in the history 
for more information, see https://pre-commit.ci
  • Loading branch information
@pre-commit-ci @florent-lamiraux
pre-commit-ci[bot] authored and florent-lamiraux committed Jul 28, 2023
1 parent 1537352 commit 6f46ff4
Show file tree
Hide file tree
Showing 8 changed files with 66 additions and 48 deletions.
6 changes: 4 additions & 2 deletions include/sot/dynamic-pinocchio/angle-estimator.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,7 +68,8 @@ class SOTANGLEESTIMATOR_EXPORT AngleEstimator : public dg::Entity {
contactEmbeddedPositionSIN; // waistRleg
dg::SignalTimeDependent<dynamicgraph::Vector, sigtime_t>
anglesSOUT; // [ flex1 flex2 yaw_drift ]
dg::SignalTimeDependent<MatrixRotation, sigtime_t> flexibilitySOUT; // footRleg
dg::SignalTimeDependent<MatrixRotation, sigtime_t>
flexibilitySOUT; // footRleg
dg::SignalTimeDependent<MatrixRotation, sigtime_t>
driftSOUT; // Ryaw = worldRc est(wRc)^-1
dg::SignalTimeDependent<MatrixRotation, sigtime_t>
Expand All @@ -90,7 +91,8 @@ class SOTANGLEESTIMATOR_EXPORT AngleEstimator : public dg::Entity {
const sigtime_t& time);
MatrixRotation& computeFlexibilityFromAngles(MatrixRotation& res,
const sigtime_t& time);
MatrixRotation& computeDriftFromAngles(MatrixRotation& res, const sigtime_t& time);
MatrixRotation& computeDriftFromAngles(MatrixRotation& res,
const sigtime_t& time);
MatrixRotation& computeSensorWorldRotation(MatrixRotation& res,
const sigtime_t& time);
MatrixRotation& computeWaistWorldRotation(MatrixRotation& res,
Expand Down
3 changes: 2 additions & 1 deletion include/sot/dynamic-pinocchio/force-compensation.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -152,7 +152,8 @@ class SOTFORCECOMPENSATION_EXPORT ForceCompensationPlugin
dg::SignalPtr<dynamicgraph::Vector, sigtime_t> momentumSIN;

/* --- OUTPUTS --- */
dg::SignalTimeDependent<dynamicgraph::Vector, sigtime_t> torsorCompensatedSOUT;
dg::SignalTimeDependent<dynamicgraph::Vector, sigtime_t>
torsorCompensatedSOUT;
dg::SignalTimeDependent<dynamicgraph::Vector, sigtime_t> torsorDeadZoneSOUT;

typedef int sotDummyType;
Expand Down
3 changes: 2 additions & 1 deletion include/sot/dynamic-pinocchio/integrator-force.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,7 +70,8 @@ class SOTINTEGRATORFORCE_EXPORT IntegratorForce : public dg::Entity {
/* Memory of the previous iteration. The sig is fed by the previous
* computations. */
dg::SignalPtr<dynamicgraph::Vector, sigtime_t> velocityPrecSIN;
dg::SignalTimeDependent<dynamicgraph::Vector, sigtime_t> velocityDerivativeSOUT;
dg::SignalTimeDependent<dynamicgraph::Vector, sigtime_t>
velocityDerivativeSOUT;
dg::SignalTimeDependent<dynamicgraph::Vector, sigtime_t> velocitySOUT;

dg::SignalPtr<dynamicgraph::Matrix, sigtime_t> massSIN;
Expand Down
3 changes: 2 additions & 1 deletion include/sot/dynamic-pinocchio/mass-apparent.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,7 +69,8 @@ class SOTMASSAPPARENT_EXPORT MassApparent : public dg::Entity {
public: /* --- FUNCTIONS --- */
dynamicgraph::Matrix& computeMassInverse(dynamicgraph::Matrix& res,
const sigtime_t& time);
dynamicgraph::Matrix& computeMass(dynamicgraph::Matrix& res, const sigtime_t& time);
dynamicgraph::Matrix& computeMass(dynamicgraph::Matrix& res,
const sigtime_t& time);
dynamicgraph::Matrix& computeInertiaInverse(dynamicgraph::Matrix& res,
const sigtime_t& time);
};
Expand Down
8 changes: 4 additions & 4 deletions src/angle-estimator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -265,8 +265,8 @@ MatrixRotation& AngleEstimator::computeDriftFromAngles(MatrixRotation& res,
return res;
}

MatrixRotation& AngleEstimator::computeSensorWorldRotation(MatrixRotation& res,
const sigtime_t& time) {
MatrixRotation& AngleEstimator::computeSensorWorldRotation(
MatrixRotation& res, const sigtime_t& time) {
sotDEBUGIN(15);

const MatrixRotation& worldRworldest = driftSOUT(time);
Expand All @@ -278,8 +278,8 @@ MatrixRotation& AngleEstimator::computeSensorWorldRotation(MatrixRotation& res,
return res;
}

MatrixRotation& AngleEstimator::computeWaistWorldRotation(MatrixRotation& res,
const sigtime_t& time) {
MatrixRotation& AngleEstimator::computeWaistWorldRotation(
MatrixRotation& res, const sigtime_t& time) {
sotDEBUGIN(15);

// chest = sensor
Expand Down
5 changes: 2 additions & 3 deletions src/dynamic-python-module-py.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,9 +18,8 @@ BOOST_PYTHON_MODULE(wrap) {
bp::make_function(&dgs::DynamicPinocchio::getModel,
reference_existing_object()),
bp::make_function(&dgs::DynamicPinocchio::setModel))
.add_property("data",
bp::make_function(&dgs::DynamicPinocchio::getData,
reference_existing_object()))
.add_property("data", bp::make_function(&dgs::DynamicPinocchio::getData,
reference_existing_object()))
.def("setModel", &dgs::DynamicPinocchio::setModel)
.def("createData", &dgs::DynamicPinocchio::createData);
}
82 changes: 48 additions & 34 deletions src/sot-dynamic-pinocchio.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -285,7 +285,8 @@ DynamicPinocchio::~DynamicPinocchio(void) {
// has. if (0!=m_data ) { delete m_data ; m_data =NULL; } if (0!=m_model) {
// delete m_model; m_model=NULL; }

for (std::list<SignalBase<sigtime_t>*>::iterator iter = genericSignalRefs.begin();
for (std::list<SignalBase<sigtime_t>*>::iterator iter =
genericSignalRefs.begin();
iter != genericSignalRefs.end(); ++iter) {
SignalBase<sigtime_t>* sigPtr = *iter;
delete sigPtr;
Expand Down Expand Up @@ -427,7 +428,8 @@ dg::Vector& DynamicPinocchio::getMaxEffortLimits(dg::Vector& res,
}

/* ---------------- INTERNAL ------------------------------------------------ */
dg::Vector& DynamicPinocchio::getPinocchioPos(dg::Vector& q, const sigtime_t& time) {
dg::Vector& DynamicPinocchio::getPinocchioPos(dg::Vector& q,
const sigtime_t& time) {
sotDEBUGIN(15);
dg::Vector qJoints = jointPositionSIN.access(time);
if (!sphericalJoints.empty()) {
Expand Down Expand Up @@ -472,7 +474,8 @@ dg::Vector& DynamicPinocchio::getPinocchioPos(dg::Vector& q, const sigtime_t& ti
return q;
}

dg::Vector& DynamicPinocchio::getPinocchioVel(dg::Vector& v, const sigtime_t& time) {
dg::Vector& DynamicPinocchio::getPinocchioVel(dg::Vector& v,
const sigtime_t& time) {
const Eigen::VectorXd vJoints = jointVelocitySIN.access(time);
if (freeFlyerVelocitySIN) {
const Eigen::VectorXd vFF = freeFlyerVelocitySIN.access(time);
Expand All @@ -486,7 +489,8 @@ dg::Vector& DynamicPinocchio::getPinocchioVel(dg::Vector& v, const sigtime_t& ti
}
}

dg::Vector& DynamicPinocchio::getPinocchioAcc(dg::Vector& a, const sigtime_t& time) {
dg::Vector& DynamicPinocchio::getPinocchioAcc(dg::Vector& a,
const sigtime_t& time) {
const Eigen::VectorXd aJoints = jointAccelerationSIN.access(time);
if (freeFlyerAccelerationSIN) {
const Eigen::VectorXd aFF = freeFlyerAccelerationSIN.access(time);
Expand Down Expand Up @@ -595,8 +599,9 @@ DynamicPinocchio::createPositionSignal(const std::string& signame,
return *sig;
}

SignalTimeDependent<dg::Vector, sigtime_t>& DynamicPinocchio::createVelocitySignal(
const std::string& signame, const std::string& jointName) {
SignalTimeDependent<dg::Vector, sigtime_t>&
DynamicPinocchio::createVelocitySignal(const std::string& signame,
const std::string& jointName) {
sotDEBUGIN(15);
assert(m_model);
long int jointId = m_model->getJointId(jointName);
Expand Down Expand Up @@ -639,7 +644,8 @@ void DynamicPinocchio::destroyJacobianSignal(const std::string& signame) {

bool deletable = false;
dg::SignalTimeDependent<dg::Matrix, sigtime_t>* sig = &jacobiansSOUT(signame);
for (std::list<SignalBase<sigtime_t>*>::iterator iter = genericSignalRefs.begin();
for (std::list<SignalBase<sigtime_t>*>::iterator iter =
genericSignalRefs.begin();
iter != genericSignalRefs.end(); ++iter) {
if ((*iter) == sig) {
genericSignalRefs.erase(iter);
Expand All @@ -662,7 +668,8 @@ void DynamicPinocchio::destroyPositionSignal(const std::string& signame) {
bool deletable = false;
dg::SignalTimeDependent<MatrixHomogeneous, sigtime_t>* sig =
&positionsSOUT(signame);
for (std::list<SignalBase<sigtime_t>*>::iterator iter = genericSignalRefs.begin();
for (std::list<SignalBase<sigtime_t>*>::iterator iter =
genericSignalRefs.begin();
iter != genericSignalRefs.end(); ++iter) {
if ((*iter) == sig) {
genericSignalRefs.erase(iter);
Expand All @@ -686,7 +693,8 @@ void DynamicPinocchio::destroyVelocitySignal(const std::string& signame) {
sotDEBUGIN(15);
bool deletable = false;
SignalTimeDependent<dg::Vector, sigtime_t>* sig = &velocitiesSOUT(signame);
for (std::list<SignalBase<sigtime_t>*>::iterator iter = genericSignalRefs.begin();
for (std::list<SignalBase<sigtime_t>*>::iterator iter =
genericSignalRefs.begin();
iter != genericSignalRefs.end(); ++iter) {
if ((*iter) == sig) {
genericSignalRefs.erase(iter);
Expand All @@ -709,8 +717,10 @@ void DynamicPinocchio::destroyVelocitySignal(const std::string& signame) {
void DynamicPinocchio::destroyAccelerationSignal(const std::string& signame) {
sotDEBUGIN(15);
bool deletable = false;
dg::SignalTimeDependent<dg::Vector, sigtime_t>* sig = &accelerationsSOUT(signame);
for (std::list<SignalBase<sigtime_t>*>::iterator iter = genericSignalRefs.begin();
dg::SignalTimeDependent<dg::Vector, sigtime_t>* sig =
&accelerationsSOUT(signame);
for (std::list<SignalBase<sigtime_t>*>::iterator iter =
genericSignalRefs.begin();
iter != genericSignalRefs.end(); ++iter) {
if ((*iter) == sig) {
genericSignalRefs.erase(iter);
Expand All @@ -735,7 +745,8 @@ void DynamicPinocchio::destroyAccelerationSignal(const std::string& signame) {
/* --------------------- COMPUTE
* ------------------------------------------------- */

dg::Vector& DynamicPinocchio::computeZmp(dg::Vector& res, const sigtime_t& time) {
dg::Vector& DynamicPinocchio::computeZmp(dg::Vector& res,
const sigtime_t& time) {
// TODO: To be verified
sotDEBUGIN(25);
assert(m_data);
Expand Down Expand Up @@ -765,7 +776,8 @@ int& DynamicPinocchio::computeJacobians(int& dummy, const sigtime_t& time) {
sotDEBUGOUT(25);
return dummy;
}
int& DynamicPinocchio::computeForwardKinematics(int& dummy, const sigtime_t& time) {
int& DynamicPinocchio::computeForwardKinematics(int& dummy,
const sigtime_t& time) {
sotDEBUGIN(25);
assert(m_model);
assert(m_data);
Expand Down Expand Up @@ -809,11 +821,9 @@ dg::Matrix& DynamicPinocchio::computeGenericJacobian(const bool isFrame,
return res;
}

dg::Matrix& DynamicPinocchio::computeGenericEndeffJacobian(const bool isFrame,
const bool isLocal,
const int id,
dg::Matrix& res,
const sigtime_t& time) {
dg::Matrix& DynamicPinocchio::computeGenericEndeffJacobian(
const bool isFrame, const bool isLocal, const int id, dg::Matrix& res,
const sigtime_t& time) {
sotDEBUGIN(25);
assert(m_model);
assert(m_data);
Expand Down Expand Up @@ -861,7 +871,8 @@ dg::Matrix& DynamicPinocchio::computeGenericEndeffJacobian(const bool isFrame,
}

MatrixHomogeneous& DynamicPinocchio::computeGenericPosition(
const bool isFrame, const int id, MatrixHomogeneous& res, const sigtime_t& time) {
const bool isFrame, const int id, MatrixHomogeneous& res,
const sigtime_t& time) {
sotDEBUGIN(25);
forwardKinematicsSINTERN(time);
if (isFrame) {
Expand Down Expand Up @@ -890,9 +901,8 @@ dg::Vector& DynamicPinocchio::computeGenericVelocity(const int jointId,
return res;
}

dg::Vector& DynamicPinocchio::computeGenericAcceleration(const int jointId,
dg::Vector& res,
const sigtime_t& time) {
dg::Vector& DynamicPinocchio::computeGenericAcceleration(
const int jointId, dg::Vector& res, const sigtime_t& time) {
sotDEBUGIN(25);
forwardKinematicsSINTERN(time);
res.resize(6);
Expand All @@ -919,15 +929,17 @@ int& DynamicPinocchio::computeNewtonEuler(int& dummy, const sigtime_t& time) {
return dummy;
}

dg::Matrix& DynamicPinocchio::computeJcom(dg::Matrix& Jcom, const sigtime_t& time) {
dg::Matrix& DynamicPinocchio::computeJcom(dg::Matrix& Jcom,
const sigtime_t& time) {
sotDEBUGIN(25);
forwardKinematicsSINTERN(time);
Jcom = pinocchio::jacobianCenterOfMass(*m_model, *m_data, false);
sotDEBUGOUT(25);
return Jcom;
}

dg::Vector& DynamicPinocchio::computeCom(dg::Vector& com, const sigtime_t& time) {
dg::Vector& DynamicPinocchio::computeCom(dg::Vector& com,
const sigtime_t& time) {
sotDEBUGIN(25);
if (JcomSOUT.needUpdate(time)) {
forwardKinematicsSINTERN(time);
Expand All @@ -938,7 +950,8 @@ dg::Vector& DynamicPinocchio::computeCom(dg::Vector& com, const sigtime_t& time)
return com;
}

dg::Matrix& DynamicPinocchio::computeInertia(dg::Matrix& res, const sigtime_t& time) {
dg::Matrix& DynamicPinocchio::computeInertia(dg::Matrix& res,
const sigtime_t& time) {
sotDEBUGIN(25);
const Eigen::VectorXd& q = pinocchioPosSINTERN.access(time);
res = pinocchio::crba(*m_model, *m_data, q);
Expand Down Expand Up @@ -1023,7 +1036,7 @@ dg::SignalTimeDependent<dg::Matrix, sigtime_t>& DynamicPinocchio::jacobiansSOUT(
dg::SignalTimeDependent<dg::Matrix, sigtime_t>& res =
dynamic_cast<dg::SignalTimeDependent<dg::Matrix, sigtime_t>&>(sigabs);
return res;
} catch (const std::bad_cast &e) {
} catch (const std::bad_cast& e) {
SOT_THROW ExceptionSignal(ExceptionSignal::BAD_CAST, "Impossible cast.",
" (while getting signal <%s> of type matrix.",
name.c_str());
Expand All @@ -1034,37 +1047,38 @@ DynamicPinocchio::positionsSOUT(const std::string& name) {
SignalBase<sigtime_t>& sigabs = Entity::getSignal(name);
try {
dg::SignalTimeDependent<MatrixHomogeneous, sigtime_t>& res =
dynamic_cast<dg::SignalTimeDependent<MatrixHomogeneous, sigtime_t>&>(sigabs);
dynamic_cast<dg::SignalTimeDependent<MatrixHomogeneous, sigtime_t>&>(
sigabs);
return res;
} catch (const std::bad_cast &e) {
} catch (const std::bad_cast& e) {
SOT_THROW ExceptionSignal(ExceptionSignal::BAD_CAST, "Impossible cast.",
" (while getting signal <%s> of type matrixHomo.",
name.c_str());
}
}

dg::SignalTimeDependent<dg::Vector, sigtime_t>& DynamicPinocchio::velocitiesSOUT(
const std::string& name) {
dg::SignalTimeDependent<dg::Vector, sigtime_t>&
DynamicPinocchio::velocitiesSOUT(const std::string& name) {
SignalBase<sigtime_t>& sigabs = Entity::getSignal(name);
try {
dg::SignalTimeDependent<dg::Vector, sigtime_t>& res =
dynamic_cast<dg::SignalTimeDependent<dg::Vector, sigtime_t>&>(sigabs);
return res;
} catch (const std::bad_cast &e) {
} catch (const std::bad_cast& e) {
SOT_THROW ExceptionSignal(ExceptionSignal::BAD_CAST, "Impossible cast.",
" (while getting signal <%s> of type Vector.",
name.c_str());
}
}

dg::SignalTimeDependent<dg::Vector, sigtime_t>& DynamicPinocchio::accelerationsSOUT(
const std::string& name) {
dg::SignalTimeDependent<dg::Vector, sigtime_t>&
DynamicPinocchio::accelerationsSOUT(const std::string& name) {
SignalBase<sigtime_t>& sigabs = Entity::getSignal(name);
try {
dg::SignalTimeDependent<dg::Vector, sigtime_t>& res =
dynamic_cast<dg::SignalTimeDependent<dg::Vector, sigtime_t>&>(sigabs);
return res;
} catch (const std::bad_cast &e) {
} catch (const std::bad_cast& e) {
SOT_THROW ExceptionSignal(ExceptionSignal::BAD_CAST, "Impossible cast.",
" (while getting signal <%s> of type Vector.",
name.c_str());
Expand Down
4 changes: 2 additions & 2 deletions src/zmp-from-forces.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,10 +21,10 @@ namespace dynamic {
using dynamicgraph::Entity;
using dynamicgraph::SignalPtr;
using dynamicgraph::SignalTimeDependent;
using dynamicgraph::sigtime_t;
using dynamicgraph::Vector;
using dynamicgraph::sot::MatrixHomogeneous;
using dynamicgraph::sigtime_t;


class ZmpFromForces : public Entity {
DYNAMIC_GRAPH_ENTITY_DECL();

Expand Down

0 comments on commit 6f46ff4

Please sign in to comment.