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 
37 #pragma once
38 #include <Eigen/Geometry>
39 #include <ignition/math/Vector3.hh>
40 #include <ignition/math/Quaternion.hh>
41 #include <ignition/math/Pose3.hh>
42 #include <gazebo/msgs/wrench_stamped.pb.h>
43 
44 inline Eigen::Matrix<double,6,1> gazeboMsgToEigen(const ::gazebo::msgs::WrenchStamped& w)
45 {
46  Eigen::Matrix<double,6,1> e;
47  e[0] = w.wrench().force().x();
48  e[1] = w.wrench().force().y();
49  e[2] = w.wrench().force().z();
50  e[3] = w.wrench().torque().x();
51  e[4] = w.wrench().torque().y();
52  e[5] = w.wrench().torque().z();
53  return e;
54 }
55 
56 inline Eigen::Quaterniond quatFromRPY(double roll,double pitch,double yaw )
57 {
58  return Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX())
59  * Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY())
60  * Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ());
61 }
62 
63 // Adapted from DARTTypes.hh
64 inline Eigen::Vector3d convVec3(const ignition::math::Vector3d &_vec3)
65 {
66  return Eigen::Vector3d(_vec3.X(),_vec3.Y(),_vec3.Z());
67 }
68 
69 inline ignition::math::Vector3d convVec3(const Eigen::Vector3d &_vec3)
70 {
71  return ignition::math::Vector3d(_vec3[0], _vec3[1], _vec3[2]);
72 }
73 
74 inline Eigen::Quaterniond convQuat(const ignition::math::Quaterniond &_quat)
75 {
76  return Eigen::Quaterniond(_quat.W(), _quat.X(), _quat.Y(), _quat.Z());
77 }
78 
79 inline ignition::math::Quaterniond convQuat(const Eigen::Quaterniond &_quat)
80 {
81  return ignition::math::Quaterniond(_quat.w(), _quat.x(), _quat.y(), _quat.z());
82 }
83 
84 inline Eigen::Affine3d convPose(const ignition::math::Pose3d &_pose)
85 {
86  Eigen::Affine3d res;
87 
88  res.translation() = convVec3(_pose.Pos());
89  res.linear() = convQuat(_pose.Rot()).toRotationMatrix();
90 
91  return res;
92 }
93 
94 inline ignition::math::Pose3d convPose(const Eigen::Affine3d &_T)
95 {
96  ignition::math::Pose3d pose;
97  pose.Pos() = convVec3(_T.translation());
98  pose.Rot() = convQuat(Eigen::Quaterniond(_T.linear()));
99  return pose;
100 }
101 
102 
103 // Allows to use the gazebo command line
104 #include <stdio.h>
105 inline std::string custom_exec(const std::string &_cmd)
106 {
107  FILE* pipe = popen(_cmd.c_str(), "r");
108 
109  if (!pipe)
110  return "ERROR";
111 
112  char buffer[128];
113  std::string result = "";
114 
115  while (!feof(pipe))
116  {
117  if (fgets(buffer, 128, pipe) != NULL)
118  result += buffer;
119  }
120 
121  pclose(pipe);
122  return result;
123 }
124 
Eigen::Matrix< double, 6, 1 > gazeboMsgToEigen(const ::gazebo::msgs::WrenchStamped &w)
Definition: Utils.h:44
Eigen::Quaterniond quatFromRPY(double roll, double pitch, double yaw)
Definition: Utils.h:56
std::string custom_exec(const std::string &_cmd)
Definition: Utils.h:105
Eigen::Vector3d convVec3(const ignition::math::Vector3d &_vec3)
Definition: Utils.h:64
Eigen::Quaterniond convQuat(const ignition::math::Quaterniond &_quat)
Definition: Utils.h:74
Eigen::Affine3d convPose(const ignition::math::Pose3d &_pose)
Definition: Utils.h:84