Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Use long int for signal time #110

Merged
merged 3 commits into from
Jul 28, 2023
Merged

Use long int for signal time #110

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
0ae127c
Use int64_t for signal time type.
florent-lamiraux Jun 16, 2023
e9865f2
[DynamicPinocchio] Remove deprecated method.
florent-lamiraux Jun 23, 2023
50aee41
[pre-commit.ci] auto fixes from pre-commit.com hooks
pre-commit-ci[bot] Jun 23, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
50 changes: 26 additions & 24 deletions include/sot/dynamic-pinocchio/angle-estimator.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -58,51 +58,53 @@ class SOTANGLEESTIMATOR_EXPORT AngleEstimator : public dg::Entity {
virtual ~AngleEstimator(void);

public: /* --- SIGNAL --- */
dg::SignalPtr<MatrixRotation, int>
dg::SignalPtr<MatrixRotation, sigtime_t>
sensorWorldRotationSIN; // estimate(worldRc)
dg::SignalPtr<MatrixHomogeneous, int>
dg::SignalPtr<MatrixHomogeneous, sigtime_t>
sensorEmbeddedPositionSIN; // waistRchest
dg::SignalPtr<MatrixHomogeneous, int>
dg::SignalPtr<MatrixHomogeneous, sigtime_t>
contactWorldPositionSIN; // estimate(worldRf)
dg::SignalPtr<MatrixHomogeneous, int>
dg::SignalPtr<MatrixHomogeneous, sigtime_t>
contactEmbeddedPositionSIN; // waistRleg
dg::SignalTimeDependent<dynamicgraph::Vector, int>
dg::SignalTimeDependent<dynamicgraph::Vector, sigtime_t>
anglesSOUT; // [ flex1 flex2 yaw_drift ]
dg::SignalTimeDependent<MatrixRotation, int> flexibilitySOUT; // footRleg
dg::SignalTimeDependent<MatrixRotation, int>
dg::SignalTimeDependent<MatrixRotation, sigtime_t>
flexibilitySOUT; // footRleg
dg::SignalTimeDependent<MatrixRotation, sigtime_t>
driftSOUT; // Ryaw = worldRc est(wRc)^-1
dg::SignalTimeDependent<MatrixRotation, int>
dg::SignalTimeDependent<MatrixRotation, sigtime_t>
sensorWorldRotationSOUT; // worldRc
dg::SignalTimeDependent<MatrixRotation, int>
dg::SignalTimeDependent<MatrixRotation, sigtime_t>
waistWorldRotationSOUT; // worldRwaist
dg::SignalTimeDependent<MatrixHomogeneous, int>
dg::SignalTimeDependent<MatrixHomogeneous, sigtime_t>
waistWorldPositionSOUT; // worldMwaist
dg::SignalTimeDependent<dynamicgraph::Vector, int>
dg::SignalTimeDependent<dynamicgraph::Vector, sigtime_t>
waistWorldPoseRPYSOUT; // worldMwaist

dg::SignalPtr<dynamicgraph::Matrix, int> jacobianSIN;
dg::SignalPtr<dynamicgraph::Vector, int> qdotSIN;
dg::SignalTimeDependent<dynamicgraph::Vector, int> xff_dotSOUT;
dg::SignalTimeDependent<dynamicgraph::Vector, int> qdotSOUT;
dg::SignalPtr<dynamicgraph::Matrix, sigtime_t> jacobianSIN;
dg::SignalPtr<dynamicgraph::Vector, sigtime_t> qdotSIN;
dg::SignalTimeDependent<dynamicgraph::Vector, sigtime_t> xff_dotSOUT;
dg::SignalTimeDependent<dynamicgraph::Vector, sigtime_t> qdotSOUT;

public: /* --- FUNCTIONS --- */
dynamicgraph::Vector& computeAngles(dynamicgraph::Vector& res,
const int& time);
const sigtime_t& time);
MatrixRotation& computeFlexibilityFromAngles(MatrixRotation& res,
const int& time);
MatrixRotation& computeDriftFromAngles(MatrixRotation& res, const int& time);
const sigtime_t& time);
MatrixRotation& computeDriftFromAngles(MatrixRotation& res,
const sigtime_t& time);
MatrixRotation& computeSensorWorldRotation(MatrixRotation& res,
const int& time);
const sigtime_t& time);
MatrixRotation& computeWaistWorldRotation(MatrixRotation& res,
const int& time);
const sigtime_t& time);
MatrixHomogeneous& computeWaistWorldPosition(MatrixHomogeneous& res,
const int& time);
const sigtime_t& time);
dynamicgraph::Vector& computeWaistWorldPoseRPY(dynamicgraph::Vector& res,
const int& time);
const sigtime_t& time);
dynamicgraph::Vector& compute_xff_dotSOUT(dynamicgraph::Vector& res,
const int& time);
const sigtime_t& time);
dynamicgraph::Vector& compute_qdotSOUT(dynamicgraph::Vector& res,
const int& time);
const sigtime_t& time);

