convert.h
Go to the documentation of this file.
1 /******************************************************************************
2 Copyright (c) 2017, Alexander W. Winkler. All rights reserved.
3 
4 Redistribution and use in source and binary forms, with or without
5 modification, are permitted provided that the following conditions are met:
6 
7 * Redistributions of source code must retain the above copyright notice, this
8  list of conditions and the following disclaimer.
9 
10 * Redistributions in binary form must reproduce the above copyright notice,
11  this list of conditions and the following disclaimer in the documentation
12  and/or other materials provided with the distribution.
13 
14 * Neither the name of the copyright holder nor the names of its
15  contributors may be used to endorse or promote products derived from
16  this software without specific prior written permission.
17 
18 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
19 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
20 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
21 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
22 FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
23 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
24 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
25 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
26 OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
27 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
28 ******************************************************************************/
29 
30 #ifndef XPP_ROS_CONVERSIONS_H_
31 #define XPP_ROS_CONVERSIONS_H_
32 
33 #include <vector>
34 
35 #include <xpp_msgs/StateLin3d.h>
36 #include <xpp_msgs/State6d.h>
37 #include <xpp_msgs/RobotStateCartesian.h>
38 #include <xpp_msgs/RobotStateCartesianTrajectory.h>
39 
40 #include <xpp_states/state.h>
42 
43 namespace xpp {
44 
51 struct Convert {
52 
53 static StateLin3d
54 ToXpp(const xpp_msgs::StateLin3d& ros)
55 {
56  StateLin3d point;
57  point.p_.x() = ros.pos.x;
58  point.p_.y() = ros.pos.y;
59  point.p_.z() = ros.pos.z;
60 
61  point.v_.x() = ros.vel.x;
62  point.v_.y() = ros.vel.y;
63  point.v_.z() = ros.vel.z;
64 
65  point.a_.x() = ros.acc.x;
66  point.a_.y() = ros.acc.y;
67  point.a_.z() = ros.acc.z;
68 
69  return point;
70 }
71 
72 static xpp_msgs::StateLin3d
74 {
75  xpp_msgs::StateLin3d ros;
76  ros.pos.x = xpp.p_.x();
77  ros.pos.y = xpp.p_.y();
78  ros.pos.z = xpp.p_.z();
79 
80  ros.vel.x = xpp.v_.x();
81  ros.vel.y = xpp.v_.y();
82  ros.vel.z = xpp.v_.z();
83 
84  ros.acc.x = xpp.a_.x();
85  ros.acc.y = xpp.a_.y();
86  ros.acc.z = xpp.a_.z();
87 
88  return ros;
89 }
90 
91 template<typename T>
92 static Vector3d
93 ToXpp(const T& ros)
94 {
95  Vector3d vec;
96  vec << ros.x, ros.y, ros.z;
97  return vec;
98 }
99 
100 template<typename T>
101 static T
103 {
104  T ros;
105  ros.x = xpp.x();
106  ros.y = xpp.y();
107  ros.z = xpp.z();
108 
109  return ros;
110 }
111 
112 template<typename T>
114 ToXpp(const std::vector<T>& ros)
115 {
116  Endeffectors<Vector3d> xpp(ros.size());
117 
118  for (auto ee : xpp.GetEEsOrdered())
119  xpp.at(ee) = ToXpp(ros.at(ee));
120 
121  return xpp;
122 }
123 
124 static Eigen::Quaterniond
125 ToXpp(const geometry_msgs::Quaternion ros)
126 {
128  xpp.w() = ros.w;
129  xpp.x() = ros.x;
130  xpp.y() = ros.y;
131  xpp.z() = ros.z;
132 
133  return xpp;
134 }
135 
136 static geometry_msgs::Quaternion
138 {
139  geometry_msgs::Quaternion ros;
140  ros.w = xpp.w();
141  ros.x = xpp.x();
142  ros.y = xpp.y();
143  ros.z = xpp.z();
144 
145  return ros;
146 }
147 
148 static xpp_msgs::State6d
150 {
151  xpp_msgs::State6d msg;
152 
153  msg.pose.position = ToRos<geometry_msgs::Point>(xpp.lin.p_);
154  msg.twist.linear = ToRos<geometry_msgs::Vector3>(xpp.lin.v_);
155  msg.accel.linear = ToRos<geometry_msgs::Vector3>(xpp.lin.a_);
156 
157  msg.pose.orientation = ToRos(xpp.ang.q);
158  msg.twist.angular = ToRos<geometry_msgs::Vector3>(xpp.ang.w);
159  msg.accel.angular = ToRos<geometry_msgs::Vector3>(xpp.ang.wd);
160 
161  return msg;
162 }
163 
164 static State3d
165 ToXpp(const xpp_msgs::State6d& ros)
166 {
167  State3d xpp;
168 
169  xpp.lin.p_ = ToXpp(ros.pose.position);
170  xpp.lin.v_ = ToXpp(ros.twist.linear);
171  xpp.lin.a_ = ToXpp(ros.accel.linear);
172 
173  xpp.ang.q = ToXpp(ros.pose.orientation);
174  xpp.ang.w = ToXpp(ros.twist.angular);
175  xpp.ang.wd = ToXpp(ros.accel.angular);
176 
177  return xpp;
178 }
179 
180 static xpp_msgs::RobotStateCartesian
182 {
183  xpp_msgs::RobotStateCartesian ros;
184 
185  ros.base = ToRos(xpp.base_);
186  ros.time_from_start = ros::Duration(xpp.t_global_);
187 
188  for (auto ee : xpp.ee_contact_.GetEEsOrdered()) {
189  ros.ee_motion. push_back(ToRos(xpp.ee_motion_.at(ee)));
190  ros.ee_contact.push_back(xpp.ee_contact_.at(ee));
191  ros.ee_forces. push_back(ToRos<geometry_msgs::Vector3>(xpp.ee_forces_.at(ee)));
192  }
193 
194  return ros;
195 }
196 
197 static RobotStateCartesian
198 ToXpp(const xpp_msgs::RobotStateCartesian& ros)
199 {
200 
201  int n_ee = ros.ee_motion.size();
202  RobotStateCartesian xpp(n_ee);
203 
204  xpp.base_ = ToXpp(ros.base);
205  xpp.t_global_ = ros.time_from_start.toSec();
206 
207  for (auto ee : xpp.ee_contact_.GetEEsOrdered()) {
208  xpp.ee_motion_.at(ee) = ToXpp(ros.ee_motion.at(ee));
209  xpp.ee_contact_.at(ee) = ros.ee_contact.at(ee);
210  xpp.ee_forces_.at(ee) = ToXpp(ros.ee_forces.at(ee));
211  }
212 
213  return xpp;
214 }
215 
216 static xpp_msgs::RobotStateCartesianTrajectory
217 ToRos(const std::vector<RobotStateCartesian>& xpp)
218 {
219  xpp_msgs::RobotStateCartesianTrajectory msg;
220 
221  for (const auto state : xpp) {
222  auto state_msg = ToRos(state);
223  msg.points.push_back(state_msg);
224  }
225 
226  return msg;
227 }
228 
229 static std::vector<RobotStateCartesian>
230 ToXpp(const xpp_msgs::RobotStateCartesianTrajectory& ros)
231 {
232  std::vector<RobotStateCartesian> xpp_vec;
233 
234  for (const auto ros_state : ros.points) {
235  auto xpp = ToXpp(ros_state);
236  xpp_vec.push_back(xpp);
237  }
238 
239  return xpp_vec;
240 }
241 
242 };
243 
244 } // namespace xpp
245 
246 #endif /* XPP_ROS_CONVERSIONS_H_ */
Quaterniond q
orientation expressed as Quaternion.
Definition: state.h:149
static geometry_msgs::Quaternion ToRos(const Eigen::Quaterniond xpp)
Definition: convert.h:137
std::vector< EndeffectorID > GetEEsOrdered() const
Definition: endeffectors.h:225
static xpp_msgs::StateLin3d ToRos(const StateLin3d &xpp)
Definition: convert.h:73
T & at(EndeffectorID ee)
Read/write access to the endeffector stored at index ee.
Definition: endeffectors.h:197
static T ToRos(const Vector3d &xpp)
Definition: convert.h:102
static std::vector< RobotStateCartesian > ToXpp(const xpp_msgs::RobotStateCartesianTrajectory &ros)
Definition: convert.h:230
Defines a complete robot state in Cartesian space.
VectorXd v_
Definition: state.h:60
static xpp_msgs::RobotStateCartesianTrajectory ToRos(const std::vector< RobotStateCartesian > &xpp)
Definition: convert.h:217
static xpp_msgs::RobotStateCartesian ToRos(const RobotStateCartesian &xpp)
Definition: convert.h:181
VectorXd a_
position, velocity and acceleration
Definition: state.h:60
Eigen::Vector3d Vector3d
Definition: state.h:48
StateAng3d ang
Quaternion, velocity and acceleration.
Definition: state.h:177
Eigen::Quaterniond Quaterniond
Definition: state.h:51
VectorXd p_
Definition: state.h:60
Vector3d w
angular velocity (omega).
Definition: state.h:150
6D-State (linear+angular) of an object in 3-dimensional space, where the angular part is expressed by...
Definition: state.h:174
StateLin3d lin
linear position, velocity and acceleration
Definition: state.h:176
static State3d ToXpp(const xpp_msgs::State6d &ros)
Definition: convert.h:165
Converts between xpp-states types and xpp-messages.
Definition: convert.h:51
static RobotStateCartesian ToXpp(const xpp_msgs::RobotStateCartesian &ros)
Definition: convert.h:198
static xpp_msgs::State6d ToRos(const State3d &xpp)
Definition: convert.h:149
Vector3d wd
angular acceleration (omega dot).
Definition: state.h:151
static Endeffectors< Vector3d > ToXpp(const std::vector< T > &ros)
Definition: convert.h:114
static Eigen::Quaterniond ToXpp(const geometry_msgs::Quaternion ros)
Definition: convert.h:125
Endeffectors< Vector3d > ee_forces_
EndeffectorsContact ee_contact_
static Vector3d ToXpp(const T &ros)
Definition: convert.h:93
static StateLin3d ToXpp(const xpp_msgs::StateLin3d &ros)
Definition: convert.h:54


xpp_states
Author(s): Alexander W. Winkler
autogenerated on Tue Mar 1 2022 00:07:15