sot-torque-control  1.6.4
Collection of dynamic-graph entities aimed at implementing torque control on different robots.
 
Loading...
Searching...
No Matches
free-flyer-locator.cpp
Go to the documentation of this file.
1/*
2 * Copyright 2017, Thomas Flayols, LAAS-CNRS
3 *
4 */
5
6#include <dynamic-graph/factory.h>
7
8#include <sot/core/debug.hh>
9#include <sot/core/stop-watch.hh>
12
13#include "pinocchio/algorithm/frames.hpp"
14
15namespace dynamicgraph {
16namespace sot {
17namespace torque_control {
18namespace dynamicgraph = ::dynamicgraph;
19using namespace dynamicgraph;
20using namespace dynamicgraph::command;
21using namespace std;
22using namespace pinocchio;
23
24typedef dynamicgraph::sot::Vector6d Vector6;
25
26#define PROFILE_FREE_FLYER_COMPUTATION "Free-flyer position computation"
27#define PROFILE_FREE_FLYER_VELOCITY_COMPUTATION \
28 "Free-flyer velocity computation"
29
30#define INPUT_SIGNALS m_base6d_encodersSIN << m_joint_velocitiesSIN
31#define OUTPUT_SIGNALS \
32 m_freeflyer_aaSOUT << m_base6dFromFoot_encodersSOUT << m_vSOUT
33
36typedef FreeFlyerLocator EntityClassName;
37
38/* --- DG FACTORY ---------------------------------------------------- */
40
41/* ------------------------------------------------------------------- */
42/* --- CONSTRUCTION -------------------------------------------------- */
43/* ------------------------------------------------------------------- */
44FreeFlyerLocator::FreeFlyerLocator(const std::string& name)
45 : Entity(name),
46 CONSTRUCT_SIGNAL_IN(base6d_encoders, dynamicgraph::Vector),
47 CONSTRUCT_SIGNAL_IN(joint_velocities, dynamicgraph::Vector),
48 CONSTRUCT_SIGNAL_INNER(kinematics_computations, dynamicgraph::Vector,
50 CONSTRUCT_SIGNAL_OUT(freeflyer_aa, dynamicgraph::Vector,
51 m_base6dFromFoot_encodersSOUT),
52 CONSTRUCT_SIGNAL_OUT(base6dFromFoot_encoders, dynamicgraph::Vector,
53 m_kinematics_computationsSINNER),
54 CONSTRUCT_SIGNAL_OUT(v, dynamicgraph::Vector,
55 m_kinematics_computationsSINNER),
56 m_initSucceeded(false),
57 m_model(0),
58 m_data(0),
59 m_robot_util(RefVoidRobotUtil()) {
60 Entity::signalRegistration(INPUT_SIGNALS << OUTPUT_SIGNALS);
61
62 /* Commands. */
63 addCommand("init",
64 makeCommandVoid1(*this, &FreeFlyerLocator::init,
65 docCommandVoid1("Initialize the entity.",
66 "Robot reference (string)")));
67
68 addCommand(
69 "displayRobotUtil",
70 makeCommandVoid0(*this, &FreeFlyerLocator::displayRobotUtil,
71 docCommandVoid0("Display the robot util data set linked "
72 "with this free flyer locator.")));
73}
75 if (m_model) delete m_model;
76 if (m_data) delete m_data;
77}
78
79void FreeFlyerLocator::init(const std::string& robotRef) {
80 try {
81 /* Retrieve m_robot_util informations */
82 std::string localName(robotRef);
83 if (isNameInRobotUtil(localName)) {
84 m_robot_util = getRobotUtil(localName);
85 std::cerr << "m_robot_util:" << m_robot_util << std::endl;
86 } else {
87 SEND_MSG(
88 "You should have an entity controller manager initialized before",
89 MSG_TYPE_ERROR);
90 return;
91 }
92
93 m_model = new pinocchio::Model();
94 m_model->name.assign("EmptyRobot");
95
96 pinocchio::urdf::buildModel(m_robot_util->m_urdf_filename,
97 pinocchio::JointModelFreeFlyer(), *m_model);
98 assert(m_model->nv == static_cast<int>(m_robot_util->m_nbJoints + 6));
99 assert(
100 m_model->existFrame(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name));
101 assert(
102 m_model->existFrame(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name));
104 m_model->getFrameId(m_robot_util->m_foot_util.m_Left_Foot_Frame_Name);
106 m_model->getFrameId(m_robot_util->m_foot_util.m_Right_Foot_Frame_Name);
107 m_q_pin.setZero(m_model->nq);
108 m_q_pin[6] = 1.; // for quaternion
109 m_q_sot.setZero(m_robot_util->m_nbJoints + 6);
110 m_v_pin.setZero(m_robot_util->m_nbJoints + 6);
111 m_v_sot.setZero(m_robot_util->m_nbJoints + 6);
112 } catch (const std::exception& e) {
113 std::cout << e.what();
114 return SEND_MSG(
115 "Init failed: Could load URDF :" + m_robot_util->m_urdf_filename,
116 MSG_TYPE_ERROR);
117 }
118 m_data = new pinocchio::Data(*m_model);
119 m_initSucceeded = true;
120}
121
122/* ------------------------------------------------------------------- */
123/* --- SIGNALS ------------------------------------------------------- */
124/* ------------------------------------------------------------------- */
125
126DEFINE_SIGNAL_INNER_FUNCTION(kinematics_computations, dynamicgraph::Vector) {
127 if (!m_initSucceeded) {
128 SEND_WARNING_STREAM_MSG(
129 "Cannot compute signal kinematics_computations before initialization!");
130 return s;
131 }
132
133 const Eigen::VectorXd& q = m_base6d_encodersSIN(iter); // n+6
134 const Eigen::VectorXd& dq = m_joint_velocitiesSIN(iter);
135 assert(q.size() == static_cast<Eigen::VectorXd::Index>(
136 m_robot_util->m_nbJoints + 6) &&
137 "Unexpected size of signal base6d_encoder");
138 assert(dq.size() ==
139 static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) &&
140 "Unexpected size of signal joint_velocities");
141
142 /* convert sot to pinocchio joint order */
143 m_robot_util->joints_sot_to_urdf(q.tail(m_robot_util->m_nbJoints),
144 m_q_pin.tail(m_robot_util->m_nbJoints));
145 m_robot_util->joints_sot_to_urdf(dq, m_v_pin.tail(m_robot_util->m_nbJoints));
146
147 /* Compute kinematic and return q with freeflyer */
148 pinocchio::forwardKinematics(*m_model, *m_data, m_q_pin, m_v_pin);
149 pinocchio::framesForwardKinematics(*m_model, *m_data);
150
151 return s;
152}
153
154DEFINE_SIGNAL_OUT_FUNCTION(base6dFromFoot_encoders, dynamicgraph::Vector) {
155 if (!m_initSucceeded) {
156 SEND_WARNING_STREAM_MSG(
157 "Cannot compute signal base6dFromFoot_encoders before initialization!");
158 return s;
159 }
160 if (s.size() !=
161 static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints + 6))
162 s.resize(m_robot_util->m_nbJoints + 6);
163
164 m_kinematics_computationsSINNER(iter);
165
166 getProfiler().start(PROFILE_FREE_FLYER_COMPUTATION);
167 {
168 const Eigen::VectorXd& q = m_base6d_encodersSIN(iter); // n+6
169 assert(q.size() == static_cast<Eigen::VectorXd::Index>(
170 m_robot_util->m_nbJoints + 6) &&
171 "Unexpected size of signal base6d_encoder");
172
173 /* Compute kinematic and return q with freeflyer */
174 const pinocchio::SE3 iMo1(m_data->oMf[m_left_foot_id].inverse());
175 const pinocchio::SE3 iMo2(m_data->oMf[m_right_foot_id].inverse());
176 // Average in SE3
177 const pinocchio::SE3::Vector3 w(0.5 * (pinocchio::log3(iMo1.rotation()) +
178 pinocchio::log3(iMo2.rotation())));
179 m_Mff = pinocchio::SE3(pinocchio::exp3(w),
180 0.5 * (iMo1.translation() + iMo2.translation()));
181
182 // due to distance from ankle to ground
183 Eigen::Map<const Eigen::Vector3d> righ_foot_sole_xyz(
184 &m_robot_util->m_foot_util.m_Right_Foot_Sole_XYZ[0]);
185
186 m_q_sot.tail(m_robot_util->m_nbJoints) = q.tail(m_robot_util->m_nbJoints);
187 base_se3_to_sot(m_Mff.translation() - righ_foot_sole_xyz, m_Mff.rotation(),
188 m_q_sot.head<6>());
189
190 s = m_q_sot;
191 }
192 getProfiler().stop(PROFILE_FREE_FLYER_COMPUTATION);
193
194 return s;
195}
196
197DEFINE_SIGNAL_OUT_FUNCTION(freeflyer_aa, dynamicgraph::Vector) {
198 m_base6dFromFoot_encodersSOUT(iter);
199 if (!m_initSucceeded) {
200 SEND_WARNING_STREAM_MSG(
201 "Cannot compute signal freeflyer_aa before initialization!");
202 return s;
203 }
204 // oMi is has been calulated before since we depend on base6dFromFoot_encoders
205 // signal. just read the data, convert to axis angle
206 if (s.size() != 6) s.resize(6);
207 //~ const pinocchio::SE3 & iMo = m_data->oMi[31].inverse();
208 const Eigen::AngleAxisd aa(m_Mff.rotation());
209 dynamicgraph::sot::Vector6d freeflyer;
210 freeflyer << m_Mff.translation(), aa.axis() * aa.angle();
211
212 // due to distance from ankle to ground
213 Eigen::Map<const Eigen::Vector3d> righ_foot_sole_xyz(
214 &m_robot_util->m_foot_util.m_Right_Foot_Sole_XYZ[0]);
215 freeflyer.head<3>() -= righ_foot_sole_xyz;
216
217 s = freeflyer;
218 return s;
219}
220
221DEFINE_SIGNAL_OUT_FUNCTION(v, dynamicgraph::Vector) {
222 if (!m_initSucceeded) {
223 SEND_WARNING_STREAM_MSG("Cannot compute signal v before initialization!");
224 return s;
225 }
226 if (s.size() !=
227 static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints + 6))
228 s.resize(m_robot_util->m_nbJoints + 6);
229
230 m_kinematics_computationsSINNER(iter);
231
232 getProfiler().start(PROFILE_FREE_FLYER_VELOCITY_COMPUTATION);
233 {
234 const Eigen::VectorXd& dq = m_joint_velocitiesSIN(iter);
235 assert(dq.size() ==
236 static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints) &&
237 "Unexpected size of signal joint_velocities");
238
239 /* Compute foot velocities */
240 const Frame& f_lf = m_model->frames[m_left_foot_id];
241 const Motion v_lf_local = f_lf.placement.actInv(m_data->v[f_lf.parent]);
242 const Vector6 v_lf = m_data->oMf[m_left_foot_id].act(v_lf_local).toVector();
243
244 const Frame& f_rf = m_model->frames[m_right_foot_id];
245 const Motion v_rf_local = f_rf.placement.actInv(m_data->v[f_rf.parent]);
246 const Vector6 v_rf =
247 m_data->oMf[m_right_foot_id].act(v_rf_local).toVector();
248
249 m_v_sot.head<6>() = -0.5 * (v_lf + v_rf);
250 m_v_sot.tail(m_robot_util->m_nbJoints) = dq;
251
252 s = m_v_sot;
253 }
254 getProfiler().stop(PROFILE_FREE_FLYER_VELOCITY_COMPUTATION);
255
256 return s;
257}
258
259/* --- COMMANDS ---------------------------------------------------------- */
261
262/* ------------------------------------------------------------------- */
263/* --- ENTITY -------------------------------------------------------- */
264/* ------------------------------------------------------------------- */
265
266void FreeFlyerLocator::display(std::ostream& os) const {
267 os << "FreeFlyerLocator " << getName();
268 try {
269 getProfiler().report_all(3, os);
270 } catch (ExceptionSignal e) {
271 }
272}
273} // namespace torque_control
274} // namespace sot
275} // namespace dynamicgraph
#define INPUT_SIGNALS
#define OUTPUT_SIGNALS
Eigen::VectorXd m_v_pin
robot configuration according to SoT convention
EIGEN_MAKE_ALIGNED_OPERATOR_NEW FreeFlyerLocator(const std::string &name)
RobotUtilShrPtr m_robot_util
robot velocities according to SoT convention
pinocchio::Data * m_data
Pinocchio robot model.
pinocchio::Model * m_model
true if the entity has been successfully initialized
Eigen::VectorXd m_v_sot
robot velocities according to pinocchio convention
Eigen::VectorXd m_q_sot
robot configuration according to pinocchio convention
#define PROFILE_FREE_FLYER_COMPUTATION
#define PROFILE_FREE_FLYER_VELOCITY_COMPUTATION
DEFINE_SIGNAL_OUT_FUNCTION(u, dynamicgraph::Vector)
DEFINE_SIGNAL_INNER_FUNCTION(kinematics_computations, dynamicgraph::Vector)
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceController, "AdmittanceController")
Eigen::Matrix< double, 6, 1 > Vector6
to read text file
Definition: treeview.dox:22