public: /* --- PARAMS --- */
void fromSensor(const bool& fs) { fromSensor_ = fs; }
Expand Down
140 changes: 68 additions & 72 deletions include/sot/dynamic-pinocchio/dynamic-pinocchio.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -93,78 +93,78 @@ class SOTDYNAMIC_EXPORT DynamicPinocchio : public dg::Entity {

public:
/* --- SIGNAL ACTIVATION --- */
dg::SignalTimeDependent<dg::Matrix, int>& createEndeffJacobianSignal(
dg::SignalTimeDependent<dg::Matrix, sigtime_t>& createEndeffJacobianSignal(
const std::string& signame, const std::string&,
const bool isLocal = true);
dg::SignalTimeDependent<dg::Matrix, int>& createJacobianSignal(
dg::SignalTimeDependent<dg::Matrix, sigtime_t>& createJacobianSignal(
const std::string& signame, const std::string&);
void destroyJacobianSignal(const std::string& signame);

dg::SignalTimeDependent<MatrixHomogeneous, int>& createPositionSignal(
dg::SignalTimeDependent<MatrixHomogeneous, sigtime_t>& createPositionSignal(
const std::string&, const std::string&);
void destroyPositionSignal(const std::string& signame);

dg::SignalTimeDependent<dg::Vector, int>& createVelocitySignal(
dg::SignalTimeDependent<dg::Vector, sigtime_t>& createVelocitySignal(
const std::string&, const std::string&);
void destroyVelocitySignal(const std::string& signame);

dg::SignalTimeDependent<dg::Vector, int>& createAccelerationSignal(
dg::SignalTimeDependent<dg::Vector, sigtime_t>& createAccelerationSignal(
const std::string&, const std::string&);
void destroyAccelerationSignal(const std::string& signame);

/*! @} */
std::list<dg::SignalBase<int>*> genericSignalRefs;
std::list<dg::SignalBase<sigtime_t>*> genericSignalRefs;

public:
/* --- SIGNAL --- */
typedef int Dummy;
dg::SignalPtr<dg::Vector, int> jointPositionSIN;
dg::SignalPtr<dg::Vector, int> freeFlyerPositionSIN;
dg::SignalPtr<dg::Vector, int> jointVelocitySIN;
dg::SignalPtr<dg::Vector, int> freeFlyerVelocitySIN;
dg::SignalPtr<dg::Vector, int> jointAccelerationSIN;
dg::SignalPtr<dg::Vector, int> freeFlyerAccelerationSIN;

dg::SignalTimeDependent<dg::Vector, int> pinocchioPosSINTERN;
dg::SignalTimeDependent<dg::Vector, int> pinocchioVelSINTERN;
dg::SignalTimeDependent<dg::Vector, int> pinocchioAccSINTERN;

dg::SignalTimeDependent<Dummy, int> newtonEulerSINTERN;
dg::SignalTimeDependent<Dummy, int> jacobiansSINTERN;
dg::SignalTimeDependent<Dummy, int> forwardKinematicsSINTERN;
dg::SignalTimeDependent<Dummy, int> ccrbaSINTERN;

int& computeNewtonEuler(int& dummy, const int& time);
int& computeForwardKinematics(int& dummy, const int& time);
int& computeCcrba(int& dummy, const int& time);
int& computeJacobians(int& dummy, const int& time);

dg::SignalTimeDependent<dg::Vector, int> zmpSOUT;
dg::SignalTimeDependent<dg::Matrix, int> JcomSOUT;
dg::SignalTimeDependent<dg::Vector, int> comSOUT;
dg::SignalTimeDependent<dg::Matrix, int> inertiaSOUT;

dg::SignalTimeDependent<dg::Matrix, int>& jacobiansSOUT(
dg::SignalPtr<dg::Vector, sigtime_t> jointPositionSIN;
dg::SignalPtr<dg::Vector, sigtime_t> freeFlyerPositionSIN;
dg::SignalPtr<dg::Vector, sigtime_t> jointVelocitySIN;
dg::SignalPtr<dg::Vector, sigtime_t> freeFlyerVelocitySIN;
dg::SignalPtr<dg::Vector, sigtime_t> jointAccelerationSIN;
dg::SignalPtr<dg::Vector, sigtime_t> freeFlyerAccelerationSIN;

dg::SignalTimeDependent<dg::Vector, sigtime_t> pinocchioPosSINTERN;
dg::SignalTimeDependent<dg::Vector, sigtime_t> pinocchioVelSINTERN;
dg::SignalTimeDependent<dg::Vector, sigtime_t> pinocchioAccSINTERN;

dg::SignalTimeDependent<Dummy, sigtime_t> newtonEulerSINTERN;
dg::SignalTimeDependent<Dummy, sigtime_t> jacobiansSINTERN;
dg::SignalTimeDependent<Dummy, sigtime_t> forwardKinematicsSINTERN;
dg::SignalTimeDependent<Dummy, sigtime_t> ccrbaSINTERN;

int& computeNewtonEuler(int& dummy, const sigtime_t& time);
int& computeForwardKinematics(int& dummy, const sigtime_t& time);
int& computeCcrba(int& dummy, const sigtime_t& time);
int& computeJacobians(int& dummy, const sigtime_t& time);

dg::SignalTimeDependent<dg::Vector, sigtime_t> zmpSOUT;
dg::SignalTimeDependent<dg::Matrix, sigtime_t> JcomSOUT;
dg::SignalTimeDependent<dg::Vector, sigtime_t> comSOUT;
dg::SignalTimeDependent<dg::Matrix, sigtime_t> inertiaSOUT;

dg::SignalTimeDependent<dg::Matrix, sigtime_t>& jacobiansSOUT(
const std::string& name);
dg::SignalTimeDependent<MatrixHomogeneous, int>& positionsSOUT(
dg::SignalTimeDependent<MatrixHomogeneous, sigtime_t>& positionsSOUT(
const std::string& name);
dg::SignalTimeDependent<dg::Vector, int>& velocitiesSOUT(
dg::SignalTimeDependent<dg::Vector, sigtime_t>& velocitiesSOUT(
const std::string& name);
dg::SignalTimeDependent<dg::Vector, int>& accelerationsSOUT(
dg::SignalTimeDependent<dg::Vector, sigtime_t>& accelerationsSOUT(
const std::string& name);

dg::SignalTimeDependent<double, int> footHeightSOUT;
dg::SignalTimeDependent<dg::Vector, int> upperJlSOUT;
dg::SignalTimeDependent<dg::Vector, int> lowerJlSOUT;
dg::SignalTimeDependent<dg::Vector, int> upperVlSOUT;
dg::SignalTimeDependent<dg::Vector, int> upperTlSOUT;
dg::SignalTimeDependent<double, sigtime_t> footHeightSOUT;
dg::SignalTimeDependent<dg::Vector, sigtime_t> upperJlSOUT;
dg::SignalTimeDependent<dg::Vector, sigtime_t> lowerJlSOUT;
dg::SignalTimeDependent<dg::Vector, sigtime_t> upperVlSOUT;
dg::SignalTimeDependent<dg::Vector, sigtime_t> upperTlSOUT;

dg::Signal<dg::Vector, int> inertiaRotorSOUT;
dg::Signal<dg::Vector, int> gearRatioSOUT;
dg::SignalTimeDependent<dg::Matrix, int> inertiaRealSOUT;
dg::SignalTimeDependent<dg::Vector, int> MomentaSOUT;
dg::SignalTimeDependent<dg::Vector, int> AngularMomentumSOUT;
dg::SignalTimeDependent<dg::Vector, int> dynamicDriftSOUT;
dg::Signal<dg::Vector, sigtime_t> inertiaRotorSOUT;
dg::Signal<dg::Vector, sigtime_t> gearRatioSOUT;
dg::SignalTimeDependent<dg::Matrix, sigtime_t> inertiaRealSOUT;
dg::SignalTimeDependent<dg::Vector, sigtime_t> MomentaSOUT;
dg::SignalTimeDependent<dg::Vector, sigtime_t> AngularMomentumSOUT;
dg::SignalTimeDependent<dg::Vector, sigtime_t> dynamicDriftSOUT;

public:
/* --- CONSTRUCTOR --- */
Expand All @@ -182,10 +182,6 @@ class SOTDYNAMIC_EXPORT DynamicPinocchio : public dg::Entity {

void createData();

/// \deprecated this function does nothing. This class has its own
/// pinocchio::Data object, which can be access with \ref getData.
void setData(pinocchio::Data*) SOT_DYNAMIC_PINOCCHIO_DEPRECATED;

pinocchio::Model* getModel() { return m_model; };

pinocchio::Data* getData() { return m_data.get(); };
Expand All @@ -196,54 +192,54 @@ class SOTDYNAMIC_EXPORT DynamicPinocchio : public dg::Entity {
///
/// \param[out] result vector
/// \return result vector
dg::Vector& getLowerPositionLimits(dg::Vector& res, const int&) const;
dg::Vector& getLowerPositionLimits(dg::Vector& res, const sigtime_t&) const;

/// \brief Get joint position upper limits
///
/// \param[out] result vector
/// \return result vector
dg::Vector& getUpperPositionLimits(dg::Vector& res, const int&) const;
dg::Vector& getUpperPositionLimits(dg::Vector& res, const sigtime_t&) const;

/// \brief Get joint velocity upper limits
///
/// \param[out] result vector
/// \return result vector
dg::Vector& getUpperVelocityLimits(dg::Vector& res, const int&) const;
dg::Vector& getUpperVelocityLimits(dg::Vector& res, const sigtime_t&) const;

/// \brief Get joint effort upper limits
///
/// \param[out] result vector
/// \return result vector
dg::Vector& getMaxEffortLimits(dg::Vector& res, const int&) const;
dg::Vector& getMaxEffortLimits(dg::Vector& res, const sigtime_t&) const;

// dg::Vector& getAnklePositionInFootFrame() const;

protected:
dg::Matrix& computeGenericJacobian(const bool isFrame, const int jointId,
dg::Matrix& res, const int& time);
dg::Matrix& res, const sigtime_t& time);
dg::Matrix& computeGenericEndeffJacobian(const bool isFrame,
const bool isLocal,
const int jointId, dg::Matrix& res,
const int& time);
const sigtime_t& time);
MatrixHomogeneous& computeGenericPosition(const bool isFrame,
const int jointId,
MatrixHomogeneous& res,
const int& time);
const sigtime_t& time);
dg::Vector& computeGenericVelocity(const int jointId, dg::Vector& res,
const int& time);
const sigtime_t& time);
dg::Vector& computeGenericAcceleration(const int jointId, dg::Vector& res,
const int& time);
const sigtime_t& time);

dg::Vector& computeZmp(dg::Vector& res, const int& time);
dg::Vector& computeMomenta(dg::Vector& res, const int& time);
dg::Vector& computeAngularMomentum(dg::Vector& res, const int& time);
dg::Matrix& computeJcom(dg::Matrix& res, const int& time);
dg::Vector& computeCom(dg::Vector& res, const int& time);
dg::Matrix& computeInertia(dg::Matrix& res, const int& time);
dg::Matrix& computeInertiaReal(dg::Matrix& res, const int& time);
double& computeFootHeight(double& res, const int& time);
dg::Vector& computeZmp(dg::Vector& res, const sigtime_t& time);
dg::Vector& computeMomenta(dg::Vector& res, const sigtime_t& time);
dg::Vector& computeAngularMomentum(dg::Vector& res, const sigtime_t& time);
dg::Matrix& computeJcom(dg::Matrix& res, const sigtime_t& time);
dg::Vector& computeCom(dg::Vector& res, const sigtime_t& time);
dg::Matrix& computeInertia(dg::Matrix& res, const sigtime_t& time);
dg::Matrix& computeInertiaReal(dg::Matrix& res, const sigtime_t& time);
double& computeFootHeight(double& res, const sigtime_t& time);

dg::Vector& computeTorqueDrift(dg::Vector& res, const int& time);
dg::Vector& computeTorqueDrift(dg::Vector& res, const sigtime_t& time);

public: /* --- PARAMS --- */
void cmd_createOpPointSignals(const std::string& sig, const std::string& j);
Expand All @@ -262,9 +258,9 @@ class SOTDYNAMIC_EXPORT DynamicPinocchio : public dg::Entity {
/// \brief map of joints in construction.
/// map: jointName -> (jointType,jointPosition (in parent frame), function_ptr
/// to pinocchio Joint)
dg::Vector& getPinocchioPos(dg::Vector& q, const int& time);
dg::Vector& getPinocchioVel(dg::Vector& v, const int& time);
dg::Vector& getPinocchioAcc(dg::Vector& a, const int& time);
dg::Vector& getPinocchioPos(dg::Vector& q, const sigtime_t& time);
dg::Vector& getPinocchioVel(dg::Vector& v, const sigtime_t& time);
dg::Vector& getPinocchioAcc(dg::Vector& a, const sigtime_t& time);

//\brief Index list for the first dof of (spherical joints)/ (spherical part
// of free-flyer joint).
Expand Down
49 changes: 25 additions & 24 deletions include/sot/dynamic-pinocchio/force-compensation.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -125,41 +125,42 @@ class SOTFORCECOMPENSATION_EXPORT ForceCompensationPlugin

public: /* --- SIGNAL --- */
/* --- INPUTS --- */
dg::SignalPtr<dynamicgraph::Vector, int> torsorSIN;
dg::SignalPtr<MatrixRotation, int> worldRhandSIN;
dg::SignalPtr<dynamicgraph::Vector, sigtime_t> torsorSIN;
dg::SignalPtr<MatrixRotation, sigtime_t> worldRhandSIN;

/* --- CONSTANTS --- */
dg::SignalPtr<MatrixRotation, int> handRsensorSIN;
dg::SignalPtr<dynamicgraph::Vector, int> translationSensorComSIN;
dg::SignalPtr<dynamicgraph::Vector, int> gravitySIN;
dg::SignalPtr<dynamicgraph::Vector, int> precompensationSIN;
dg::SignalPtr<dynamicgraph::Matrix, int> gainSensorSIN;
dg::SignalPtr<dynamicgraph::Vector, int> deadZoneLimitSIN;
dg::SignalPtr<dynamicgraph::Vector, int> transSensorJointSIN;
dg::SignalPtr<dynamicgraph::Matrix, int> inertiaJointSIN;

dg::SignalPtr<dynamicgraph::Vector, int> velocitySIN;
dg::SignalPtr<dynamicgraph::Vector, int> accelerationSIN;
dg::SignalPtr<MatrixRotation, sigtime_t> handRsensorSIN;
dg::SignalPtr<dynamicgraph::Vector, sigtime_t> translationSensorComSIN;
dg::SignalPtr<dynamicgraph::Vector, sigtime_t> gravitySIN;
dg::SignalPtr<dynamicgraph::Vector, sigtime_t> precompensationSIN;
dg::SignalPtr<dynamicgraph::Matrix, sigtime_t> gainSensorSIN;
dg::SignalPtr<dynamicgraph::Vector, sigtime_t> deadZoneLimitSIN;
dg::SignalPtr<dynamicgraph::Vector, sigtime_t> transSensorJointSIN;
dg::SignalPtr<dynamicgraph::Matrix, sigtime_t> inertiaJointSIN;

dg::SignalPtr<dynamicgraph::Vector, sigtime_t> velocitySIN;
dg::SignalPtr<dynamicgraph::Vector, sigtime_t> accelerationSIN;

/* --- INTERMEDIATE OUTPUTS --- */
dg::SignalTimeDependent<MatrixForce, int> handXworldSOUT;
dg::SignalTimeDependent<MatrixForce, int> handVsensorSOUT;
dg::SignalPtr<dynamicgraph::Vector, int> torsorDeadZoneSIN;
dg::SignalTimeDependent<MatrixForce, sigtime_t> handXworldSOUT;
dg::SignalTimeDependent<MatrixForce, sigtime_t> handVsensorSOUT;
dg::SignalPtr<dynamicgraph::Vector, sigtime_t> torsorDeadZoneSIN;

dg::SignalTimeDependent<MatrixForce, int> sensorXhandSOUT;
// dg::SignalTimeDependent<dynamicgraph::Matrix,int> inertiaSensorSOUT;
dg::SignalTimeDependent<dynamicgraph::Vector, int> momentumSOUT;
dg::SignalPtr<dynamicgraph::Vector, int> momentumSIN;
dg::SignalTimeDependent<MatrixForce, sigtime_t> sensorXhandSOUT;
// dg::SignalTimeDependent<dynamicgraph::Matrix,sigtime_t> inertiaSensorSOUT;
dg::SignalTimeDependent<dynamicgraph::Vector, sigtime_t> momentumSOUT;
dg::SignalPtr<dynamicgraph::Vector, sigtime_t> momentumSIN;

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

typedef int sotDummyType;
dg::SignalTimeDependent<sotDummyType, int> calibrationTrigerSOUT;
dg::SignalTimeDependent<sotDummyType, sigtime_t> calibrationTrigerSOUT;

public: /* --- COMMANDLINE --- */
sotDummyType& calibrationTriger(sotDummyType& dummy, int time);
sotDummyType& calibrationTriger(sotDummyType& dummy, sigtime_t time);
};

} // namespace sot
Expand Down
2 changes: 1 addition & 1 deletion include/sot/dynamic-pinocchio/integrator-force-exact.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -63,7 +63,7 @@ class SOTINTEGRATORFORCEEXACT_EXPORT IntegratorForceExact
public: /* --- SIGNAL --- */
public: /* --- FUNCTIONS --- */
dynamicgraph::Vector& computeVelocityExact(dynamicgraph::Vector& res,
const int& time);
const sigtime_t& time);
};

} /* namespace sot */
Expand Down
2 changes: 1 addition & 1 deletion include/sot/dynamic-pinocchio/integrator-force-rk4.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -62,7 +62,7 @@ class SOTINTEGRATORFORCERK4_EXPORT IntegratorForceRK4 : public IntegratorForce {
public: /* --- SIGNAL --- */
public: /* --- FUNCTIONS --- */
dynamicgraph::Vector& computeDerivativeRK4(dynamicgraph::Vector& res,
const int& time);
const sigtime_t& time);
};

} /* namespace sot */
Expand Down
Loading