sot-torque-control  1.6.5
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
dynamicgraph::sot::torque_control Namespace Reference

Classes

class  AbstractTrajectoryGenerator
 
class  AdmittanceController
 
class  BaseEstimator
 
class  ConstantAccelerationTrajectoryGenerator
 
class  ControlManager
 
class  CtrlMode
 
class  CurrentController
 
class  DdpActuatorSolver
 
class  DdpPyreneActuatorSolver
 
class  DeviceTorqueCtrl
 
class  FreeFlyerLocator
 
class  ImuOffsetCompensation
 
class  InverseDynamicsBalanceController
 
class  JointTorqueController
 
class  JointTrajectoryGenerator
 
class  LinearChirpTrajectoryGenerator
 
class  MinimumJerkTrajectoryGenerator
 
class  MotorModel
 
class  NoTrajectoryGenerator
 
class  NumericalDifference
 
class  PositionController
 
class  SE3TrajectoryGenerator
 
class  SimpleInverseDyn
 
class  SinusoidTrajectoryGenerator
 
class  TextFileTrajectoryGenerator
 
class  TorqueOffsetEstimator
 
class  TracePlayer
 Entity to play data saved using a Tracer. More...
 
class  TriangleTrajectoryGenerator
 

Typedefs

typedef int dummy
 
typedef AdmittanceController EntityClassName
 
typedef SolverHQuadProgRT< 48, 30, 17 > SolverHQuadProgRT48x30x17
 
typedef SolverHQuadProgRT< 60, 36, 34 > SolverHQuadProgRT60x36x34
 
typedef Eigen::Matrix< double, 2, 1 > Vector2
 
typedef Eigen::Matrix< double, 3, 1 > Vector3
 
typedef Eigen::Matrix< double, 6, 1 > Vector6
 
typedef Eigen::Matrix< double, Eigen::Dynamic, 1 > VectorN
 
typedef Eigen::Matrix< double, Eigen::Dynamic, 1 > VectorN6
 

Enumerations

enum  ControlOutput { CONTROL_OUTPUT_VELOCITY = 0, CONTROL_OUTPUT_TORQUE = 1, CONTROL_OUTPUT_SIZE = 2 }
 

