6 #include "pinocchio/algorithm/frames.hpp" 8 #include <dynamic-graph/factory.h> 9 #include <sot/core/debug.hh> 12 #include <sot/core/stop-watch.hh> 23 typedef dynamicgraph::sot::Vector6d
Vector6;
25 #define PROFILE_FREE_FLYER_COMPUTATION "Free-flyer position computation" 26 #define PROFILE_FREE_FLYER_VELOCITY_COMPUTATION "Free-flyer velocity computation" 28 #define INPUT_SIGNALS m_base6d_encodersSIN << m_joint_velocitiesSIN 29 #define OUTPUT_SIGNALS m_freeflyer_aaSOUT << m_base6dFromFoot_encodersSOUT << m_vSOUT 43 CONSTRUCT_SIGNAL_IN(base6d_encoders,
dynamicgraph::Vector),
44 CONSTRUCT_SIGNAL_IN(joint_velocities,
dynamicgraph::Vector),
46 CONSTRUCT_SIGNAL_OUT(freeflyer_aa,
dynamicgraph::Vector, m_base6dFromFoot_encodersSOUT),
47 CONSTRUCT_SIGNAL_OUT(base6dFromFoot_encoders,
dynamicgraph::Vector, m_kinematics_computationsSINNER),
48 CONSTRUCT_SIGNAL_OUT(v,
dynamicgraph::Vector, m_kinematics_computationsSINNER),
49 m_initSucceeded(false),
52 m_robot_util(RefVoidRobotUtil()) {
57 docCommandVoid1(
"Initialize the entity.",
"Robot reference (string)")));
62 docCommandVoid0(
"Display the robot util data set linked with this free flyer locator.")));
72 std::string localName(robotRef);
73 if (isNameInRobotUtil(localName)) {
75 std::cerr <<
"m_robot_util:" <<
m_robot_util << std::endl;
77 SEND_MSG(
"You should have an entity controller manager initialized before", MSG_TYPE_ERROR);
81 m_model =
new pinocchio::Model();
82 m_model->name.assign(
"EmptyRobot");
84 pinocchio::urdf::buildModel(
m_robot_util->m_urdf_filename, pinocchio::JointModelFreeFlyer(), *
m_model);
95 }
catch (
const std::exception& e) {
96 std::cout << e.what();
97 return SEND_MSG(
"Init failed: Could load URDF :" +
m_robot_util->m_urdf_filename, MSG_TYPE_ERROR);
108 if (!m_initSucceeded) {
109 SEND_WARNING_STREAM_MSG(
"Cannot compute signal kinematics_computations before initialization!");
113 const Eigen::VectorXd& q = m_base6d_encodersSIN(iter);
114 const Eigen::VectorXd& dq = m_joint_velocitiesSIN(iter);
115 assert(q.size() ==
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints + 6) &&
116 "Unexpected size of signal base6d_encoder");
117 assert(dq.size() ==
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints) &&
118 "Unexpected size of signal joint_velocities");
121 m_robot_util->joints_sot_to_urdf(q.tail(m_robot_util->m_nbJoints), m_q_pin.tail(m_robot_util->m_nbJoints));
122 m_robot_util->joints_sot_to_urdf(dq, m_v_pin.tail(m_robot_util->m_nbJoints));
125 pinocchio::forwardKinematics(*m_model, *m_data, m_q_pin, m_v_pin);
126 pinocchio::framesForwardKinematics(*m_model, *m_data);
132 if (!m_initSucceeded) {
133 SEND_WARNING_STREAM_MSG(
"Cannot compute signal base6dFromFoot_encoders before initialization!");
136 if (s.size() !=
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints + 6))
137 s.resize(m_robot_util->m_nbJoints + 6);
139 m_kinematics_computationsSINNER(iter);
143 const Eigen::VectorXd& q = m_base6d_encodersSIN(iter);
144 assert(q.size() ==
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints + 6) &&
145 "Unexpected size of signal base6d_encoder");
148 const pinocchio::SE3 iMo1(m_data->oMf[m_left_foot_id].inverse());
149 const pinocchio::SE3 iMo2(m_data->oMf[m_right_foot_id].inverse());
152 m_Mff = pinocchio::SE3(pinocchio::exp3(w), 0.5 * (iMo1.translation() + iMo2.translation()));
155 Eigen::Map<const Eigen::Vector3d> righ_foot_sole_xyz(&m_robot_util->m_foot_util.m_Right_Foot_Sole_XYZ[0]);
157 m_q_sot.tail(m_robot_util->m_nbJoints) = q.tail(m_robot_util->m_nbJoints);
158 base_se3_to_sot(m_Mff.translation() - righ_foot_sole_xyz, m_Mff.rotation(), m_q_sot.head<6>());
168 m_base6dFromFoot_encodersSOUT(iter);
169 if (!m_initSucceeded) {
170 SEND_WARNING_STREAM_MSG(
"Cannot compute signal freeflyer_aa before initialization!");
175 if (s.size() != 6) s.resize(6);
177 const Eigen::AngleAxisd aa(m_Mff.rotation());
178 dynamicgraph::sot::Vector6d freeflyer;
179 freeflyer << m_Mff.translation(), aa.axis() * aa.angle();
182 Eigen::Map<const Eigen::Vector3d> righ_foot_sole_xyz(&m_robot_util->m_foot_util.m_Right_Foot_Sole_XYZ[0]);
183 freeflyer.head<3>() -= righ_foot_sole_xyz;
190 if (!m_initSucceeded) {
191 SEND_WARNING_STREAM_MSG(
"Cannot compute signal v before initialization!");
194 if (s.size() !=
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints + 6))
195 s.resize(m_robot_util->m_nbJoints + 6);
197 m_kinematics_computationsSINNER(iter);
201 const Eigen::VectorXd& dq = m_joint_velocitiesSIN(iter);
202 assert(dq.size() ==
static_cast<Eigen::VectorXd::Index
>(m_robot_util->m_nbJoints) &&
203 "Unexpected size of signal joint_velocities");
206 const Frame& f_lf = m_model->frames[m_left_foot_id];
207 const Motion v_lf_local = f_lf.placement.actInv(m_data->v[f_lf.parent]);
208 const Vector6 v_lf = m_data->oMf[m_left_foot_id].act(v_lf_local).toVector();
210 const Frame& f_rf = m_model->frames[m_right_foot_id];
211 const Motion v_rf_local = f_rf.placement.actInv(m_data->v[f_rf.parent]);
212 const Vector6 v_rf = m_data->oMf[m_right_foot_id].act(v_rf_local).toVector();
214 m_v_sot.head<6>() = -0.5 * (v_lf + v_rf);
215 m_v_sot.tail(m_robot_util->m_nbJoints) = dq;
232 os <<
"FreeFlyerLocator " << getName();
234 getProfiler().report_all(3, os);
235 }
catch (ExceptionSignal e) {
#define PROFILE_FREE_FLYER_VELOCITY_COMPUTATION
Eigen::VectorXd m_v_sot
robot velocities according to pinocchio convention
Eigen::Matrix< double, 3, 1 > Vector3
void init(const std::string &robotRef)
EIGEN_MAKE_ALIGNED_OPERATOR_NEW FreeFlyerLocator(const std::string &name)
Eigen::VectorXd m_q_sot
robot configuration according to pinocchio convention
Eigen::VectorXd m_v_pin
robot configuration according to SoT convention
long unsigned int m_left_foot_id
DEFINE_SIGNAL_INNER_FUNCTION(kinematics_computations, dynamicgraph::Vector)
virtual void display(std::ostream &os) const
pinocchio::Data * m_data
Pinocchio robot model.
long unsigned int m_right_foot_id
Eigen::Matrix< double, 6, 1 > Vector6
AdmittanceController EntityClassName
RobotUtilShrPtr m_robot_util
robot velocities according to SoT convention
DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector)
pinocchio::Model * m_model
true if the entity has been successfully initialized
#define PROFILE_FREE_FLYER_COMPUTATION
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController, "AdmittanceController")