visualization_moveit.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2018, Wolfgang Merkt
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 //
8 // * Redistributions of source code must retain the above copyright notice,
9 // this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright
11 // notice, this list of conditions and the following disclaimer in the
12 // documentation and/or other materials provided with the distribution.
13 // * Neither the name of nor the names of its contributors may be used to
14 // endorse or promote products derived from this software without specific
15 // prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 // POSSIBILITY OF SUCH DAMAGE.
28 //
29 
30 #include <exotica_core/server.h>
32 
33 #include <geometry_msgs/Transform.h>
34 #include <moveit_msgs/DisplayTrajectory.h>
35 
36 namespace exotica
37 {
39 {
40  if (scene->debug_) HIGHLIGHT_NAMED("VisualizationMoveIt", "Initialising visualizer");
41  Initialize();
42 }
44 
46 {
47  if (Server::IsRos())
48  {
49  trajectory_pub_ = Server::Advertise<moveit_msgs::DisplayTrajectory>(scene_->GetName() + (scene_->GetName().empty() ? "" : "/") + "Trajectory", 1, true);
50  }
51 }
52 
54 {
55  // TODO: Correctly handle dt for time_from_start
56  // TODO:
57  // http://docs.ros.org/melodic/api/moveit_msgs/html/msg/DisplayTrajectory.html
58 
59  if (!Server::IsRos()) return;
60 
61  if (trajectory.cols() != scene_->GetKinematicTree().GetNumControlledJoints())
62  ThrowPretty("Number of DoFs in trajectory does not match number of controlled joints of robot. Got " << trajectory.cols() << " but expected " << scene_->GetKinematicTree().GetNumControlledJoints());
63 
64  moveit_msgs::DisplayTrajectory traj_msg;
65  traj_msg.model_id = scene_->GetKinematicTree().GetRobotModel()->getName();
66 
67  const int num_trajectory_points = trajectory.rows();
68 
69  traj_msg.trajectory.resize(1);
70  traj_msg.trajectory[0].joint_trajectory.header.frame_id = scene_->GetRootFrameName();
71  traj_msg.trajectory[0].joint_trajectory.header.stamp = ros::Time::now();
72 
73  // Floating base support
74  int base_offset = 0;
75  switch (scene_->GetKinematicTree().GetControlledBaseType())
76  {
77  case BaseType::FIXED:
78  // HIGHLIGHT("Fixed base, no offset");
79  break;
80  case BaseType::PLANAR:
81  // HIGHLIGHT("Planar, offset 3");
82  base_offset = 3;
83  break;
84  case BaseType::FLOATING:
85  // HIGHLIGHT("Floating, offset 6");
86  base_offset = 6;
87  break;
88  default:
89  ThrowPretty("Unknown base type.");
90  }
91 
92  // Insert floating base if not a fixed-base controlled robot
93  if (scene_->GetKinematicTree().GetControlledBaseType() != BaseType::FIXED)
94  {
95  traj_msg.trajectory[0].multi_dof_joint_trajectory.header.frame_id = scene_->GetRootFrameName();
96  traj_msg.trajectory[0].multi_dof_joint_trajectory.joint_names.push_back(scene_->GetKinematicTree().GetRootJointName());
97  traj_msg.trajectory[0].multi_dof_joint_trajectory.points.resize(num_trajectory_points);
98 
99  geometry_msgs::Transform base_transform;
100  Eigen::Quaterniond quat;
101  for (int i = 0; i < num_trajectory_points; ++i)
102  {
103  base_transform.translation.x = trajectory(i, 0);
104  base_transform.translation.y = trajectory(i, 1);
105 
106  if (scene_->GetKinematicTree().GetControlledBaseType() == BaseType::FLOATING)
107  {
108  base_transform.translation.z = trajectory(i, 2);
109  quat = Eigen::AngleAxisd(trajectory(i, 3), Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(trajectory(i, 4), Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(trajectory(i, 5), Eigen::Vector3d::UnitZ());
110  }
111  else if (scene_->GetKinematicTree().GetControlledBaseType() == BaseType::PLANAR)
112  {
113  base_transform.translation.z = 0.0; // TODO: Technically: trans_z
114  quat = Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()) * Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(trajectory(i, 2), Eigen::Vector3d::UnitZ());
115  }
116 
117  base_transform.rotation.x = quat.x();
118  base_transform.rotation.y = quat.y();
119  base_transform.rotation.z = quat.z();
120  base_transform.rotation.w = quat.w();
121 
122  traj_msg.trajectory[0].multi_dof_joint_trajectory.points[i].transforms.push_back(base_transform);
123  }
124  }
125 
126  // Handle unactuated joints, i.e., joints whose values are not part of the trajectory:
127  // TODO: How to handle unactuated floating-base?
128  traj_msg.trajectory_start.joint_state.header.frame_id = scene_->GetRootFrameName();
129 
130  // Insert all starting joint states - including the floating base
131  auto model_state_map = scene_->GetKinematicTree().GetModelStateMap();
132  for (const auto& pair : model_state_map)
133  {
134  // Only push back if not part of the floating base - i.e., the joint name includes the root frame name.
135  if (pair.first.find(scene_->GetRootFrameName()) == std::string::npos)
136  {
137  traj_msg.trajectory_start.joint_state.name.push_back(pair.first);
138  traj_msg.trajectory_start.joint_state.position.push_back(pair.second);
139  }
140  }
141 
142  // Handle actuated joints - the joint names need to _exclude_ the base joints.
143  const int num_actuated_joints_without_base = trajectory.cols() - base_offset;
144  traj_msg.trajectory[0].joint_trajectory.points.resize(num_trajectory_points);
145  traj_msg.trajectory[0].joint_trajectory.joint_names.resize(num_actuated_joints_without_base);
146  for (int i = 0; i < num_actuated_joints_without_base; ++i)
147  traj_msg.trajectory[0].joint_trajectory.joint_names[i] = scene_->GetKinematicTree().GetControlledJointNames()[base_offset + i];
148 
149  // Insert actuated joints without base
150  for (int i = 0; i < num_trajectory_points; ++i)
151  {
152  traj_msg.trajectory[0].joint_trajectory.points[i].positions.resize(num_actuated_joints_without_base);
153  for (int n = 0; n < num_actuated_joints_without_base; ++n)
154  {
155  traj_msg.trajectory[0].joint_trajectory.points[i].positions[n] = trajectory(i, n + base_offset);
156  }
157  }
158 
159  trajectory_pub_.publish(traj_msg);
160 }
161 } // namespace exotica
visualization_moveit.h
exotica::VisualizationMoveIt::Initialize
void Initialize()
Definition: visualization_moveit.cpp:45
exotica::PLANAR
@ PLANAR
Definition: kinematic_tree.h:53
exotica::VisualizationMoveIt::DisplayTrajectory
void DisplayTrajectory(Eigen::MatrixXdRefConst trajectory)
Definition: visualization_moveit.cpp:53
generate_initializers.n
n
Definition: generate_initializers.py:637
exotica::VisualizationMoveIt::trajectory_pub_
ros::Publisher trajectory_pub_
Definition: visualization_moveit.h:52
exotica
Definition: collision_scene.h:46
exotica::FLOATING
@ FLOATING
Definition: kinematic_tree.h:52
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
HIGHLIGHT_NAMED
#define HIGHLIGHT_NAMED(name, x)
Definition: printable.h:62
exotica::ScenePtr
std::shared_ptr< Scene > ScenePtr
Definition: scene.h:246
Eigen::MatrixXdRefConst
const typedef Eigen::Ref< const Eigen::MatrixXd > & MatrixXdRefConst
Definition: conversions.h:54
exotica::VisualizationMoveIt::scene_
ScenePtr scene_
Definition: visualization_moveit.h:51
ThrowPretty
#define ThrowPretty(m)
Definition: exception.h:36
exotica::VisualizationMoveIt::VisualizationMoveIt
VisualizationMoveIt(ScenePtr scene)
Definition: visualization_moveit.cpp:38
exotica::VisualizationMoveIt::~VisualizationMoveIt
virtual ~VisualizationMoveIt()
exotica::FIXED
@ FIXED
Definition: kinematic_tree.h:51
server.h
exotica::Server::IsRos
static bool IsRos()
Definition: server.h:96
ros::Time::now
static Time now()


exotica_core
Author(s): Yiming Yang, Michael Camilleri
autogenerated on Fri Aug 2 2024 08:43:02