Functions

 DEFINE_SIGNAL_INNER_FUNCTION (active_joints_checked, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_INNER_FUNCTION (collectSensorData, dummy)
 
 DEFINE_SIGNAL_INNER_FUNCTION (kinematics_computations, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_INNER_FUNCTION (x_dx_ddx, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (a_ac, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (accelerometer_out, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (base6dFromFoot_encoders, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (base_orientation, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (com, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (com_acc, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (com_acc_des, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (com_vel, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (ddq, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (ddx, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (dead_zone_compensation, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (dq, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (dqDes, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (dv_des, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (dx, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (f_des_left_foot, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (f_des_right_foot, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (fLeftFoot, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (fLeftHand, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (freeflyer_aa, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (fRightFoot, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (fRightHand, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (gyrometer_out, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (i_errors, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (i_errors_ll_wo_bemf, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (i_low_level, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (i_real, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (i_sensor_offsets_real_out, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (jointTorquesEstimated, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (left_foot_acc, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (left_foot_acc_des, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (left_foot_pos, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (left_foot_vel, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (left_hand_acc, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (left_hand_pos, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (left_hand_vel, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (lf_xyzquat, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (M, dynamicgraph::Matrix)
 
 DEFINE_SIGNAL_OUT_FUNCTION (pwmDes, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (q, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (q_des, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (q_imu, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (q_lf, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (q_rf, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (qError, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (rf_xyzquat, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (right_foot_acc, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (right_foot_acc_des, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (right_foot_pos, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (right_foot_vel, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (right_hand_acc, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (right_hand_pos, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (right_hand_vel, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (smoothSignDq, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (tau, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (tau_des, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (torque_error_integral, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (trigger, int)
 
 DEFINE_SIGNAL_OUT_FUNCTION (u, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (u_safe, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (v, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (v_ac, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (v_des, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (v_flex, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (v_gyr, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (v_imu, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (v_kin, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (vDesLeftFoot, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (vDesRightFoot, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (w_lf, double)
 
 DEFINE_SIGNAL_OUT_FUNCTION (w_lf_filtered, double)
 
 DEFINE_SIGNAL_OUT_FUNCTION (w_rf, double)
 
 DEFINE_SIGNAL_OUT_FUNCTION (w_rf_filtered, double)
 
 DEFINE_SIGNAL_OUT_FUNCTION (x, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (x_filtered, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (zmp, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (zmp_des, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (zmp_des_left_foot, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (zmp_des_left_foot_local, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (zmp_des_right_foot, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (zmp_des_right_foot_local, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (zmp_left_foot, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (zmp_ref, dynamicgraph::Vector)
 
 DEFINE_SIGNAL_OUT_FUNCTION (zmp_right_foot, dynamicgraph::Vector)
 
 DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN (AdmittanceController, "AdmittanceController")
 
 DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN (BaseEstimator, "BaseEstimator")
 
 DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN (ControlManager, "ControlManager")
 
 DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN (CurrentController, "CurrentController")
 
 DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN (DdpActuatorSolver, "DdpActuatorSolver")
 
 DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN (DdpPyreneActuatorSolver, "DdpPyreneActuatorSolver")
 
 DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN (FreeFlyerLocator, "FreeFlyerLocator")
 
 DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN (ImuOffsetCompensation, "ImuOffsetCompensation")
 
 DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN (InverseDynamicsBalanceController, "InverseDynamicsBalanceController")
 
 DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN (JointTorqueController, "JointTorqueController")
 
 DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN (JointTrajectoryGenerator, "JointTrajectoryGenerator")
 
 DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN (NumericalDifference, "NumericalDifference")
 
 DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN (PositionController, "PositionController")
 
 DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN (SE3TrajectoryGenerator, "SE3TrajectoryGenerator")
 
 DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN (SimpleInverseDyn, "SimpleInverseDyn")
 
 DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN (TorqueOffsetEstimator, "TorqueOffsetEstimator")
 
 DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN (TracePlayer, "TracePlayer")
 
void matrixToRpy (const Eigen::Matrix3d &M, Eigen::Vector3d &rpy)
 
std::ostream & operator<< (std::ostream &os, const CtrlMode &s)
 
void pointRotationByQuaternion (const Eigen::Vector3d &point, const Eigen::Vector4d &quat, Eigen::Vector3d &rotatedPoint)
 
void quanternionMult (const Eigen::Vector4d &q1, const Eigen::Vector4d &q2, Eigen::Vector4d &q12)
 
Eigen::MatrixXd readMatrixFromFile (const char *filename)
 
void rpyToMatrix (const Eigen::Vector3d &rpy, Eigen::Matrix3d &R)
 
void rpyToMatrix (double r, double p, double y, Eigen::Matrix3d &R)
 
void se3Interp (const pinocchio::SE3 &s1, const pinocchio::SE3 &s2, const double alpha, pinocchio::SE3 &s12)
 
VectorXd svdSolveWithDamping (const JacobiSVD< MatrixXd > &A, const VectorXd &b, double damping)
 

Variables

const std::string ControlOutput_s [] = {"velocity", "torque"}
 

Typedef Documentation

◆ dummy

typedef int dummy

Definition at line 32 of file torque-offset-estimator.cpp.

◆ EntityClassName

Define EntityClassName here rather than in the header file so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.

Definition at line 51 of file admittance-controller.cpp.

◆ SolverHQuadProgRT48x30x17

typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17

Definition at line 73 of file inverse-dynamics-balance-controller.cpp.

◆ SolverHQuadProgRT60x36x34

typedef SolverHQuadProgRT<60, 36, 34> SolverHQuadProgRT60x36x34

Definition at line 72 of file inverse-dynamics-balance-controller.cpp.

◆ Vector2

typedef Eigen::Matrix< double, 2, 1 > Vector2

Definition at line 133 of file inverse-dynamics-balance-controller.cpp.

◆ Vector3

typedef Eigen::Matrix<double, 3, 1> Vector3

Definition at line 53 of file admittance-controller.cpp.

◆ Vector6

typedef dynamicgraph::sot::Vector6d Vector6

Definition at line 54 of file admittance-controller.cpp.

◆ VectorN

typedef Eigen::Matrix< double, Eigen::Dynamic, 1 > VectorN

Definition at line 134 of file inverse-dynamics-balance-controller.cpp.

◆ VectorN6

typedef Eigen::Matrix< double, Eigen::Dynamic, 1 > VectorN6

Definition at line 135 of file inverse-dynamics-balance-controller.cpp.

Enumeration Type Documentation

◆ ControlOutput

Enumerator
CONTROL_OUTPUT_VELOCITY 
CONTROL_OUTPUT_TORQUE 
CONTROL_OUTPUT_SIZE 

Definition at line 66 of file simple-inverse-dyn.hh.

Function Documentation

◆ DEFINE_SIGNAL_INNER_FUNCTION() [1/4]

DEFINE_SIGNAL_INNER_FUNCTION ( active_joints_checked  ,
dynamicgraph::Vector   
)

Copy active_joints only if a valid transition occurs. (From all OFF) or (To all OFF)

Definition at line 629 of file inverse-dynamics-balance-controller.cpp.

◆ DEFINE_SIGNAL_INNER_FUNCTION() [2/4]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_INNER_FUNCTION ( collectSensorData  ,
dummy   
)

Definition at line 149 of file torque-offset-estimator.cpp.

◆ DEFINE_SIGNAL_INNER_FUNCTION() [3/4]

DEFINE_SIGNAL_INNER_FUNCTION ( kinematics_computations  ,
dynamicgraph::Vector   
)

Definition at line 587 of file base-estimator.cpp.

◆ DEFINE_SIGNAL_INNER_FUNCTION() [4/4]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_INNER_FUNCTION ( x_dx_ddx  ,
dynamicgraph::Vector   
)

Signal Filtering and Differentiation.

Definition at line 100 of file numerical-difference.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [1/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( a_ac  ,
dynamicgraph::Vector   
)

Definition at line 1237 of file base-estimator.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [2/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( accelerometer_out  ,
dynamicgraph::Vector   
)

Definition at line 194 of file imu_offset_compensation.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [3/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( base6dFromFoot_encoders  ,
dynamicgraph::Vector   
)

Definition at line 154 of file free-flyer-locator.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [4/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( base_orientation  ,
dynamicgraph::Vector   
)

Definition at line 1278 of file inverse-dynamics-balance-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [5/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( com  ,
dynamicgraph::Vector   
)

Definition at line 1250 of file inverse-dynamics-balance-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [6/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( com_acc  ,
dynamicgraph::Vector   
)

Definition at line 1030 of file inverse-dynamics-balance-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [7/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( com_acc_des  ,
dynamicgraph::Vector   
)

Definition at line 1018 of file inverse-dynamics-balance-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [8/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( com_vel  ,
dynamicgraph::Vector   
)

Definition at line 1264 of file inverse-dynamics-balance-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [9/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( ddq  ,
dynamicgraph::Vector   
)

Definition at line 330 of file joint-trajectory-generator.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [10/84]

DEFINE_SIGNAL_OUT_FUNCTION ( ddx  ,
dynamicgraph::Vector   
)

Definition at line 145 of file numerical-difference.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [11/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( dead_zone_compensation  ,
dynamicgraph::Vector   
)

Definition at line 324 of file current-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [12/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( dq  ,
dynamicgraph::Vector   
)

Definition at line 310 of file joint-trajectory-generator.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [13/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( dqDes  ,
dynamicgraph::Vector   
)

*** Compute all Jacobians ***

Compute admittance control law

Definition at line 220 of file admittance-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [14/84]

DEFINE_SIGNAL_OUT_FUNCTION ( dv_des  ,
dynamicgraph::Vector   
)

Definition at line 974 of file inverse-dynamics-balance-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [15/84]

DEFINE_SIGNAL_OUT_FUNCTION ( dx  ,
dynamicgraph::Vector   
)

Definition at line 136 of file numerical-difference.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [16/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( f_des_left_foot  ,
dynamicgraph::Vector   
)

Definition at line 1002 of file inverse-dynamics-balance-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [17/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( f_des_right_foot  ,
dynamicgraph::Vector   
)

Definition at line 986 of file inverse-dynamics-balance-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [18/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( fLeftFoot  ,
dynamicgraph::Vector   
)

Definition at line 361 of file joint-trajectory-generator.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [19/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( fLeftHand  ,
dynamicgraph::Vector   
)

Definition at line 377 of file joint-trajectory-generator.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [20/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( freeflyer_aa  ,
dynamicgraph::Vector   
)

Definition at line 197 of file free-flyer-locator.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [21/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( fRightFoot  ,
dynamicgraph::Vector   
)

Definition at line 351 of file joint-trajectory-generator.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [22/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( fRightHand  ,
dynamicgraph::Vector   
)

Definition at line 369 of file joint-trajectory-generator.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [23/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( gyrometer_out  ,
dynamicgraph::Vector   
)

Definition at line 209 of file imu_offset_compensation.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [24/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( i_errors  ,
dynamicgraph::Vector   
)

Definition at line 336 of file current-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [25/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( i_errors_ll_wo_bemf  ,
dynamicgraph::Vector   
)

Definition at line 348 of file current-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [26/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( i_low_level  ,
dynamicgraph::Vector   
)

Definition at line 269 of file current-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [27/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( i_real  ,
dynamicgraph::Vector   
)

Definition at line 250 of file current-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [28/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( i_sensor_offsets_real_out  ,
dynamicgraph::Vector   
)

Definition at line 286 of file current-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [29/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( jointTorquesEstimated  ,
dynamicgraph::Vector   
)

Definition at line 223 of file torque-offset-estimator.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [30/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( left_foot_acc  ,
dynamicgraph::Vector   
)

Definition at line 1406 of file inverse-dynamics-balance-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [31/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( left_foot_acc_des  ,
dynamicgraph::Vector   
)

Definition at line 1457 of file inverse-dynamics-balance-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [32/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( left_foot_pos  ,
dynamicgraph::Vector   
)

Definition at line 1292 of file inverse-dynamics-balance-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [33/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( left_foot_vel  ,
dynamicgraph::Vector   
)

Definition at line 1352 of file inverse-dynamics-balance-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [34/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( left_hand_acc  ,
dynamicgraph::Vector   
)

Definition at line 1421 of file inverse-dynamics-balance-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [35/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( left_hand_pos  ,
dynamicgraph::Vector   
)

Definition at line 1308 of file inverse-dynamics-balance-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [36/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( left_hand_vel  ,
dynamicgraph::Vector   
)

Definition at line 1366 of file inverse-dynamics-balance-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [37/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( lf_xyzquat  ,
dynamicgraph::Vector   
)

Definition at line 825 of file base-estimator.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [38/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( ,
dynamicgraph::Matrix   
)

Definition at line 962 of file inverse-dynamics-balance-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [39/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( pwmDes  ,
dynamicgraph::Vector   
)

Definition at line 119 of file position-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [40/84]

DEFINE_SIGNAL_OUT_FUNCTION ( ,
dynamicgraph::Vector   
)

apply feedback correction

convert from xyzquat to se3

find translation to apply to both feet to minimise distances to reference positions

two vectors define by left to right feet translation

angle betwin this two vectors projected in horizontal plane is the yaw drift

apply correction to cancel this drift

Definition at line 621 of file base-estimator.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [41/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( q_des  ,
dynamicgraph::Vector   
)

Definition at line 589 of file simple-inverse-dyn.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [42/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( q_imu  ,
dynamicgraph::Vector   
)

Definition at line 883 of file base-estimator.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [43/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( q_lf  ,
dynamicgraph::Vector   
)

Definition at line 849 of file base-estimator.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [44/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( q_rf  ,
dynamicgraph::Vector   
)

Definition at line 866 of file base-estimator.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [45/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( qError  ,
dynamicgraph::Vector   
)

Definition at line 167 of file position-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [46/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( rf_xyzquat  ,
dynamicgraph::Vector   
)

Definition at line 837 of file base-estimator.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [47/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( right_foot_acc  ,
dynamicgraph::Vector   
)

Definition at line 1432 of file inverse-dynamics-balance-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [48/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( right_foot_acc_des  ,
dynamicgraph::Vector   
)

Definition at line 1471 of file inverse-dynamics-balance-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [49/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( right_foot_pos  ,
dynamicgraph::Vector   
)

Definition at line 1322 of file inverse-dynamics-balance-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [50/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( right_foot_vel  ,
dynamicgraph::Vector   
)

Definition at line 1380 of file inverse-dynamics-balance-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [51/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( right_hand_acc  ,
dynamicgraph::Vector   
)

Definition at line 1446 of file inverse-dynamics-balance-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [52/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( right_hand_pos  ,
dynamicgraph::Vector   
)

Definition at line 1338 of file inverse-dynamics-balance-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [53/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( right_hand_vel  ,
dynamicgraph::Vector   
)

Definition at line 1393 of file inverse-dynamics-balance-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [54/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( smoothSignDq  ,
dynamicgraph::Vector   
)

Definition at line 264 of file joint-torque-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [55/84]

DEFINE_SIGNAL_OUT_FUNCTION ( tau  ,
dynamicgraph::Vector   
)

-— Get the information --— Desired position

Measured position

Measured speed

Measured temperature

Measured torque

Desired torque

— Initialize solver —

— Solve the DDP —

— Get the command —

-— Get the information --— Desired position

Measured joint position

Measured joint speed

Desired torque

— Solve the DDP —

— Get the command —

-— Get the information --— Desired position

Measured joint position

Measured joint speed

Desired torque

— Solve the DDP —

— Get the command —

Definition at line 96 of file ddp-actuator-solver.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [56/84]

DEFINE_SIGNAL_OUT_FUNCTION ( tau_des  ,
dynamicgraph::Vector   
)

Definition at line 668 of file inverse-dynamics-balance-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [57/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( torque_error_integral  ,
dynamicgraph::Vector   
)

Definition at line 245 of file joint-torque-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [58/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( trigger  ,
int   
)

Definition at line 64 of file trace-player.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [59/84]

DEFINE_SIGNAL_OUT_FUNCTION ( ,
dynamicgraph::Vector   
)

Definition at line 193 of file admittance-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [60/84]

DEFINE_SIGNAL_OUT_FUNCTION ( u_safe  ,
dynamicgraph::Vector   
)

Definition at line 305 of file control-manager.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [61/84]

DEFINE_SIGNAL_OUT_FUNCTION ( ,
dynamicgraph::Vector   
)

TODO Read this transform from setable parameter /// TODO chesk the sign of the translation

  • Extract DC component by low pass filter and remove it*‍/

TODO

Definition at line 995 of file base-estimator.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [62/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( v_ac  ,
dynamicgraph::Vector   
)

Definition at line 1226 of file base-estimator.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [63/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( v_des  ,
dynamicgraph::Vector   
)

Definition at line 572 of file simple-inverse-dyn.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [64/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( v_flex  ,
dynamicgraph::Vector   
)

Definition at line 1193 of file base-estimator.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [65/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( v_gyr  ,
dynamicgraph::Vector   
)

Definition at line 1215 of file base-estimator.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [66/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( v_imu  ,
dynamicgraph::Vector   
)

Definition at line 1204 of file base-estimator.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [67/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( v_kin  ,
dynamicgraph::Vector   
)

Definition at line 1182 of file base-estimator.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [68/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( vDesLeftFoot  ,
dynamicgraph::Vector   
)

Definition at line 316 of file admittance-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [69/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( vDesRightFoot  ,
dynamicgraph::Vector   
)

Definition at line 273 of file admittance-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [70/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( w_lf  ,
double   
)

Definition at line 903 of file base-estimator.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [71/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( w_lf_filtered  ,
double   
)

Definition at line 981 of file base-estimator.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [72/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( w_rf  ,
double   
)

Definition at line 935 of file base-estimator.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [73/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( w_rf_filtered  ,
double   
)

Definition at line 967 of file base-estimator.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [74/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( ,
dynamicgraph::Vector   
)

Definition at line 162 of file se3-trajectory-generator.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [75/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( x_filtered  ,
dynamicgraph::Vector   
)

/// The following signals depend only on other inner signals, so they just need to copy the interested part of the inner signal they depend on.


///

Definition at line 127 of file numerical-difference.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [76/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( zmp  ,
dynamicgraph::Vector   
)

Definition at line 1231 of file inverse-dynamics-balance-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [77/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( zmp_des  ,
dynamicgraph::Vector   
)

Definition at line 1128 of file inverse-dynamics-balance-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [78/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( zmp_des_left_foot  ,
dynamicgraph::Vector   
)

Definition at line 1105 of file inverse-dynamics-balance-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [79/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( zmp_des_left_foot_local  ,
dynamicgraph::Vector   
)

Definition at line 1063 of file inverse-dynamics-balance-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [80/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( zmp_des_right_foot  ,
dynamicgraph::Vector   
)

Definition at line 1082 of file inverse-dynamics-balance-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [81/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( zmp_des_right_foot_local  ,
dynamicgraph::Vector   
)

Definition at line 1042 of file inverse-dynamics-balance-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [82/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( zmp_left_foot  ,
dynamicgraph::Vector   
)

Definition at line 1208 of file inverse-dynamics-balance-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [83/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( zmp_ref  ,
dynamicgraph::Vector   
)

Definition at line 1144 of file inverse-dynamics-balance-controller.cpp.

◆ DEFINE_SIGNAL_OUT_FUNCTION() [84/84]

dynamicgraph::sot::torque_control::DEFINE_SIGNAL_OUT_FUNCTION ( zmp_right_foot  ,
dynamicgraph::Vector   
)

Definition at line 1184 of file inverse-dynamics-balance-controller.cpp.

◆ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN() [1/17]

dynamicgraph::sot::torque_control::DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN ( AdmittanceController  ,
"AdmittanceController"   
)

◆ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN() [2/17]

dynamicgraph::sot::torque_control::DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN ( BaseEstimator  ,
"BaseEstimator"   
)

◆ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN() [3/17]

dynamicgraph::sot::torque_control::DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN ( ControlManager  ,
"ControlManager"   
)

◆ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN() [4/17]

dynamicgraph::sot::torque_control::DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN ( CurrentController  ,
"CurrentController"   
)

◆ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN() [5/17]

dynamicgraph::sot::torque_control::DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN ( DdpActuatorSolver  ,
"DdpActuatorSolver"   
)

◆ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN() [6/17]

dynamicgraph::sot::torque_control::DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN ( DdpPyreneActuatorSolver  ,
"DdpPyreneActuatorSolver"   
)

◆ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN() [7/17]

dynamicgraph::sot::torque_control::DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN ( FreeFlyerLocator  ,
"FreeFlyerLocator"   
)

◆ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN() [8/17]

dynamicgraph::sot::torque_control::DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN ( ImuOffsetCompensation  ,
"ImuOffsetCompensation"   
)

◆ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN() [9/17]

dynamicgraph::sot::torque_control::DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN ( InverseDynamicsBalanceController  ,
"InverseDynamicsBalanceController"   
)

◆ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN() [10/17]

dynamicgraph::sot::torque_control::DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN ( JointTorqueController  ,
"JointTorqueController"   
)

◆ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN() [11/17]

dynamicgraph::sot::torque_control::DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN ( JointTrajectoryGenerator  ,
"JointTrajectoryGenerator"   
)

◆ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN() [12/17]

dynamicgraph::sot::torque_control::DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN ( NumericalDifference  ,
"NumericalDifference"   
)

◆ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN() [13/17]

dynamicgraph::sot::torque_control::DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN ( PositionController  ,
"PositionController"   
)

◆ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN() [14/17]

dynamicgraph::sot::torque_control::DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN ( SE3TrajectoryGenerator  ,
"SE3TrajectoryGenerator"   
)

◆ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN() [15/17]

dynamicgraph::sot::torque_control::DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN ( SimpleInverseDyn  ,
"SimpleInverseDyn"   
)

◆ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN() [16/17]

dynamicgraph::sot::torque_control::DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN ( TorqueOffsetEstimator  ,
"TorqueOffsetEstimator"   
)

◆ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN() [17/17]

dynamicgraph::sot::torque_control::DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN ( TracePlayer  ,
"TracePlayer"   
)

◆ matrixToRpy()

void matrixToRpy ( const Eigen::Matrix3d &  M,
Eigen::Vector3d &  rpy 
)

Convert from Transformation Matrix to Roll, Pitch, Yaw

Definition at line 47 of file base-estimator.cpp.

◆ operator<<()

std::ostream& dynamicgraph::sot::torque_control::operator<< ( std::ostream &  os,
const CtrlMode s 
)

Definition at line 63 of file control-manager.hh.

◆ pointRotationByQuaternion()

void pointRotationByQuaternion ( const Eigen::Vector3d &  point,
const Eigen::Vector4d &  quat,
Eigen::Vector3d &  rotatedPoint 
)

Rotate a point or a vector by a quaternion stored in (w,x,y,z) format

Definition at line 67 of file base-estimator.cpp.

◆ quanternionMult()

void quanternionMult ( const Eigen::Vector4d &  q1,
const Eigen::Vector4d &  q2,
Eigen::Vector4d &  q12 
)

Multiply to quaternions stored in (w,x,y,z) format

Definition at line 59 of file base-estimator.cpp.

◆ readMatrixFromFile()

Eigen::MatrixXd dynamicgraph::sot::torque_control::readMatrixFromFile ( const char *  filename)

Definition at line 42 of file trajectory-generators.hh.

◆ rpyToMatrix() [1/2]

void rpyToMatrix ( const Eigen::Vector3d &  rpy,
Eigen::Matrix3d &  R 
)

Convert from Roll, Pitch, Yaw to transformation Matrix.

Definition at line 43 of file base-estimator.cpp.

◆ rpyToMatrix() [2/2]

void rpyToMatrix ( double  r,
double  p,
double  y,
Eigen::Matrix3d &  R 
)

Convert from Roll, Pitch, Yaw to transformation Matrix.

Definition at line 35 of file base-estimator.cpp.

◆ se3Interp()

void se3Interp ( const pinocchio::SE3 &  s1,
const pinocchio::SE3 &  s2,
const double  alpha,
pinocchio::SE3 &  s12 
)

Compute s12 as an intermediate transform between s1 and s2 SE3 transforms

Definition at line 25 of file base-estimator.cpp.

◆ svdSolveWithDamping()

VectorXd dynamicgraph::sot::torque_control::svdSolveWithDamping ( const JacobiSVD< MatrixXd > &  A,
const VectorXd &  b,
double  damping 
)

Definition at line 419 of file admittance-controller.cpp.

Variable Documentation

◆ ControlOutput_s

const std::string ControlOutput_s[] = {"velocity", "torque"}

Definition at line 72 of file simple-inverse-dyn.hh.