Skip to content

Commit d175c48

Browse files
author
omid
committed
When using motion planning display in rviz, the reference trajectory is not set. Check this and warn the user if ref traj is empty
trajectory_test not working yet
1 parent a484a1c commit d175c48

3 files changed

Lines changed: 162 additions & 73 deletions

File tree

‎moveit_planners/trajopt/trajopt_interface/src/trajopt_interface.cpp‎

Lines changed: 28 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -121,16 +121,35 @@ bool TrajOptInterface::solve(const planning_scene::PlanningSceneConstPtr& planni
121121
setProblemInfoParam(problem_info);
122122

123123
ROS_INFO(" ======================================= Populate init info");
124+
// Check if the reference trajectoriy is empty when init_info.type is not set to STATIONARY
125+
if (problem_info.init_info.type != trajopt::InitInfo::STATIONARY)
126+
{
127+
if (req.reference_trajectories.empty())
128+
{
129+
ROS_ERROR_STREAM_NAMED(name_, "reference_trajectoies is empty. If you are using Motion Planning Display in RViz, "
130+
"set init_info.type in the yaml file to STATIONARY");
131+
res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE;
132+
return false;
133+
}
134+
else if (req.reference_trajectories[0].joint_trajectory.empty())
135+
{
136+
ROS_ERROR_STREAM_NAMED(name_, "joint_trajectory is empty. If you are using Motion Planning Display in RViz, set "
137+
"init_info.type in the yaml file to STATIONARY");
138+
res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE;
139+
return false;
140+
}
141+
}
142+
124143
// For type JOINT_INTERPOLATED, we need the configuration of the robot at one state (which should be the goal), we do
125-
// not need the robot configuration along the whole trajectory. That is why we need one index which is 0 for
126-
// "points[]"
144+
// not need the robot configuration along the whole trajectory. That is why the last element of points, i.e.
145+
// poinst.back() is considered.
127146
if (problem_info.init_info.type == trajopt::InitInfo::JOINT_INTERPOLATED)
128147
{
129148
Eigen::VectorXd initial_joint_values_eigen(dof);
130149
for (int joint_index = 0; joint_index < dof; ++joint_index)
131150
{
132151
initial_joint_values_eigen(joint_index) =
133-
req.reference_trajectories[0].joint_trajectory[0].points[0].positions[joint_index];
152+
req.reference_trajectories[0].joint_trajectory[0].points.back().positions[joint_index];
134153
}
135154

136155
problem_info.init_info.data = initial_joint_values_eigen;
@@ -290,17 +309,17 @@ bool TrajOptInterface::solve(const planning_scene::PlanningSceneConstPtr& planni
290309
ROS_INFO(" ======================================= TrajOpt Solution");
291310
trajopt::TrajArray opt_solution = trajopt::getTraj(opt.x(), trajopt_problem_->GetVars());
292311

293-
// assume that the trajectory is now optimized, fill in the output structure:
312+
// Assume that the trajectory is now optimized, fill in the output structure:
294313
ROS_INFO_STREAM_NAMED("num_rows", opt_solution.rows());
295314
ROS_INFO_STREAM_NAMED("num_cols", opt_solution.cols());
296315

297316
res.trajectory.resize(1);
298317
res.trajectory[0].joint_trajectory.joint_names = group_joint_names;
299318
res.trajectory[0].joint_trajectory.header = req.start_state.joint_state.header;
300319

301-
// fill in the entire trajectory
320+
// Fill in the entire trajectory
302321
res.trajectory[0].joint_trajectory.points.resize(opt_solution.rows());
303-
for (int i = 0; i < opt_solution.rows(); i++)
322+
for (size_t i = 0; i < opt_solution.rows(); i++)
304323
{
305324
res.trajectory[0].joint_trajectory.points[i].positions.resize(opt_solution.cols());
306325
for (size_t j = 0; j < opt_solution.cols(); j++)
@@ -321,8 +340,9 @@ bool TrajOptInterface::solve(const planning_scene::PlanningSceneConstPtr& planni
321340

322341
for (int jn = 0; jn < res.trajectory[0].joint_trajectory.points.back().positions.size(); ++jn)
323342
{
324-
ROS_INFO_STREAM_NAMED("joint_value", res.trajectory[0].joint_trajectory.points.back().positions[jn]
325-
<< " " << req.goal_constraints.back().joint_constraints[jn].position);
343+
ROS_INFO_STREAM_NAMED("joint_value",
344+
"response: " << res.trajectory[0].joint_trajectory.points.back().positions[jn]
345+
<< " goal: " << req.goal_constraints.back().joint_constraints[jn].position);
326346
}
327347

328348
bool constraints_are_ok = true;

‎moveit_planners/trajopt/trajopt_interface/test/trajectory_test.cpp‎

Lines changed: 84 additions & 62 deletions
Original file line numberDiff line numberDiff line change
@@ -21,23 +21,28 @@
2121
// Testing
2222
#include <gtest/gtest.h>
2323

24-
#include <trajopt/common.hpp>
25-
2624
#include <moveit/planning_interface/planning_interface.h>
2725
#include <moveit/robot_model_loader/robot_model_loader.h>
2826
#include <moveit/planning_scene/planning_scene.h>
27+
#include <moveit/planning_pipeline/planning_pipeline.h>
28+
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
2929
#include <moveit/robot_state/robot_state.h>
3030
#include <moveit/utils/robot_model_test_utils.h>
3131

3232
#include <trajopt/problem_description.h>
3333
#include <trajopt/kinematic_terms.h>
34+
#include <trajopt/common.hpp>
3435

3536
#include <moveit/kinematic_constraints/utils.h>
3637
#include <moveit/move_group_interface/move_group_interface.h>
3738
#include <moveit/robot_model_loader/robot_model_loader.h>
3839
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
3940
#include <moveit_msgs/MotionPlanResponse.h>
4041

42+
#include <tf2/utils.h>
43+
#include <tf2_eigen/tf2_eigen.h>
44+
#include <tf2_ros/transform_listener.h>
45+
4146
class TrajectoryTest : public ::testing::Test
4247
{
4348
public:
@@ -90,40 +95,77 @@ TEST_F(TrajectoryTest, goalTolerance)
9095
{
9196
const std::string NODE_NAME = "trajectory_test";
9297

93-
// Create a RobotState and JointModelGroup to keep track of the current robot pose and planning group
94-
robot_state::RobotStatePtr current_state(new robot_state::RobotState(robot_model_));
95-
current_state->setToDefaultValues();
98+
// Create a planing scene monitor
99+
planning_scene_monitor::PlanningSceneMonitorPtr psm(
100+
new planning_scene_monitor::PlanningSceneMonitor("robot_description"));
101+
102+
psm->startSceneMonitor();
103+
psm->startWorldGeometryMonitor();
104+
psm->startStateMonitor();
96105

97-
const robot_state::JointModelGroup* joint_model_group = current_state->getJointModelGroup(PLANNING_GROUP);
98-
EXPECT_NE(joint_model_group, nullptr);
106+
// Create a RobotState to keep track of the current robot pose and planning group
107+
robot_state::RobotStatePtr robot_state(
108+
new robot_state::RobotState(planning_scene_monitor::LockedPlanningSceneRO(psm)->getCurrentState()));
109+
robot_state->setToDefaultValues();
110+
robot_state->update();
111+
112+
// Create JointModelGroup
113+
const robot_state::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP);
99114
const std::vector<std::string>& joint_names = joint_model_group->getActiveJointModelNames();
100-
EXPECT_EQ(joint_names.size(), 7);
115+
const std::vector<std::string>& link_model_names = joint_model_group->getLinkModelNames();
116+
ROS_INFO_NAMED(NODE_NAME, "end effector name %s\n", link_model_names.back().c_str());
101117

102118
planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model_));
103119

120+
// Set the planner
121+
std::string planner_plugin_name = "trajopt_interface/TrajOptPlanner";
122+
node_handle_.setParam("planning_plugin", planner_plugin_name);
123+
124+
// Create pipeline
125+
planning_pipeline::PlanningPipelinePtr planning_pipeline(
126+
new planning_pipeline::PlanningPipeline(robot_model_, node_handle_, "planning_plugin", "request_adapters"));
127+
128+
// Current state
129+
std::vector<double> current_joint_values = { 0, 0, 0, -1.5, 0, 0.6, 0.9 };
130+
robot_state->setJointGroupPositions(joint_model_group, current_joint_values);
131+
geometry_msgs::Pose pose_msg_current;
132+
const Eigen::Isometry3d& end_effector_transform_current =
133+
robot_state->getGlobalLinkTransform(link_model_names.back());
134+
pose_msg_current = tf2::toMsg(end_effector_transform_current);
135+
104136
// Create response and request
105137
planning_interface::MotionPlanRequest req;
106138
planning_interface::MotionPlanResponse res;
107139

140+
std::vector<std::string> keys;
141+
node_handle_.getParamNames(keys);
142+
for (std::string key : keys)
143+
{
144+
std::cerr << key << std::endl;
145+
}
146+
108147
// Set start state
109-
// ======================================================================================
148+
// ========================================================================================
110149
std::vector<double> start_joint_values = { 0.4, 0.3, 0.5, -0.55, 0.88, 1.0, -0.075 };
111-
robot_state::RobotStatePtr start_state(new robot_state::RobotState(robot_model_));
112-
start_state->setJointGroupPositions(joint_model_group, start_joint_values);
113-
start_state->update();
150+
robot_state->setJointGroupPositions(joint_model_group, start_joint_values);
151+
robot_state->update();
114152

115153
req.start_state.joint_state.name = joint_names;
116154
req.start_state.joint_state.position = start_joint_values;
117155
req.goal_constraints.clear();
118156
req.group_name = PLANNING_GROUP;
119157

120-
// Set the goal state and joints tolerance
158+
geometry_msgs::Pose pose_msg_start;
159+
const Eigen::Isometry3d& end_effector_transform_start = robot_state->getGlobalLinkTransform(link_model_names.back());
160+
pose_msg_start = tf2::toMsg(end_effector_transform_start);
161+
162+
// Set the goal state
121163
// ========================================================================================
122-
robot_state::RobotStatePtr goal_state(new robot_state::RobotState(robot_model_));
123164
std::vector<double> goal_joint_values = { 0.8, 0.7, 1, -1.3, 1.9, 2.2, -0.1 };
124-
goal_state->setJointGroupPositions(joint_model_group, goal_joint_values);
125-
goal_state->update();
126-
moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(*goal_state, joint_model_group);
165+
robot_state->setJointGroupPositions(joint_model_group, goal_joint_values);
166+
robot_state->update();
167+
moveit_msgs::Constraints joint_goal =
168+
kinematic_constraints::constructGoalConstraints(*robot_state, joint_model_group);
127169
req.goal_constraints.push_back(joint_goal);
128170
req.goal_constraints[0].name = "goal_pos";
129171
// Set joint tolerance
@@ -134,59 +176,39 @@ TEST_F(TrajectoryTest, goalTolerance)
134176
req.goal_constraints[0].joint_constraints[x].tolerance_below = 0.001;
135177
}
136178

137-
// Load planner
138-
// ======================================================================================
139-
// We will now construct a loader to load a planner, by name.
140-
// Note that we are using the ROS pluginlib library here.
141-
boost::scoped_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager>> planner_plugin_loader;
142-
planning_interface::PlannerManagerPtr planner_instance;
143-
144-
std::string planner_plugin_name = "trajopt_interface/TrajOptPlanner";
145-
node_handle_.setParam("planning_plugin", planner_plugin_name);
179+
geometry_msgs::Pose pose_msg_goal;
180+
const Eigen::Isometry3d& end_effector_transform_goal = robot_state->getGlobalLinkTransform(link_model_names.back());
181+
pose_msg_goal = tf2::toMsg(end_effector_transform_goal);
146182

147-
// Make sure the planner plugin is loaded
148-
EXPECT_TRUE(node_handle_.getParam("planning_plugin", planner_plugin_name));
149-
try
150-
{
151-
planner_plugin_loader.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>(
152-
"moveit_core", "planning_interface::PlannerManager"));
153-
}
154-
catch (pluginlib::PluginlibException& ex)
155-
{
156-
ROS_FATAL_STREAM_NAMED(NODE_NAME, "Exception while creating planning plugin loader " << ex.what());
157-
}
158-
try
183+
// Reference Trajectory. The type should be defined in the yaml file.
184+
// ========================================================================================
185+
// type: JOINT_INTERPOLATED
186+
req.reference_trajectories.resize(1);
187+
req.reference_trajectories[0].joint_trajectory.resize(1);
188+
req.reference_trajectories[0].joint_trajectory[0].joint_names = joint_names;
189+
req.reference_trajectories[0].joint_trajectory[0].points.resize(1);
190+
req.reference_trajectories[0].joint_trajectory[0].points[0].positions = goal_joint_values;
191+
192+
// Solve the problem
193+
// ========================================================================================
159194
{
160-
planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name));
161-
if (!planner_instance->initialize(robot_model_, node_handle_.getNamespace()))
162-
ROS_FATAL_STREAM_NAMED(NODE_NAME, "Could not initialize planner instance");
163-
ROS_INFO_STREAM_NAMED(NODE_NAME, "Using planning interface '" << planner_instance->getDescription() << "'");
195+
planning_scene_monitor::LockedPlanningSceneRO lscene(psm);
196+
// Now, call the pipeline and check whether planning was successful.
197+
planning_pipeline->generatePlan(lscene, req, res);
164198
}
165-
catch (pluginlib::PluginlibException& ex)
199+
200+
// Check that the planning was successful
201+
if (res.error_code_.val != res.error_code_.SUCCESS)
166202
{
167-
const std::vector<std::string>& classes = planner_plugin_loader->getDeclaredClasses();
168-
std::stringstream ss;
169-
for (std::size_t i = 0; i < classes.size(); ++i)
170-
ss << classes[i] << " ";
171-
ROS_ERROR_STREAM_NAMED(NODE_NAME, "Exception while loading planner '" << planner_plugin_name << "': " << ex.what()
172-
<< std::endl
173-
<< "Available plugins: " << ss.str());
203+
ROS_ERROR_STREAM_NAMED(NODE_NAME, "Could not compute plan successfully");
204+
return;
174205
}
175206

