ORCA: Optimization-based framework for Robotic Control Applications
Utils.h
Go to the documentation of this file.
1 //| This file is a part of the ORCA framework.
2 //|
3 //| Copyright 2018, Fuzzy Logic Robotics
4 //| Copyright 2017, ISIR / Universite Pierre et Marie Curie (UPMC)
5 //|
6 //| Main contributor(s): Antoine Hoarau, Ryan Lober, and
7 //| Fuzzy Logic Robotics <info@fuzzylogicrobotics.com>
8 //|
9 //| ORCA is a whole-body reactive controller framework for robotics.
10 //|
11 //| This software is governed by the CeCILL-C license under French law and
12 //| abiding by the rules of distribution of free software. You can use,
13 //| modify and/ or redistribute the software under the terms of the CeCILL-C
14 //| license as circulated by CEA, CNRS and INRIA at the following URL
15 //| "http://www.cecill.info".
16 //|
17 //| As a counterpart to the access to the source code and rights to copy,
18 //| modify and redistribute granted by the license, users are provided only
19 //| with a limited warranty and the software's author, the holder of the
20 //| economic rights, and the successive licensors have only limited
21 //| liability.
22 //|
23 //| In this respect, the user's attention is drawn to the risks associated
24 //| with loading, using, modifying and/or developing or reproducing the
25 //| software by the user in light of its specific status of free software,
26 //| that may mean that it is complicated to manipulate, and that also
27 //| therefore means that it is reserved for developers and experienced
28 //| professionals having in-depth computer knowledge. Users are therefore
29 //| encouraged to load and test the software's suitability as regards their
30 //| requirements in conditions enabling the security of their systems and/or
31 //| data to be ensured and, more generally, to use and operate it in the
32 //| same conditions as regards security.
33 //|
34 //| The fact that you are presently reading this means that you have had
35 //| knowledge of the CeCILL-C license and that you accept its terms.
36 #pragma once
37 #include <Eigen/Geometry>
38 #include <ignition/math/Vector3.hh>
39 #include <ignition/math/Quaternion.hh>
40 #include <ignition/math/Pose3.hh>
41 #include <gazebo/msgs/wrench_stamped.pb.h>
42 #include <orca/math/Utils.h>
43 
44 inline Eigen::Matrix<double,6,1>& gazeboMsgToEigen(const ::gazebo::msgs::WrenchStamped& w, Eigen::Matrix<double,6,1>& e_out)
45 {
46  e_out[0] = w.wrench().force().x();
47  e_out[1] = w.wrench().force().y();
48  e_out[2] = w.wrench().force().z();
49  e_out[3] = w.wrench().torque().x();
50  e_out[4] = w.wrench().torque().y();
51  e_out[5] = w.wrench().torque().z();
52  return e_out;
53 }
54 
55 inline Eigen::Matrix<double,6,1> gazeboMsgToEigen(const ::gazebo::msgs::WrenchStamped& w)
56 {
57  Eigen::Matrix<double,6,1> e;
58  return gazeboMsgToEigen(w, e);
59 }
60 
61 inline Eigen::Quaterniond quatFromRPY(double roll,double pitch,double yaw )
62 {
63  return Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX())
64  * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())
65  * Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ());
66 }
67 
68 // Adapted from DARTTypes.hh
69 inline Eigen::Vector3d convVec3(const ignition::math::Vector3d &_vec3)
70 {
71  return Eigen::Vector3d(_vec3.X(),_vec3.Y(),_vec3.Z());
72 }
73 
74 inline Eigen::Vector3d convVec3(const gazebo::msgs::Vector3d &_vec3)
75 {
76  return Eigen::Vector3d(_vec3.x(),_vec3.y(),_vec3.z());
77 }
78 
79 inline ignition::math::Vector3d convVec3(const Eigen::Vector3d &_vec3)
80 {
81  return ignition::math::Vector3d(_vec3[0], _vec3[1], _vec3[2]);
82 }
83 
84 inline Eigen::Quaterniond convQuat(const ignition::math::Quaterniond &_quat)
85 {
86  return Eigen::Quaterniond(_quat.W(), _quat.X(), _quat.Y(), _quat.Z());
87 }
88 
89 inline ignition::math::Quaterniond convQuat(const Eigen::Quaterniond &_quat)
90 {
91  return ignition::math::Quaterniond(_quat.w(), _quat.x(), _quat.y(), _quat.z());
92 }
93 
94 inline Eigen::Affine3d convPose(const ignition::math::Pose3d &_pose)
95 {
96  Eigen::Affine3d res;
97 
98  res.translation() = convVec3(_pose.Pos());
99  res.linear() = convQuat(_pose.Rot()).toRotationMatrix();
100 
101  return res;
102 }
103 
104 inline ignition::math::Pose3d convPose(const Eigen::Affine3d &_T)
105 {
106  ignition::math::Pose3d pose;
107  pose.Pos() = convVec3(_T.translation());
108  pose.Rot() = convQuat(Eigen::Quaterniond(_T.linear()));
109  return pose;
110 }
111 
112 
113 // Allows to use the gazebo command line
114 #include <stdio.h>
115 inline std::string custom_exec(const std::string &_cmd)
116 {
117  FILE* pipe = popen(_cmd.c_str(), "r");
118 
119  if (!pipe)
120  return "ERROR";
121 
122  char buffer[128];
123  std::string result = "";
124 
125  while (!feof(pipe))
126  {
127  if (fgets(buffer, 128, pipe) != NULL)
128  result += buffer;
129  }
130 
131  pclose(pipe);
132  return result;
133 }
134 
135 
136 namespace orca
137 {
138 namespace gazebo
139 {
140 
149 {
150 public:
152  {}
153 
155  {}
156 
157 protected:
158  GazeboWrenchSensor(const std::string& base_link)
159  : base_link_(base_link)
160  {}
161 
162 public:
166  const Eigen::Matrix<double,6,1>& getWrench() const
167  {
168  return wrench_;
169  }
170 
174  const std::string& getBaseLinkName()
175  {
176  return base_link_;
177  }
178 
179 protected:
180  void setConnection(::gazebo::event::ConnectionPtr conn_ptr)
181  {
182  connection_ptr_ = conn_ptr;
183  }
184 
189  void setWrench(const Eigen::Matrix<double,6,1>& wrench)
190  {
191  wrench_ = wrench;
192  }
193 
194  Eigen::Matrix<double,6,1>& wrench()
195  {
196  return wrench_;
197  }
198 
199 private:
200  ::gazebo::event::ConnectionPtr connection_ptr_;
201  std::string base_link_;
202  Eigen::Matrix<double,6,1> wrench_;
203 
204 }; // GazeboWrenchSensor
205 
207 {
208 public:
209  GazeboForceTorqueSensor(const std::string& base_link)
210  : GazeboWrenchSensor(base_link)
211  {}
212 
213  GazeboForceTorqueSensor(::gazebo::physics::LinkPtr link)
214  : GazeboWrenchSensor(link->GetName())
215  {}
216 
217 public:
218  void connectUpdate(std::function<void (::gazebo::msgs::WrenchStamped)> subscriber)
219  {
220  if (sensor_ != nullptr)
222  sensor_->ConnectUpdate(subscriber)
223  );
224  }
225 
230  void setSensor(::gazebo::sensors::ForceTorqueSensorPtr sensor)
231  {
232  sensor_ = sensor;
233  }
234 
235  ::gazebo::sensors::ForceTorqueSensorPtr getSensor() const
236  {
237  return sensor_;
238  }
239 
240  void setWrenchFrom(const ::gazebo::msgs::WrenchStamped& wrench_stamped)
241  {
242  gazeboMsgToEigen(wrench_stamped, wrench());
243  }
244 
245 private:
246  ::gazebo::sensors::ForceTorqueSensorPtr sensor_ = nullptr;
247 }; // GazeboForceTorqueSensor
248 
250 {
251 public:
252  GazeboContactSensor(::gazebo::physics::LinkPtr link, const std::string& collision)
253  : GazeboWrenchSensor(link->GetName()), collision_(collision), link_(link)
254  {}
255 
256 public:
257  void connectUpdate(std::function<void (void)> subscriber)
258  {
259  if (sensor_ != nullptr)
261  sensor_->ConnectUpdated(subscriber)
262  );
263  }
264 
265  const std::string& getCollisionName() const
266  {
267  return collision_;
268  }
269 
273  void setSensor(::gazebo::sensors::ContactSensorPtr sensor)
274  {
275  sensor_ = sensor;
276  }
277 
278  ::gazebo::sensors::ContactSensorPtr getSensor() const
279  {
280  return sensor_;
281  }
282 
283  ::gazebo::physics::LinkPtr getLink() const
284  {
285  return link_;
286  }
287 
288  void setWrenchFrom( void )
289  {
299  if (link_ == nullptr)
300  throw std::runtime_error("Link not set.");
301  ::gazebo::msgs::Contacts contacts = this->getSensor()->Contacts();
302  // "The forces and torques are applied at the CG of perspective links for each collision body, specified in the inertial frame."
303  // -> appears to be wrong, forces and torques in the LINK frame w.r.t COG
304  Eigen::Matrix<double,6,1> wrench_res_atCoG_inLink = Eigen::Matrix<double,6,1>::Zero();
305  auto linkCoGPose = this->getLink()->GetInertial()->Pose(); // the Pose of the CoG in the link Frame
306  std::string scoped_collision_name = link_->GetModel()->GetName() + "::" + this->getBaseLinkName() + "::" + this->getCollisionName();
307  for (unsigned int i = 0; i < contacts.contact_size(); ++i)
308  {
309  for (unsigned int j = 0; j < contacts.contact(i).wrench_size(); ++j)
310  {
311  if (0 == contacts.contact(i).wrench(j).body_1_name().compare(scoped_collision_name))
312  {
313  wrench_res_atCoG_inLink.head<3>() += convVec3(contacts.contact(i).wrench(j).body_1_wrench().force());
314  wrench_res_atCoG_inLink.tail<3>() += convVec3(contacts.contact(i).wrench(j).body_1_wrench().torque());
315  }
316  else if (0 == contacts.contact(i).wrench(j).body_2_name().compare(scoped_collision_name))
317  {
318  wrench_res_atCoG_inLink.head<3>() += convVec3(contacts.contact(i).wrench(j).body_2_wrench().force());
319  wrench_res_atCoG_inLink.tail<3>() += convVec3(contacts.contact(i).wrench(j).body_2_wrench().torque());
320  }
321  else
322  throw std::runtime_error("Mismatching collision bodies names");
323  }
324  }
325  orca::math::transportWrench(wrench_res_atCoG_inLink, convVec3(linkCoGPose.Pos()) , this->wrench());
326  }
327 
328 private:
329  ::gazebo::sensors::ContactSensorPtr sensor_ = nullptr;
330  ::gazebo::physics::LinkPtr link_ = nullptr;
331  std::string collision_;
332 }; // GazeboContactSensor
333 
334 
335 } // namespace gazebo
336 } // namespace orca
const Eigen::Matrix< double, 6, 1 > & getWrench() const
returns the wrench in the base_link frame
Definition: Utils.h:166
const std::string & getCollisionName() const
Definition: Utils.h:265
void connectUpdate(std::function< void(::gazebo::msgs::WrenchStamped)> subscriber)
Definition: Utils.h:218
GazeboForceTorqueSensor(::gazebo::physics::LinkPtr link)
Definition: Utils.h:213
void setSensor(::gazebo::sensors::ForceTorqueSensorPtr sensor)
sets the ForceTorqueSensorPtr to connect to
Definition: Utils.h:230
Definition: GazeboServer.h:59
void setWrench(const Eigen::Matrix< double, 6, 1 > &wrench)
sets the wrench in the base_link frame
Definition: Utils.h:189
Definition: Utils.h:249
Eigen::Matrix< double, 6, 1 > & wrench()
Definition: Utils.h:194
Eigen::Quaterniond quatFromRPY(double roll, double pitch, double yaw)
Definition: Utils.h:61
void setWrenchFrom(void)
Definition: Utils.h:288
::gazebo::sensors::ForceTorqueSensorPtr getSensor() const
Definition: Utils.h:235
const std::string & getBaseLinkName()
returns the wrench in the base_link name, from the world to the body
Definition: Utils.h:174
std::string custom_exec(const std::string &_cmd)
Definition: Utils.h:115
GazeboWrenchSensor()
Definition: Utils.h:151
void setSensor(::gazebo::sensors::ContactSensorPtr sensor)
sets the ContactSensorPtr to get contact messages from
Definition: Utils.h:273
void connectUpdate(std::function< void(void)> subscriber)
Definition: Utils.h:257
GazeboWrenchSensor(const std::string &base_link)
Definition: Utils.h:158
::gazebo::physics::LinkPtr getLink() const
Definition: Utils.h:283
Eigen::Vector3d convVec3(const ignition::math::Vector3d &_vec3)
Definition: Utils.h:69
void setWrenchFrom(const ::gazebo::msgs::WrenchStamped &wrench_stamped)
Definition: Utils.h:240
GazeboForceTorqueSensor(const std::string &base_link)
Definition: Utils.h:209
virtual ~GazeboWrenchSensor()
Definition: Utils.h:154
Definition: CartesianAccelerationPID.h:44
Eigen::Matrix< double, 6, 1 > & gazeboMsgToEigen(const ::gazebo::msgs::WrenchStamped &w, Eigen::Matrix< double, 6, 1 > &e_out)
Definition: Utils.h:44
Eigen::Quaterniond convQuat(const ignition::math::Quaterniond &_quat)
Definition: Utils.h:84
void setConnection(::gazebo::event::ConnectionPtr conn_ptr)
Definition: Utils.h:180
GazeboContactSensor(::gazebo::physics::LinkPtr link, const std::string &collision)
Definition: Utils.h:252
Eigen::Affine3d convPose(const ignition::math::Pose3d &_pose)
Definition: Utils.h:94
A Wrench sensor w.r.t. a base_link. Wrench is expressed from the world to the body, in the base_link frame.
Definition: Utils.h:148
::gazebo::sensors::ContactSensorPtr getSensor() const
Definition: Utils.h:278
Eigen::Matrix< double, 6, 1 > & transportWrench(const Eigen::Matrix< double, 6, 1 > &wrench_in_a, const Eigen::Matrix< double, 3, 1 > &b_to_a, Eigen::Matrix< double, 6, 1 > &wrench_in_b_out)
Definition: Utils.h:97