-
Notifications
You must be signed in to change notification settings - Fork 30
/
Conversions.hpp
35 lines (30 loc) · 1.33 KB
/
Conversions.hpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
#ifndef AIKIDO_CONTROL_ROS_CONVERSIONS_HPP_
#define AIKIDO_CONTROL_ROS_CONVERSIONS_HPP_
#include <memory>
#include <trajectory_msgs/JointTrajectory.h>
#include <aikido/statespace/dart/MetaSkeletonStateSpace.hpp>
#include <aikido/trajectory/Spline.hpp>
#include <map>
namespace aikido {
namespace control {
namespace ros {
/// Converts a ROS JointTrajectory into an aikido's Spline trajectory.
/// This method only handles single-DOF joints.
/// \param[in] space MetaSkeletonStateSpace for Spline trajectory.
// Subspaces must be either 1D RnJoint or SO2Joint.
/// \param[in] jointTrajectory ROS JointTrajectory to be converted.
/// \return Spline trajectory.
std::unique_ptr<aikido::trajectory::Spline> toSplineJointTrajectory(
const std::shared_ptr<
aikido::statespace::dart::MetaSkeletonStateSpace>& space,
const trajectory_msgs::JointTrajectory& jointTrajectory);
/// Converts Aikido Trajectory to ROS JointTrajectory.
/// Supports only 1D RnJoints and SO2Joints.
/// \param[in] trajectory Aikido trajectory to be converted.
/// \param[in] timestep Timestep between two consecutive waypoints.
trajectory_msgs::JointTrajectory toRosJointTrajectory(
const aikido::trajectory::TrajectoryPtr& trajectory, double timestep);
} // namespace ros
} // namespace control
} // namespace aikido
#endif // ifndef AIKIDO_CONTROL_ROS_CONVERSIONS_HPP_