176-
// Creat planning context
177-
// ========================================================================================
178-
planning_interface::PlanningContextPtr context =
179-
planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
180-
181-
context->solve(res);
182-
EXPECT_EQ(res.error_code_.val, res.error_code_.SUCCESS);
183-
184-
moveit_msgs::MotionPlanResponse response;
185-
res.getMessage(response);
186-
187207
// Check the difference between the last step in the solution and the goal
188208
// ========================================================================================
189-
std::vector<double> joints_values_last_step = response.trajectory.joint_trajectory.points.back().positions;
209+
moveit_msgs::RobotTrajectory solution_trajectory;
210+
res.trajectory_->getRobotTrajectoryMsg(solution_trajectory);
211+
std::vector<double> joints_values_last_step = solution_trajectory.joint_trajectory.points.back().positions;
190212

191213
for (std::size_t joint_index = 0; joint_index < joints_values_last_step.size(); ++joint_index)
192214
{

‎moveit_planners/trajopt/trajopt_interface/test/trajectory_test.test‎

Lines changed: 50 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1,10 +1,57 @@
11
<?xml version="1.0" encoding="utf-8"?>
22
<launch>
33

4-
<test pkg="moveit_planners_trajopt" type="trajectory_test" test-name="trajectory_test" time-limit="300" args="">
4+
<rosparam ns="trajectory_test" command="load" file="$(find panda_moveit_config)/config/trajopt_planning.yaml"/>
55

6-
<rosparam command="load" file="$(find panda_moveit_config)/config/trajopt_planning.yaml"/>
6+
<!-- By default, we do not start a database (it can be large) -->
7+
<arg name="db" default="false" />
8+
<!-- Allow user to specify database location -->
9+
<arg name="db_path" default="$(find panda_moveit_config)/default_warehouse_mongo_db" />
710

8-
</test>
11+
<!-- By default, we are not in debug mode -->
12+
<arg name="debug" default="false" />
13+
<arg name="pipeline" default="trajopt" />
14+
15+
<!--
16+
By default, hide joint_state_publisher's GUI
17+
18+
MoveIt!'s "demo" mode replaces the real robot driver with the joint_state_publisher.
19+
The latter one maintains and publishes the current joint configuration of the simulated robot.
20+
It also provides a GUI to move the simulated robot around "manually".
21+
This corresponds to moving around the real robot without the use of MoveIt.
22+
-->
23+
<arg name="rviz_tutorial" default="false" />
24+
<arg name="use_gui" default="false" />
25+
26+
<!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
27+
<include file="$(find panda_moveit_config)/launch/planning_context.launch">
28+
<arg name="load_robot_description" value="true"/>
29+
</include>
30+
31+
<!-- If needed, broadcast static tf for robot root -->
32+
<node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_1" args="0 0 0 0 0 0 world panda_link0" />
33+
34+
<!-- We do not have a robot connected, so publish fake joint states -->
35+
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
36+
<param name="/use_gui" value="$(arg use_gui)"/>
37+
<rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam>
38+
</node>
39+
<node name="joint_state_desired_publisher" pkg="topic_tools" type="relay" args="joint_states joint_states_desired" />
40+
41+
<!-- Given the published joint states, publish tf for the robot links -->
42+
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
43+
44+
<!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
45+
<!--include file="$(find panda_moveit_config)/launch/move_group.launch">
46+
<arg name="allow_trajectory_execution" value="true"/>
47+
<arg name="fake_execution" value="true"/>
48+
<arg name="info" value="true"/>
49+
<arg name="debug" value="$(arg debug)"/>
50+
<arg name="pipeline" value="$(arg pipeline)" />
51+
</include-->
52+
53+
54+
<!-- Unit Test /-->
55+
<test pkg="moveit_planners_trajopt" type="trajectory_test" test-name="trajectory_test" time-limit="300" args="" />
956

1057
</launch>

0 commit comments

Comments
 (0)