Commit 3a4f933e authored by Rohan Budhiraja's avatar Rohan Budhiraja
Browse files

[sot-v3] Start moving to sot-v3.

parent a558431c
......@@ -50,10 +50,9 @@ SEARCH_FOR_BOOST()
SEARCH_FOR_EIGEN()
ADD_REQUIRED_DEPENDENCY("jrl-mal >= 1.8.0")
ADD_REQUIRED_DEPENDENCY("dynamic-graph >= 1.0.0")
ADD_REQUIRED_DEPENDENCY("dynamic-graph-python")
ADD_REQUIRED_DEPENDENCY("sot-core")
ADD_REQUIRED_DEPENDENCY("dynamic-graph >= 3.0.0")
ADD_REQUIRED_DEPENDENCY("dynamic-graph-python >=3.0.0")
ADD_REQUIRED_DEPENDENCY("sot-core >= 3.0.0")
ADD_REQUIRED_DEPENDENCY("pinocchio >= 1.2")
ADD_REQUIRED_DEPENDENCY("PinInvDyn")
......@@ -136,7 +135,6 @@ SET_TARGET_PROPERTIES(${LIBRARY_NAME}
SOVERSION ${PROJECT_VERSION}
INSTALL_RPATH ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR})
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} jrl-mal)
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} dynamic-graph)
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} dynamic-graph-python)
PKG_CONFIG_USE_DEPENDENCY(${LIBRARY_NAME} sot-core)
......
......@@ -78,28 +78,28 @@ namespace dynamicgraph {
void init(const double& dt);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(base6d_encoders, ml::Vector);
DECLARE_SIGNAL_IN(jointsVelocities, ml::Vector);
DECLARE_SIGNAL_IN(Kd, ml::Vector);
DECLARE_SIGNAL_IN(Kf, ml::Vector);
DECLARE_SIGNAL_IN(fRightFootRef, ml::Vector); /// 6d reference force
DECLARE_SIGNAL_IN(fLeftFootRef, ml::Vector); /// 6d reference force
DECLARE_SIGNAL_IN(fRightHandRef, ml::Vector); /// 6d reference force
DECLARE_SIGNAL_IN(fLeftHandRef, ml::Vector); /// 6d reference force
DECLARE_SIGNAL_IN(fRightFoot, ml::Vector); /// 6d estimated force
DECLARE_SIGNAL_IN(fLeftFoot, ml::Vector); /// 6d estimated force
DECLARE_SIGNAL_IN(fRightHand, ml::Vector); /// 6d estimated force
DECLARE_SIGNAL_IN(fLeftHand, ml::Vector); /// 6d estimated force
DECLARE_SIGNAL_IN(controlledJoints, ml::Vector); /// mask with 1 for controlled joints, 0 otherwise
DECLARE_SIGNAL_IN(damping, ml::Vector); /// damping factors used for the 4 end-effectors
DECLARE_SIGNAL_OUT(qDes, ml::Vector); /// integral of dqDes
DECLARE_SIGNAL_IN(base6d_encoders, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(jointsVelocities, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(Kd, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(Kf, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(fRightFootRef, dynamicgraph::Vector); /// 6d reference force
DECLARE_SIGNAL_IN(fLeftFootRef, dynamicgraph::Vector); /// 6d reference force
DECLARE_SIGNAL_IN(fRightHandRef, dynamicgraph::Vector); /// 6d reference force
DECLARE_SIGNAL_IN(fLeftHandRef, dynamicgraph::Vector); /// 6d reference force
DECLARE_SIGNAL_IN(fRightFoot, dynamicgraph::Vector); /// 6d estimated force
DECLARE_SIGNAL_IN(fLeftFoot, dynamicgraph::Vector); /// 6d estimated force
DECLARE_SIGNAL_IN(fRightHand, dynamicgraph::Vector); /// 6d estimated force
DECLARE_SIGNAL_IN(fLeftHand, dynamicgraph::Vector); /// 6d estimated force
DECLARE_SIGNAL_IN(controlledJoints, dynamicgraph::Vector); /// mask with 1 for controlled joints, 0 otherwise
DECLARE_SIGNAL_IN(damping, dynamicgraph::Vector); /// damping factors used for the 4 end-effectors
DECLARE_SIGNAL_OUT(qDes, dynamicgraph::Vector); /// integral of dqDes
// DEBUG SIGNALS
DECLARE_SIGNAL_OUT(dqDes, ml::Vector); /// dqDes = J^+ * Kf * (fRef-f)
DECLARE_SIGNAL_OUT(fRightFootError, ml::Vector); /// fRef-f
DECLARE_SIGNAL_OUT(fLeftFootError, ml::Vector); /// fRef-f
DECLARE_SIGNAL_OUT(fRightHandError, ml::Vector); /// fRef-f
DECLARE_SIGNAL_OUT(fLeftHandError, ml::Vector); /// fRef-f
DECLARE_SIGNAL_OUT(dqDes, dynamicgraph::Vector); /// dqDes = J^+ * Kf * (fRef-f)
DECLARE_SIGNAL_OUT(fRightFootError, dynamicgraph::Vector); /// fRef-f
DECLARE_SIGNAL_OUT(fLeftFootError, dynamicgraph::Vector); /// fRef-f
DECLARE_SIGNAL_OUT(fRightHandError, dynamicgraph::Vector); /// fRef-f
DECLARE_SIGNAL_OUT(fLeftHandError, dynamicgraph::Vector); /// fRef-f
/* --- COMMANDS --- */
......
......@@ -93,23 +93,23 @@ namespace dynamicgraph {
void init(const double& dt);
/* --- SIGNALS --- */
std::vector<dynamicgraph::SignalPtr<ml::Vector,int>*> m_ctrlInputsSIN;
std::vector<dynamicgraph::Signal<ml::Vector,int>*> m_jointsCtrlModesSOUT;
DECLARE_SIGNAL_IN(base6d_encoders, ml::Vector);
DECLARE_SIGNAL_IN(dq, ml::Vector); /// Joint velocities; used to compensate for BEMF effect on low level current loop
DECLARE_SIGNAL_IN(bemfFactor, ml::Vector); /// Link betwin velocity and current; to compensate for BEMF effect on low level current loop (in A/rad.s-1)
DECLARE_SIGNAL_IN(tau, ml::Vector); /// estimated joint torques (using dynamic robot model + F/T sensors)
DECLARE_SIGNAL_IN(tau_predicted, ml::Vector); /// predicted joint torques (using motor model)
DECLARE_SIGNAL_IN(max_current, ml::Vector); /// max current allowed before stopping the controller (in Ampers)
DECLARE_SIGNAL_IN(max_tau, ml::Vector); /// max torque allowed before stopping the controller
DECLARE_SIGNAL_IN(percentageDriverDeadZoneCompensation, ml::Vector); /// percentatge in [0;1] of the motor driver dead zone that we should compensate 0 is none, 1 is all of it
DECLARE_SIGNAL_IN(signWindowsFilterSize, ml::Vector); /// windows size to detect changing of control sign (to then apply motor driver dead zone compensation) 0 is no filter. 1,2,3...
std::vector<dynamicgraph::SignalPtr<dynamicgraph::Vector,int>*> m_ctrlInputsSIN;
std::vector<dynamicgraph::Signal<dynamicgraph::Vector,int>*> m_jointsCtrlModesSOUT;
DECLARE_SIGNAL_IN(base6d_encoders, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(dq, dynamicgraph::Vector); /// Joint velocities; used to compensate for BEMF effect on low level current loop
DECLARE_SIGNAL_IN(bemfFactor, dynamicgraph::Vector); /// Link betwin velocity and current; to compensate for BEMF effect on low level current loop (in A/rad.s-1)
DECLARE_SIGNAL_IN(tau, dynamicgraph::Vector); /// estimated joint torques (using dynamic robot model + F/T sensors)
DECLARE_SIGNAL_IN(tau_predicted, dynamicgraph::Vector); /// predicted joint torques (using motor model)
DECLARE_SIGNAL_IN(max_current, dynamicgraph::Vector); /// max current allowed before stopping the controller (in Ampers)
DECLARE_SIGNAL_IN(max_tau, dynamicgraph::Vector); /// max torque allowed before stopping the controller
DECLARE_SIGNAL_IN(percentageDriverDeadZoneCompensation, dynamicgraph::Vector); /// percentatge in [0;1] of the motor driver dead zone that we should compensate 0 is none, 1 is all of it
DECLARE_SIGNAL_IN(signWindowsFilterSize, dynamicgraph::Vector); /// windows size to detect changing of control sign (to then apply motor driver dead zone compensation) 0 is no filter. 1,2,3...
DECLARE_SIGNAL_IN(emergencyStop, bool ); /// emergency stop input. If true, control is set to zero forever
DECLARE_SIGNAL_OUT(pwmDes, ml::Vector);
DECLARE_SIGNAL_OUT(pwmDesSafe, ml::Vector); /// same as pwmDes when everything is fine, 0 otherwise //TODO change since pwmDes is now the desired current and pwmDesSafe is the DAC
DECLARE_SIGNAL_OUT(signOfControlFiltered, ml::Vector); /// sign of control filtered (indicating dead zone compensation applyed)
DECLARE_SIGNAL_OUT(signOfControl, ml::Vector); /// sign of control without filtered (indicating what would be the dead zone compensation applyed if no filtering on sign)
DECLARE_SIGNAL_OUT(pwmDes, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(pwmDesSafe, dynamicgraph::Vector); /// same as pwmDes when everything is fine, 0 otherwise //TODO change since pwmDes is now the desired current and pwmDesSafe is the DAC
DECLARE_SIGNAL_OUT(signOfControlFiltered, dynamicgraph::Vector); /// sign of control filtered (indicating dead zone compensation applyed)
DECLARE_SIGNAL_OUT(signOfControl, dynamicgraph::Vector); /// sign of control without filtered (indicating what would be the dead zone compensation applyed if no filtering on sign)
......
......@@ -159,49 +159,49 @@ namespace dynamicgraph {
DYNAMIC_GRAPH_ENTITY_DECL();
public: /* --- SIGNALS --- */
DECLARE_SIGNAL_IN(base6d_encoders, ml::Vector);
DECLARE_SIGNAL_IN(accelerometer, ml::Vector);
DECLARE_SIGNAL_IN(gyroscope, ml::Vector);
DECLARE_SIGNAL_IN(ftSensLeftFoot, ml::Vector);
DECLARE_SIGNAL_IN(ftSensRightFoot, ml::Vector);
DECLARE_SIGNAL_IN(ftSensLeftHand, ml::Vector);
DECLARE_SIGNAL_IN(ftSensRightHand, ml::Vector);
DECLARE_SIGNAL_IN(ddqRef, ml::Vector);
DECLARE_SIGNAL_IN(dqRef, ml::Vector);
DECLARE_SIGNAL_IN(currentMeasure, ml::Vector);
DECLARE_SIGNAL_IN(saturationCurrent,ml::Vector);
DECLARE_SIGNAL_IN(wCurrentTrust, ml::Vector);
DECLARE_SIGNAL_IN(motorParameterKt_p, ml::Vector);
DECLARE_SIGNAL_IN(motorParameterKt_n, ml::Vector);
DECLARE_SIGNAL_IN(motorParameterKf_p, ml::Vector);
DECLARE_SIGNAL_IN(motorParameterKf_n, ml::Vector);
DECLARE_SIGNAL_IN(motorParameterKv_p, ml::Vector);
DECLARE_SIGNAL_IN(motorParameterKv_n, ml::Vector);
DECLARE_SIGNAL_IN(motorParameterKa_p, ml::Vector);
DECLARE_SIGNAL_IN(motorParameterKa_n, ml::Vector);
DECLARE_SIGNAL_IN(tauDes, ml::Vector); // desired joint torques
DECLARE_SIGNAL_IN(base6d_encoders, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(accelerometer, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(gyroscope, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(ftSensLeftFoot, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(ftSensRightFoot, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(ftSensLeftHand, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(ftSensRightHand, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(ddqRef, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(dqRef, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(currentMeasure, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(saturationCurrent,dynamicgraph::Vector);
DECLARE_SIGNAL_IN(wCurrentTrust, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(motorParameterKt_p, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(motorParameterKt_n, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(motorParameterKf_p, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(motorParameterKf_n, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(motorParameterKv_p, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(motorParameterKv_n, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(motorParameterKa_p, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(motorParameterKa_n, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(tauDes, dynamicgraph::Vector); // desired joint torques
DECLARE_SIGNAL_OUT(ftSensRightFootPrediction, ml::Vector); /// debug signal
DECLARE_SIGNAL_OUT(jointsPositions, ml::Vector);
DECLARE_SIGNAL_OUT(jointsVelocities, ml::Vector);
DECLARE_SIGNAL_OUT(jointsAccelerations, ml::Vector);
DECLARE_SIGNAL_OUT(torsoAcceleration, ml::Vector); // 6d
DECLARE_SIGNAL_OUT(torsoAngularVelocity, ml::Vector); // 3d
DECLARE_SIGNAL_OUT(baseAcceleration, ml::Vector); // 6d
DECLARE_SIGNAL_OUT(baseAngularVelocity, ml::Vector); // 3d
DECLARE_SIGNAL_OUT(contactWrenchLeftFoot, ml::Vector);
DECLARE_SIGNAL_OUT(contactWrenchRightFoot, ml::Vector);
DECLARE_SIGNAL_OUT(contactWrenchLeftSole, ml::Vector);
DECLARE_SIGNAL_OUT(contactWrenchRightSole, ml::Vector);
DECLARE_SIGNAL_OUT(contactWrenchLeftHand, ml::Vector);
DECLARE_SIGNAL_OUT(contactWrenchRightHand, ml::Vector);
DECLARE_SIGNAL_OUT(contactWrenchBody, ml::Vector);
DECLARE_SIGNAL_OUT(currentFiltered, ml::Vector);
DECLARE_SIGNAL_OUT(jointsTorques, ml::Vector);
DECLARE_SIGNAL_OUT(jointsTorquesFromMotorModel, ml::Vector);
DECLARE_SIGNAL_OUT(jointsTorquesFromInertiaModel, ml::Vector);
DECLARE_SIGNAL_OUT(dynamicsError, ml::Vector); // error between desired torques and estimated (n+6)
DECLARE_SIGNAL_OUT(ftSensRightFootPrediction, dynamicgraph::Vector); /// debug signal
DECLARE_SIGNAL_OUT(jointsPositions, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(jointsVelocities, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(jointsAccelerations, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(torsoAcceleration, dynamicgraph::Vector); // 6d
DECLARE_SIGNAL_OUT(torsoAngularVelocity, dynamicgraph::Vector); // 3d
DECLARE_SIGNAL_OUT(baseAcceleration, dynamicgraph::Vector); // 6d
DECLARE_SIGNAL_OUT(baseAngularVelocity, dynamicgraph::Vector); // 3d
DECLARE_SIGNAL_OUT(contactWrenchLeftFoot, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(contactWrenchRightFoot, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(contactWrenchLeftSole, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(contactWrenchRightSole, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(contactWrenchLeftHand, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(contactWrenchRightHand, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(contactWrenchBody, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(currentFiltered, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(jointsTorques, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(jointsTorquesFromMotorModel, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(jointsTorquesFromInertiaModel, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(dynamicsError, dynamicgraph::Vector); // error between desired torques and estimated (n+6)
/// The following inner signals are used because this entity has some output signals
/// whose related quantities are computed at the same time by the same algorithm
......@@ -212,13 +212,13 @@ namespace dynamicgraph {
/// Inner signals are not exposed, so that nobody can access them.
/// This signal contains the joints' torques and all the 5 contact wrenches
DECLARE_SIGNAL_INNER(torques_wrenches, ml::Vector);
DECLARE_SIGNAL_INNER(torques_wrenches, dynamicgraph::Vector);
/// This signal contains the joints' torques estimated from motor model and current measurment
DECLARE_SIGNAL_INNER(torquesFromMotorModel, ml::Vector);
DECLARE_SIGNAL_INNER(torquesFromMotorModel, dynamicgraph::Vector);
/// This signal contains the estimated joints positions, velocities and accelerations.
DECLARE_SIGNAL_INNER(q_dq_ddq, ml::Vector);
DECLARE_SIGNAL_INNER(q_dq_ddq, dynamicgraph::Vector);
/// This signal contains the estimated base angular velocity and lin/ang accelerations.
DECLARE_SIGNAL_INNER(w_dv_torso, ml::Vector);
DECLARE_SIGNAL_INNER(w_dv_torso, dynamicgraph::Vector);
protected:
......@@ -294,7 +294,7 @@ namespace dynamicgraph {
Eigen::VectorXd m_ftSensRightHand_offset;
Eigen::VectorXd m_ftSensLeftFoot_offset;
Eigen::VectorXd m_ftSensRightFoot_offset;
maal::boost::Vector m_FTsensorOffsets;
Eigen::VectorXd m_FTsensorOffsets;
// *************************** Metapod ******************************
typedef metapod::hrp2_14<double> Hrp2_14;
......@@ -334,8 +334,8 @@ namespace dynamicgraph {
Hrp2_14::confVector m_q, m_dq, m_ddq;
Hrp2_14::confVector m_torques;
boost::circular_buffer<ml::Vector> m_tauDesBuffer;
boost::circular_buffer<ml::Vector> m_tauBuffer;
boost::circular_buffer<dynamicgraph::Vector> m_tauDesBuffer;
boost::circular_buffer<dynamicgraph::Vector> m_tauBuffer;
double m_delayTauDes;
public:
......@@ -365,7 +365,7 @@ namespace dynamicgraph {
const double& delayGyro,const double& delayCurrent,
const bool &computeForceSensorsOffsets);
void setFTsensorOffsets(const ml::Vector& offsets);
void setFTsensorOffsets(const dynamicgraph::Vector& offsets);
void recomputeFTsensorOffsets();
protected:
......
......@@ -84,12 +84,15 @@ namespace dynamicgraph {
void resetIntegral();
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(base6d_encoders, ml::Vector);
DECLARE_SIGNAL_IN(joint_velocities, ml::Vector);
DECLARE_SIGNAL_INNER(kinematics_computations, ml::Vector);
DECLARE_SIGNAL_OUT(freeflyer_aa, ml::Vector); /// freeflyer position with angle axis format
DECLARE_SIGNAL_OUT(base6dFromFoot_encoders, ml::Vector); /// base6d_encoders with base6d in RPY
DECLARE_SIGNAL_OUT(v, ml::Vector); /// n+6 robot velocities
DECLARE_SIGNAL_IN(base6d_encoders, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(joint_velocities, dynamicgraph::Vector);
DECLARE_SIGNAL_INNER(kinematics_computations, dynamicgraph::Vector);
/// freeflyer position with angle axis format
DECLARE_SIGNAL_OUT(freeflyer_aa, dynamicgraph::Vector);
/// base6d_encoders with base6d in RPY
DECLARE_SIGNAL_OUT(base6dFromFoot_encoders, dynamicgraph::Vector);
/// n+6 robot velocities
DECLARE_SIGNAL_OUT(v, dynamicgraph::Vector);
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
......
......@@ -23,7 +23,7 @@
#include <dynamic-graph/linear-algebra.h>
#include <sot/core/device.hh>
#include <sot/core/abstract-sot-external-interface.hh>
#include <sot/core/matrix-rotation.hh>
//#include <sot/core/matrix-geometry.hh>
#include <sot/torque_control/signal-helper.hh>
#include <sot/torque_control/hrp2-common.hh>
......@@ -43,7 +43,7 @@
#include <Eigen/Cholesky>
namespace dgsot=dynamicgraph::sot;
namespace dynamicgraphsot=dynamicgraph::sot;
namespace dynamicgraph
{
......@@ -76,7 +76,7 @@ namespace torque_control
* and move it in the specific subclasses.
*/
class HRP2DevicePosCtrl: public
dgsot::Device
dynamicgraphsot::Device
{
public:
......@@ -106,38 +106,38 @@ protected:
double timestep_;
/// proportional gains
dynamicgraph::SignalPtr <ml::Vector, int> kpSIN_;
dynamicgraph::SignalPtr <dynamicgraph::Vector, int> kpSIN_;
/// derivative gains
dynamicgraph::SignalPtr <ml::Vector, int> kdSIN_;
dynamicgraph::SignalPtr <dynamicgraph::Vector, int> kdSIN_;
/// input force sensor values
dynamicgraph::SignalPtr <ml::Vector, int>* forcesSIN_[4];
dynamicgraph::SignalPtr <dynamicgraph::Vector, int>* forcesSIN_[4];
/// Accelerations measured by accelerometers
dynamicgraph::Signal <ml::Vector, int> accelerometerSOUT_;
dynamicgraph::Signal <dynamicgraph::Vector, int> accelerometerSOUT_;
/// Rotation velocity measured by gyrometers
dynamicgraph::Signal <ml::Vector, int> gyrometerSOUT_;
dynamicgraph::Signal <dynamicgraph::Vector, int> gyrometerSOUT_;
/// base 6d pose + joints' angles measured by encoders
dynamicgraph::Signal <ml::Vector, int> robotStateSOUT_;
dynamicgraph::Signal <dynamicgraph::Vector, int> robotStateSOUT_;
/// joints' velocities computed by the integrator
dynamicgraph::Signal <ml::Vector, int> jointsVelocitiesSOUT_;
dynamicgraph::Signal <dynamicgraph::Vector, int> jointsVelocitiesSOUT_;
/// joints' accelerations computed by the integrator
dynamicgraph::Signal <ml::Vector, int> jointsAccelerationsSOUT_;
dynamicgraph::Signal <dynamicgraph::Vector, int> jointsAccelerationsSOUT_;
/// motor currents
dynamicgraph::Signal <ml::Vector, int> currentSOUT_;
dynamicgraph::Signal <dynamicgraph::Vector, int> currentSOUT_;
/// proportional and derivative position-control gains
dynamicgraph::Signal <ml::Vector, int> p_gainsSOUT_;
dynamicgraph::Signal <ml::Vector, int> d_gainsSOUT_;
dynamicgraph::Signal <dynamicgraph::Vector, int> p_gainsSOUT_;
dynamicgraph::Signal <dynamicgraph::Vector, int> d_gainsSOUT_;
/// Intermediate variables to avoid allocation during control
ml::Vector accelerometer_;
ml::Vector gyrometer_;
dynamicgraph::Vector accelerometer_;
dynamicgraph::Vector gyrometer_;
ml::Vector base6d_encoders_; /// base 6d pose + joints' angles
ml::Vector jointsVelocities_; /// joints' velocities
ml::Vector jointsAccelerations_; /// joints' accelerations
dynamicgraph::Vector base6d_encoders_; /// base 6d pose + joints' angles
dynamicgraph::Vector jointsVelocities_; /// joints' velocities
dynamicgraph::Vector jointsAccelerations_; /// joints' accelerations
ml::Vector wrenches_[4];
ml::Vector temp6_;
dynamicgraph::Vector wrenches_[4];
dynamicgraph::Vector temp6_;
/// robot geometric/inertial data
typedef metapod::hrp2_14<double> Hrp2_14;
......
......@@ -79,73 +79,73 @@ namespace dynamicgraph {
void init(const double& dt, const std::string& urdfFile);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(com_ref_pos, ml::Vector);
DECLARE_SIGNAL_IN(com_ref_vel, ml::Vector);
DECLARE_SIGNAL_IN(com_ref_acc, ml::Vector);
DECLARE_SIGNAL_IN(posture_ref_pos, ml::Vector);
DECLARE_SIGNAL_IN(posture_ref_vel, ml::Vector);
DECLARE_SIGNAL_IN(posture_ref_acc, ml::Vector);
DECLARE_SIGNAL_IN(base_orientation_ref_pos, ml::Vector);
DECLARE_SIGNAL_IN(base_orientation_ref_vel, ml::Vector);
DECLARE_SIGNAL_IN(base_orientation_ref_acc, ml::Vector);
DECLARE_SIGNAL_IN(kp_base_orientation, ml::Vector);
DECLARE_SIGNAL_IN(kd_base_orientation, ml::Vector);
DECLARE_SIGNAL_IN(kp_constraints, ml::Vector);
DECLARE_SIGNAL_IN(kd_constraints, ml::Vector);
DECLARE_SIGNAL_IN(kp_com, ml::Vector);
DECLARE_SIGNAL_IN(kd_com, ml::Vector);
DECLARE_SIGNAL_IN(kp_posture, ml::Vector);
DECLARE_SIGNAL_IN(kd_posture, ml::Vector);
DECLARE_SIGNAL_IN(kp_pos, ml::Vector);
DECLARE_SIGNAL_IN(kd_pos, ml::Vector);
DECLARE_SIGNAL_IN(com_ref_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(com_ref_vel, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(com_ref_acc, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(posture_ref_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(posture_ref_vel, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(posture_ref_acc, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(base_orientation_ref_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(base_orientation_ref_vel, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(base_orientation_ref_acc, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kp_base_orientation, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kd_base_orientation, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kp_constraints, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kd_constraints, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kp_com, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kd_com, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kp_posture, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kd_posture, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kp_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kd_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(w_com, double);
DECLARE_SIGNAL_IN(w_posture, double);
DECLARE_SIGNAL_IN(w_base_orientation, double);
DECLARE_SIGNAL_IN(w_torques, double);
DECLARE_SIGNAL_IN(w_forces, double);
DECLARE_SIGNAL_IN(weight_contact_forces, ml::Vector);
DECLARE_SIGNAL_IN(weight_contact_forces, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(mu, double);
DECLARE_SIGNAL_IN(contact_points, ml::Matrix);
DECLARE_SIGNAL_IN(contact_normal, ml::Vector);
DECLARE_SIGNAL_IN(contact_points, dynamicgraph::Matrix);
DECLARE_SIGNAL_IN(contact_normal, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(f_min, double);
DECLARE_SIGNAL_IN(rotor_inertias, ml::Vector);
DECLARE_SIGNAL_IN(gear_ratios, ml::Vector);
DECLARE_SIGNAL_IN(tau_max, ml::Vector);
DECLARE_SIGNAL_IN(q_min, ml::Vector);
DECLARE_SIGNAL_IN(q_max, ml::Vector);
DECLARE_SIGNAL_IN(dq_max, ml::Vector);
DECLARE_SIGNAL_IN(ddq_max, ml::Vector);
DECLARE_SIGNAL_IN(dt_joint_pos_limits, double );
DECLARE_SIGNAL_IN(tau_estimated, ml::Vector);
DECLARE_SIGNAL_IN(q, ml::Vector);
DECLARE_SIGNAL_IN(v, ml::Vector);
DECLARE_SIGNAL_IN(wrench_base, ml::Vector);
DECLARE_SIGNAL_IN(wrench_left_foot, ml::Vector);
DECLARE_SIGNAL_IN(wrench_right_foot, ml::Vector);
DECLARE_SIGNAL_IN(active_joints, ml::Vector); /// mask with 1 for controlled joints, 0 otherwise
DECLARE_SIGNAL_IN(rotor_inertias, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(gear_ratios, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(tau_max, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(q_min, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(q_max, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(dq_max, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(ddq_max, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(dt_joint_pos_limits, double);
DECLARE_SIGNAL_IN(tau_estimated, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(q, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(v, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(wrench_base, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(wrench_left_foot, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(wrench_right_foot, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(active_joints, dynamicgraph::Vector); /// mask with 1 for controlled joints, 0 otherwise
DECLARE_SIGNAL_OUT(tau_des, ml::Vector);
DECLARE_SIGNAL_OUT(dv_des, ml::Vector);
DECLARE_SIGNAL_OUT(f_des_right_foot, ml::Vector);
DECLARE_SIGNAL_OUT(f_des_left_foot, ml::Vector);
DECLARE_SIGNAL_OUT(zmp_des_right_foot, ml::Vector);
DECLARE_SIGNAL_OUT(zmp_des_left_foot, ml::Vector);
DECLARE_SIGNAL_OUT(zmp_des_right_foot_local, ml::Vector);
DECLARE_SIGNAL_OUT(zmp_des_left_foot_local, ml::Vector);
DECLARE_SIGNAL_OUT(zmp_des, ml::Vector);
DECLARE_SIGNAL_OUT(com, ml::Vector);
DECLARE_SIGNAL_OUT(base_orientation, ml::Vector);
DECLARE_SIGNAL_OUT(right_foot_pos, ml::Vector);
DECLARE_SIGNAL_OUT(left_foot_pos, ml::Vector);
DECLARE_SIGNAL_OUT(tau_des, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(dv_des, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(f_des_right_foot, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(f_des_left_foot, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(zmp_des_right_foot, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(zmp_des_left_foot, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(zmp_des_right_foot_local, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(zmp_des_left_foot_local, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(zmp_des, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(com, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(base_orientation, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(right_foot_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(left_foot_pos, dynamicgraph::Vector);
/// This signal copies active_joints only if it changes from a all false or to an all false value
DECLARE_SIGNAL_INNER(active_joints_checked, ml::Vector);
DECLARE_SIGNAL_INNER(active_joints_checked, dynamicgraph::Vector);
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
......
......@@ -78,36 +78,36 @@ namespace dynamicgraph {
void resetForceIntegral();
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(base6d_encoders, ml::Vector);
DECLARE_SIGNAL_IN(jointsVelocities, ml::Vector);
DECLARE_SIGNAL_IN(baseAngularVelocity, ml::Vector);
DECLARE_SIGNAL_IN(baseAcceleration, ml::Vector);
DECLARE_SIGNAL_IN(qRef, ml::Vector);
DECLARE_SIGNAL_IN(dqRef, ml::Vector);
DECLARE_SIGNAL_IN(ddqRef, ml::Vector);
DECLARE_SIGNAL_IN(Kp, ml::Vector); /// joint proportional gains
DECLARE_SIGNAL_IN(Kd, ml::Vector); /// joint derivative gains
DECLARE_SIGNAL_IN(Kf, ml::Vector); /// force proportional gains
DECLARE_SIGNAL_IN(Ki, ml::Vector); /// force integral gains
DECLARE_SIGNAL_IN(fRightFootRef, ml::Vector); /// right sole 6d reference force
DECLARE_SIGNAL_IN(fLeftFootRef, ml::Vector); /// left sole 6d reference force
DECLARE_SIGNAL_IN(fRightHandRef, ml::Vector); /// right gripper 6d reference force
DECLARE_SIGNAL_IN(fLeftHandRef, ml::Vector); /// left gripper 6d reference force
DECLARE_SIGNAL_IN(fRightFoot, ml::Vector); /// right sole 6d estimated force
DECLARE_SIGNAL_IN(fLeftFoot, ml::Vector); /// left sole 6d estimated force
DECLARE_SIGNAL_IN(fRightHand, ml::Vector); /// right gripper 6d estimated force
DECLARE_SIGNAL_IN(fLeftHand, ml::Vector); /// left gripper 6d estimated force
DECLARE_SIGNAL_IN(controlledJoints, ml::Vector); /// mask with 1 for controlled joints, 0 otherwise
DECLARE_SIGNAL_IN(dynamicsError, ml::Vector); /// estimated error of the robot dynamic model (n+6)
DECLARE_SIGNAL_IN(dynamicsErrorGain,ml::Vector); /// gain multiplying the dynamics error (n+6)
DECLARE_SIGNAL_OUT(tauDes, ml::Vector); /// M*ddqRef + h - J^T*(fRef+Kf*e_f) + Kp*e_q + Kd*de_q
DECLARE_SIGNAL_IN(base6d_encoders, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(jointsVelocities, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(baseAngularVelocity, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(baseAcceleration, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(qRef, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(dqRef, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(ddqRef, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(Kp, dynamicgraph::Vector); /// joint proportional gains
DECLARE_SIGNAL_IN(Kd, dynamicgraph::Vector); /// joint derivative gains
DECLARE_SIGNAL_IN(Kf, dynamicgraph::Vector); /// force proportional gains
DECLARE_SIGNAL_IN(Ki, dynamicgraph::Vector); /// force integral gains
DECLARE_SIGNAL_IN(fRightFootRef, dynamicgraph::Vector); /// right sole 6d reference force
DECLARE_SIGNAL_IN(fLeftFootRef, dynamicgraph::Vector); /// left sole 6d reference force
DECLARE_SIGNAL_IN(fRightHandRef, dynamicgraph::Vector); /// right gripper 6d reference force
DECLARE_SIGNAL_IN(fLeftHandRef, dynamicgraph::Vector); /// left gripper 6d reference force
DECLARE_SIGNAL_IN(fRightFoot, dynamicgraph::Vector); /// right sole 6d estimated force
DECLARE_SIGNAL_IN(fLeftFoot, dynamicgraph::Vector); /// left sole 6d estimated force
DECLARE_SIGNAL_IN(fRightHand, dynamicgraph::Vector); /// right gripper 6d estimated force
DECLARE_SIGNAL_IN(fLeftHand, dynamicgraph::Vector); /// left gripper 6d estimated force
DECLARE_SIGNAL_IN(controlledJoints, dynamicgraph::Vector); /// mask with 1 for controlled joints, 0 otherwise
DECLARE_SIGNAL_IN(dynamicsError, dynamicgraph::Vector); /// estimated error of the robot dynamic model (n+6)
DECLARE_SIGNAL_IN(dynamicsErrorGain,dynamicgraph::Vector); /// gain multiplying the dynamics error (n+6)
DECLARE_SIGNAL_OUT(tauDes, dynamicgraph::Vector); /// M*ddqRef + h - J^T*(fRef+Kf*e_f) + Kp*e_q + Kd*de_q
// DEBUG SIGNALS
DECLARE_SIGNAL_OUT(tauFF, ml::Vector); /// M*ddqRef + h - J^T*fRef
DECLARE_SIGNAL_OUT(tauFB, ml::Vector); /// Kp*(qRef-q) + Kd*(dqRef-dq) - J^T*Kf*e_f
DECLARE_SIGNAL_OUT(tauFB2, ml::Vector); /// same thing but computed differently (just for debug)
DECLARE_SIGNAL_OUT(ddqDes, ml::Vector); /// ddqRef + Kp*(qRef-q) + Kd*(dqRef-dq)
DECLARE_SIGNAL_OUT(qError, ml::Vector); /// qRef-q
DECLARE_SIGNAL_OUT(tauFF, dynamicgraph::Vector); /// M*ddqRef + h - J^T*fRef
DECLARE_SIGNAL_OUT(tauFB, dynamicgraph::Vector); /// Kp*(qRef-q) + Kd*(dqRef-dq) - J^T*Kf*e_f
DECLARE_SIGNAL_OUT(tauFB2, dynamicgraph::Vector); /// same thing but computed differently (just for debug)
DECLARE_SIGNAL_OUT(ddqDes, dynamicgraph::Vector); /// ddqRef + Kp*(qRef-q) + Kd*(dqRef-dq)
DECLARE_SIGNAL_OUT(qError, dynamicgraph::Vector); /// qRef-q