Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[12010] pkg/trunk
|
From: <is...@us...> - 2009-03-03 00:38:08
|
Revision: 12010
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=12010&view=rev
Author: isucan
Date: 2009-03-03 00:38:05 +0000 (Tue, 03 Mar 2009)
Log Message:
-----------
renaming plan_kinematic_path to plan_ompl_path
Modified Paths:
--------------
pkg/trunk/demos/tabletop_manipulation/manifest.xml
Added Paths:
-----------
pkg/trunk/motion_planning/plan_ompl_path/
pkg/trunk/motion_planning/plan_ompl_path/CMakeLists.txt
pkg/trunk/motion_planning/plan_ompl_path/Makefile
pkg/trunk/motion_planning/plan_ompl_path/manifest.xml
pkg/trunk/motion_planning/plan_ompl_path/src/
pkg/trunk/motion_planning/plan_ompl_path/src/base/
pkg/trunk/motion_planning/plan_ompl_path/src/base/execute_plan_to_state_minimal.cpp
pkg/trunk/motion_planning/plan_ompl_path/src/plan_kinematic_path.cpp
pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/
pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/call_plan_to_state.cpp
pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_plan_to_position_minimal.cpp
pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_plan_to_state.cpp
pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_plan_to_state_minimal.cpp
pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp
pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/test_path_execution.cpp
Removed Paths:
-------------
pkg/trunk/motion_planning/plan_kinematic_path/
Modified: pkg/trunk/demos/tabletop_manipulation/manifest.xml
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/manifest.xml 2009-03-03 00:37:43 UTC (rev 12009)
+++ pkg/trunk/demos/tabletop_manipulation/manifest.xml 2009-03-03 00:38:05 UTC (rev 12010)
@@ -12,7 +12,7 @@
<depend package="collision_map"/>
<depend package="ompl_planning"/>
<depend package="executive_python"/>
- <depend package="plan_kinematic_path"/>
+ <depend package="plan_ompl_path"/>
<depend package="pr2_msgs"/>
<depend package="rosviz"/>
<depend package="tf"/>
Added: pkg/trunk/motion_planning/plan_ompl_path/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/plan_ompl_path/CMakeLists.txt (rev 0)
+++ pkg/trunk/motion_planning/plan_ompl_path/CMakeLists.txt 2009-03-03 00:38:05 UTC (rev 12010)
@@ -0,0 +1,14 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+rospack(plan_ompl_path)
+
+rospack_add_executable(plan_kinematic_path src/plan_kinematic_path.cpp)
+
+rospack_add_executable(rarm_execute_plan_to_state src/right_arm/execute_plan_to_state.cpp)
+rospack_add_executable(rarm_execute_plan_to_state_minimal src/right_arm/execute_plan_to_state_minimal.cpp)
+rospack_add_executable(rarm_execute_plan_to_position_minimal src/right_arm/execute_plan_to_position_minimal.cpp)
+rospack_add_executable(rarm_execute_replan_to_state src/right_arm/execute_replan_to_state.cpp)
+rospack_add_executable(rarm_test_path_execution src/right_arm/test_path_execution.cpp)
+rospack_add_executable(rarm_call_plan_to_state src/right_arm/call_plan_to_state.cpp)
+
+rospack_add_executable(base_execute_plan_to_state_minimal src/base/execute_plan_to_state_minimal.cpp)
Added: pkg/trunk/motion_planning/plan_ompl_path/Makefile
===================================================================
--- pkg/trunk/motion_planning/plan_ompl_path/Makefile (rev 0)
+++ pkg/trunk/motion_planning/plan_ompl_path/Makefile 2009-03-03 00:38:05 UTC (rev 12010)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Added: pkg/trunk/motion_planning/plan_ompl_path/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/plan_ompl_path/manifest.xml (rev 0)
+++ pkg/trunk/motion_planning/plan_ompl_path/manifest.xml 2009-03-03 00:38:05 UTC (rev 12010)
@@ -0,0 +1,24 @@
+<package>
+ <description>Test package that can plan a kinematic path. Various examples on how to use the available sampling-based planners in conjunction with controllers</description>
+ <author>Ioan A. Sucan</author>
+ <license>BSD</license>
+ <review status="unreviewed" notes=""/>
+ <depend package="roscpp" />
+ <depend package="rosconsole" />
+
+ <depend package="std_msgs" />
+ <depend package="std_srvs" />
+
+ <depend package="robot_msgs" />
+ <depend package="robot_srvs" />
+
+ <depend package="pr2_msgs" />
+ <depend package="pr2_srvs" />
+
+ <depend package="planning_models" />
+ <depend package="ompl_planning" />
+
+ <depend package="robot_mechanism_controllers" />
+ <depend package="pr2_mechanism_controllers" />
+
+</package>
Added: pkg/trunk/motion_planning/plan_ompl_path/src/base/execute_plan_to_state_minimal.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_ompl_path/src/base/execute_plan_to_state_minimal.cpp (rev 0)
+++ pkg/trunk/motion_planning/plan_ompl_path/src/base/execute_plan_to_state_minimal.cpp 2009-03-03 00:38:05 UTC (rev 12010)
@@ -0,0 +1,136 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+
+/** \author Ioan Sucan */
+
+/** This is a simple program that shows how to plan a motion to a state.
+ The minimum number of things to get the base to move is done.
+
+ This example is incomplete since I do not know how to interact
+ with the base controller, and the kinematic_planning node still
+ runs in the robot frame. We need to switch to the map frame at
+ some point, if we want to do planning for the base.
+*/
+
+#include <kinematic_planning/KinematicStateMonitor.h>
+
+// service for planning to a state
+#include <robot_srvs/KinematicPlanState.h>
+
+
+static const std::string GROUPNAME = "pr2::base";
+
+class Example : public kinematic_planning::KinematicStateMonitor
+{
+public:
+
+ Example(ros::Node *node) : kinematic_planning::KinematicStateMonitor(node)
+ {
+ }
+
+ void runExample(void)
+ {
+ // construct the request for the motion planner
+ robot_msgs::KinematicPlanStateRequest req;
+
+ req.params.model_id = GROUPNAME;
+ req.params.distance_metric = "L2Square";
+ req.params.planner_id = "SBL";
+ req.threshold = 0.1;
+ req.interpolate = 1;
+ req.times = 1;
+
+ // skip setting the start state
+
+ // 3 DOF for the base; pick a goal state (joint angles)
+ req.goal_state.set_vals_size(3);
+ for (unsigned int i = 0 ; i < req.goal_state.get_vals_size(); ++i)
+ req.goal_state.vals[i] = m_basePos[i];
+
+ // move by one meter on X or Y
+ req.goal_state.vals[0] = 1;
+
+ // set the 2D space in which the base is allowed to move
+ req.params.volumeMin.x = -5.0 + m_basePos[0];
+ req.params.volumeMin.y = -5.0 + m_basePos[1];
+ req.params.volumeMin.z = 0.0;
+
+ req.params.volumeMax.x = 5.0 + m_basePos[0];
+ req.params.volumeMax.y = 5.0 + m_basePos[1];
+ req.params.volumeMax.z = 0.0;
+
+ // allow 1 second computation time
+ req.allowed_time = 1.0;
+
+ // define the service messages
+ robot_srvs::KinematicPlanState::Request s_req;
+ robot_srvs::KinematicPlanState::Response s_res;
+ s_req.value = req;
+
+ if (ros::service::call("plan_kinematic_path_state", s_req, s_res))
+ {
+ // send command to the base
+ // no idea how to interface with that yet ...
+ }
+ else
+ ROS_ERROR("Service 'plan_kinematic_path_state' failed");
+ }
+
+ void run(void)
+ {
+ loadRobotDescription();
+ if (loadedRobot())
+ {
+ sleep(1);
+ runExample();
+ }
+ sleep(1);
+ }
+
+private:
+
+};
+
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv);
+
+ ros::Node node("example_execute_plan_to_state_minimal");
+ Example plan(&node);
+ plan.run();
+
+ return 0;
+}
Added: pkg/trunk/motion_planning/plan_ompl_path/src/plan_kinematic_path.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_ompl_path/src/plan_kinematic_path.cpp (rev 0)
+++ pkg/trunk/motion_planning/plan_ompl_path/src/plan_kinematic_path.cpp 2009-03-03 00:38:05 UTC (rev 12010)
@@ -0,0 +1,324 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+
+/** \author Ioan Sucan */
+
+/** This is a simple program that shows how to (re)plan a motion to a
+ state or a link position. The minimum number of things to get the
+ arm to move (and replan) is done.
+ */
+
+#include <kinematic_planning/KinematicStateMonitor.h>
+
+// service for (re)planning to a state
+#include <robot_srvs/KinematicReplanState.h>
+
+// service for (re)planning to a link position
+#include <robot_srvs/KinematicReplanLinkPosition.h>
+
+// message for receiving the planning status
+#include <robot_msgs/KinematicPlanStatus.h>
+
+// message sent to visualizer to display the path
+#include <robot_msgs/DisplayKinematicPath.h>
+
+// messages to interact with the joint trajectory controller
+#include <robot_msgs/JointTraj.h>
+
+// message to interact with the cartesian trajectory controller
+#include <robot_msgs/PoseStamped.h>
+
+static const std::string GROUPNAME = "pr2::right_arm";
+
+class PlanKinematicPath : public kinematic_planning::KinematicStateMonitor
+{
+public:
+
+ PlanKinematicPath(ros::Node *node) : kinematic_planning::KinematicStateMonitor(node)
+ {
+ plan_id_ = -1;
+ robot_stopped_ = true;
+
+ // we use the topic for sending commands to the controller, so we need to advertise it
+ m_node->advertise<robot_msgs::JointTraj>("right_arm_trajectory_command", 1);
+ m_node->advertise<robot_msgs::PoseStamped>("cartesian_trajectory/command", 1);
+
+ // advertise the topic for displaying kinematic plans
+ m_node->advertise<robot_msgs::DisplayKinematicPath>("display_kinematic_path", 10);
+
+ m_node->subscribe("kinematic_planning_status", plan_status_, &PlanKinematicPath::receiveStatus, this, 1);
+ }
+
+ void runRightArmToPositionA(void)
+ {
+ // construct the request for the motion planner
+ robot_msgs::KinematicPlanStateRequest req;
+
+ req.params.model_id = GROUPNAME;
+ req.params.distance_metric = "L2Square";
+ req.params.planner_id = "KPIECE";
+ req.threshold = 0.1;
+ req.interpolate = 1;
+ req.times = 1;
+
+ // skip setting the start state
+
+ // 7 DOF for the arm; pick a goal state (joint angles)
+ req.goal_state.set_vals_size(7);
+ for (unsigned int i = 0 ; i < req.goal_state.get_vals_size(); ++i)
+ req.goal_state.vals[i] = 0.0;
+ req.goal_state.vals[0] = -1.5;
+ req.goal_state.vals[1] = -0.2;
+
+ // allow 1 second computation time
+ req.allowed_time = 1.0;
+
+ // define the service messages
+ robot_srvs::KinematicReplanState::Request s_req;
+ robot_srvs::KinematicReplanState::Response s_res;
+ s_req.value = req;
+
+ if (ros::service::call("replan_kinematic_path_state", s_req, s_res))
+ plan_id_ = s_res.id;
+ else
+ ROS_ERROR("Service 'replan_kinematic_path_state' failed");
+ }
+
+ void runRightArmToPositionB(void)
+ {
+ // construct the request for the motion planner
+ robot_msgs::KinematicPlanLinkPositionRequest req;
+
+ req.params.model_id = GROUPNAME;
+ req.params.distance_metric = "L2Square";
+ req.params.planner_id = "IKKPIECE";
+ req.interpolate = 1;
+ req.times = 1;
+
+ // skip setting the start state
+
+ // set the goal constraints
+ req.set_goal_constraints_size(1);
+ req.goal_constraints[0].type = robot_msgs::PoseConstraint::POSITION_XYZ + robot_msgs::PoseConstraint::ORIENTATION_RPY;
+ req.goal_constraints[0].robot_link = "r_gripper_r_finger_tip_link";
+ req.goal_constraints[0].x = 0.75025;
+ req.goal_constraints[0].y = -0.188;
+ req.goal_constraints[0].z = 0.829675;
+
+ req.goal_constraints[0].roll = 0.0;
+ req.goal_constraints[0].pitch = 0.0;
+ req.goal_constraints[0].yaw = 0.0;
+
+ req.goal_constraints[0].position_distance = 0.0001;
+ req.goal_constraints[0].orientation_distance = 0.1;
+ req.goal_constraints[0].orientation_importance = 0.0001;
+
+ // allow 1 second computation time
+ req.allowed_time = 0.5;
+
+ // define the service messages
+ robot_srvs::KinematicReplanLinkPosition::Request s_req;
+ robot_srvs::KinematicReplanLinkPosition::Response s_res;
+ s_req.value = req;
+
+ if (ros::service::call("replan_kinematic_path_position", s_req, s_res))
+ plan_id_ = s_res.id;
+ else
+ ROS_ERROR("Service 'replan_kinematic_path_position' failed");
+ }
+
+ void runRightArmToCoordinates(void)
+ {
+ robot_msgs::PoseStamped ps;
+ ps.header.frame_id = "base_link"; // make sure this is true; this should be take from input header
+ ps.header.stamp = ros::Time::now(); // again, should be taken from input header
+ ps.pose.position.x = 0.8;
+ ps.pose.position.y = -0.188;
+ ps.pose.position.z = 0.829675;
+ ps.pose.orientation.x = 0;
+ ps.pose.orientation.y = 0;
+ ps.pose.orientation.z = 0;
+ ps.pose.orientation.w = 1;
+ m_node->publish("cartesian_trajectory/command", ps);
+ }
+
+ void run(void)
+ {
+ loadRobotDescription();
+ if (loadedRobot())
+ {
+ ros::Duration d(1.0);
+ d.sleep();
+
+ while (m_node->ok())
+ {
+ ros::Duration d(10.0);
+
+ runRightArmToPositionA();
+
+ d.sleep();
+
+ runRightArmToPositionB();
+
+ d.sleep();
+ }
+
+ /*
+ sleep(30);
+
+ plan->runRightArmToCoordinates();
+ */
+
+ m_node->spin();
+ }
+ }
+
+protected:
+
+ // handle new status message
+ void receiveStatus(void)
+ {
+ if (plan_id_ >= 0 && plan_status_.id == plan_id_)
+ {
+ if (plan_status_.valid && !plan_status_.unsafe)
+ {
+ if (!plan_status_.path.states.empty())
+ {
+ robot_stopped_ = false;
+ sendArmCommand(plan_status_.path, GROUPNAME);
+ }
+ }
+ else
+ stopRobot();
+ if (plan_status_.done)
+ {
+ plan_id_ = -1;
+ robot_stopped_ = true;
+ ROS_INFO("Execution is complete");
+ }
+ }
+ }
+
+ void stopRobot(void)
+ {
+ if (robot_stopped_)
+ return;
+ robot_stopped_ = true;
+
+ // get the current params for the robot's right arm
+ double cmd[7];
+ m_robotState->copyParamsGroup(cmd, GROUPNAME);
+
+ robot_msgs::KinematicPath stop_path;
+ stop_path.set_states_size(1);
+ stop_path.states[0].set_vals_size(7);
+ for (unsigned int i = 0 ; i < 7 ; ++i)
+ stop_path.states[0].vals[i] = cmd[i];
+
+ sendArmCommand(stop_path, GROUPNAME);
+ }
+
+ // get the current state from the StateParams instance monitored by the KinematicStateMonitor
+ void currentState(robot_msgs::KinematicState &state)
+ {
+ state.set_vals_size(m_kmodel->getModelInfo().stateDimension);
+ for (unsigned int i = 0 ; i < state.get_vals_size() ; ++i)
+ state.vals[i] = m_robotState->getParams()[i];
+ }
+
+ // convert a kinematic path message to a trajectory for the controller
+ void getTrajectoryMsg(robot_msgs::KinematicPath &path, robot_msgs::JointTraj &traj)
+ {
+ traj.set_points_size(path.get_states_size());
+ for (unsigned int i = 0 ; i < path.get_states_size() ; ++i)
+ {
+ traj.points[i].set_positions_size(path.states[i].get_vals_size());
+ for (unsigned int j = 0 ; j < path.states[i].get_vals_size() ; ++j)
+ traj.points[i].positions[j] = path.states[i].vals[j];
+ traj.points[i].time = 0.0;
+ }
+ }
+
+ // send a command to the trajectory controller using a topic
+ void sendArmCommand(robot_msgs::KinematicPath &path, const std::string &model)
+ {
+ sendDisplay(path, model);
+ printPath(path);
+ robot_msgs::JointTraj traj;
+ getTrajectoryMsg(path, traj);
+ m_node->publish("right_arm_trajectory_command", traj);
+ ROS_INFO("Sent trajectory to controller");
+ }
+
+ void sendDisplay(robot_msgs::KinematicPath &path, const std::string &model)
+ {
+ robot_msgs::DisplayKinematicPath dpath;
+ dpath.frame_id = "base_link";
+ dpath.model_name = model;
+ currentState(dpath.start_state);
+ dpath.path = path;
+ m_node->publish("display_kinematic_path", dpath);
+ ROS_INFO("Sent planned path to display");
+ }
+
+ void printPath(robot_msgs::KinematicPath &path)
+ {
+ printf("Path with %d states\n", (int)path.states.size());
+ for (unsigned int i = 0 ; i < path.states.size() ; ++i)
+ {
+ for (unsigned int j = 0 ; j < path.states[i].vals.size() ; ++j)
+ printf("%f ", path.states[i].vals[j]);
+ printf("\n");
+ }
+ printf("\n");
+ }
+
+ robot_msgs::KinematicPlanStatus plan_status_;
+ int plan_id_;
+ bool robot_stopped_;
+
+};
+
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv);
+ ros::Node *node = new ros::Node("plan_kinematic_path");
+
+ PlanKinematicPath plan(node);
+ plan.run();
+
+ return 0;
+}
Added: pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/call_plan_to_state.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/call_plan_to_state.cpp (rev 0)
+++ pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/call_plan_to_state.cpp 2009-03-03 00:38:05 UTC (rev 12010)
@@ -0,0 +1,95 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+
+/** \author Ioan Sucan */
+
+/** This is a simple program that shows how to plan a motion to a
+ state. The minimum number of things to get the arm to move is
+ done. This code only interfaces with a highlevel controller, so
+ getting the motion executed is no longer our responsibility.
+ */
+
+#include <kinematic_planning/KinematicStateMonitor.h>
+
+// interface to a high level controller
+#include <pr2_msgs/MoveArmGoal.h>
+
+class Example : public kinematic_planning::KinematicStateMonitor
+{
+public:
+
+ Example(ros::Node *node) : kinematic_planning::KinematicStateMonitor(node)
+ {
+ // we use the topic for sending commands to the controller, so we need to advertise it
+ m_node->advertise<pr2_msgs::MoveArmGoal>("right_arm_goal", 1);
+ }
+
+ void runExample(void)
+ {
+ // construct the request for the highlevel controller
+ pr2_msgs::MoveArmGoal ag;
+ ag.enable = 1;
+ ag.timeout = 10.0;
+
+ ag.set_goal_configuration_size(1);
+ ag.goal_configuration[0].name = "r_shoulder_pan_joint";
+ ag.goal_configuration[0].position = -0.5;
+ m_node->publish("right_arm_goal", ag);
+ }
+
+ void run(void)
+ {
+ loadRobotDescription();
+ if (loadedRobot())
+ {
+ sleep(1);
+ runExample();
+ }
+ sleep(1);
+ }
+
+};
+
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv);
+
+ ros::Node node("example_call_plan_to_state");
+ Example plan(&node);
+ plan.run();
+
+ return 0;
+}
Added: pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_plan_to_position_minimal.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_plan_to_position_minimal.cpp (rev 0)
+++ pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_plan_to_position_minimal.cpp 2009-03-03 00:38:05 UTC (rev 12010)
@@ -0,0 +1,158 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+
+/** \author Ioan Sucan */
+
+/** This is a simple program that shows how to plan a motion to a link position
+ The minimum number of things to get the arm to move is done.
+ */
+
+#include <kinematic_planning/KinematicStateMonitor.h>
+
+// service for planning to a link position
+#include <robot_srvs/KinematicPlanLinkPosition.h>
+
+// messages to interact with the trajectory controller
+#include <robot_msgs/JointTraj.h>
+
+static const std::string GROUPNAME = "pr2::right_arm";
+
+class Example : public kinematic_planning::KinematicStateMonitor
+{
+public:
+
+ Example(ros::Node *node) : kinematic_planning::KinematicStateMonitor(node)
+ {
+ // we use the topic for sending commands to the controller, so we need to advertise it
+ m_node->advertise<robot_msgs::JointTraj>("right_arm_trajectory_command", 1);
+ }
+
+ void runExample(void)
+ {
+ // construct the request for the motion planner
+ robot_msgs::KinematicPlanLinkPositionRequest req;
+
+ req.params.model_id = GROUPNAME;
+ req.params.distance_metric = "L2Square";
+ req.params.planner_id = "IKSBL";
+ req.interpolate = 1;
+ req.times = 1;
+
+ // skip setting the start state
+
+ // allow 1 second computation time
+ req.allowed_time = 1.0;
+
+
+ // place some constraints on the goal
+ req.set_goal_constraints_size(1);
+ // see the constraints message definition
+ req.goal_constraints[0].type = robot_msgs::PoseConstraint::POSITION_XYZ + robot_msgs::PoseConstraint::ORIENTATION_RY;
+ req.goal_constraints[0].robot_link = "r_gripper_palm_link";
+ req.goal_constraints[0].x = 0.75025;
+ req.goal_constraints[0].y = -0.188;
+ req.goal_constraints[0].z = 0.829675;
+
+ req.goal_constraints[0].roll = 0.0;
+ req.goal_constraints[0].yaw = 0.0;
+
+ // the threshold for solution is position_distance + orientation_distance * orientation_importance
+ // but the distance requirements must be satisfied by
+ req.goal_constraints[0].position_distance = 0.005; // in L2square norm
+ req.goal_constraints[0].orientation_distance = 0.05; // difference in radians between the quaternions
+ req.goal_constraints[0].orientation_importance = 0.005; // factor of importance of orientation relative to importance of position
+
+ // define the service messages
+ robot_srvs::KinematicPlanLinkPosition::Request s_req;
+ robot_srvs::KinematicPlanLinkPosition::Response s_res;
+ s_req.value = req;
+
+ if (ros::service::call("plan_kinematic_path_position", s_req, s_res))
+ sendArmCommand(s_res.value.path, GROUPNAME);
+ else
+ ROS_ERROR("Service 'plan_kinematic_path_state' failed");
+ }
+
+ void run(void)
+ {
+ loadRobotDescription();
+ if (loadedRobot())
+ {
+ sleep(1);
+ runExample();
+ }
+ sleep(1);
+ }
+
+protected:
+
+ // convert a kinematic path message to a trajectory for the controller
+ void getTrajectoryMsg(robot_msgs::KinematicPath &path, robot_msgs::JointTraj &traj)
+ {
+ traj.set_points_size(path.get_states_size());
+ for (unsigned int i = 0 ; i < path.get_states_size() ; ++i)
+ {
+ traj.points[i].set_positions_size(path.states[i].get_vals_size());
+ for (unsigned int j = 0 ; j < path.states[i].get_vals_size() ; ++j)
+ traj.points[i].positions[j] = path.states[i].vals[j];
+ traj.points[i].time = 0.0;
+ }
+ }
+
+ // send a command to the trajectory controller using a topic
+ void sendArmCommand(robot_msgs::KinematicPath &path, const std::string &model)
+ {
+ robot_msgs::JointTraj traj;
+ getTrajectoryMsg(path, traj);
+ m_node->publish("right_arm_trajectory_command", traj);
+ ROS_INFO("Sent trajectory to controller (using a topic)");
+ }
+
+private:
+
+
+};
+
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv);
+
+ ros::Node node("example_execute_plan_to_position_minimal");
+ Example plan(&node);
+ plan.run();
+
+ return 0;
+}
Added: pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_plan_to_state.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_plan_to_state.cpp (rev 0)
+++ pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_plan_to_state.cpp 2009-03-03 00:38:05 UTC (rev 12010)
@@ -0,0 +1,294 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+
+/** \author Ioan Sucan */
+
+/** This is a simple program that shows how to plan a motion to a state.
+ More than what is necessary is presented:
+ - an example of initializing the start state of the trajectory (this is not
+ mandatory; the planner will use the current state if one is not given)
+ - an example of how to impose constraints on the path is also given
+ - example using the trajectory controller: sending a trajectory on a topic
+ or using a service (only one is needed) if using the service, waiting for
+ trajectory completion is also possible
+ - sending the received path to be displayed is also possible
+ - checking whether a straight line path would have been valid
+ (using the motion_validator node)
+ - printing the path to the screen
+ */
+
+#include <kinematic_planning/KinematicStateMonitor.h>
+
+// service for planning to a state
+#include <robot_srvs/KinematicPlanState.h>
+
+// service for validating a straight line path
+#include <robot_srvs/ValidateKinematicPath.h>
+
+// message sent to visualizer to display the path
+#include <robot_msgs/DisplayKinematicPath.h>
+
+// messages and services to interact with the trajectory controller
+#include <robot_msgs/JointTraj.h>
+#include <pr2_mechanism_controllers/TrajectoryStart.h>
+#include <pr2_mechanism_controllers/TrajectoryQuery.h>
+
+static const std::string GROUPNAME = "pr2::right_arm";
+
+class Example : public kinematic_planning::KinematicStateMonitor
+{
+public:
+
+ Example(ros::Node *node) : kinematic_planning::KinematicStateMonitor(node)
+ {
+ // if we use the topic for sending commands to the controller, we need to advertise it
+ m_node->advertise<robot_msgs::JointTraj>("right_arm_trajectory_command", 1);
+
+ // advertise the topic for displaying kinematic plans
+ m_node->advertise<robot_msgs::DisplayKinematicPath>("display_kinematic_path", 10);
+
+ // we can send commands to the trajectory controller both by using a topic, and by using services
+ use_topic_ = false;
+ }
+
+ void runExample(void)
+ {
+ // construct the request for the motion planner
+ robot_msgs::KinematicPlanStateRequest req;
+
+ req.params.model_id = GROUPNAME;
+ req.params.distance_metric = "L2Square";
+ req.params.planner_id = "SBL";
+ req.threshold = 0.1;
+ req.interpolate = 1;
+ req.times = 1;
+
+ // we retrieve the current state; if we do not do this, the
+ // planner will do it for us so, we do not in fact need this
+ // line; we would set the starting state if we do not want to
+ // plan from the current state
+ currentState(req.start_state);
+
+ // 7 DOF for the arm; pick a goal state (joint angles)
+ req.goal_state.set_vals_size(7);
+ for (unsigned int i = 0 ; i < req.goal_state.get_vals_size(); ++i)
+ req.goal_state.vals[i] = 0.0;
+ req.goal_state.vals[0] = -1.5;
+ req.goal_state.vals[1] = -0.2;
+
+
+ // add some constraints for the solution path
+ // these constraints must be met throughout the execution of the path
+
+ // this constraing only makes sure the link is within a
+ // defined distance from a specific point at all times; values
+ // are set large so that we always find a solution
+ // all these values have to be in the base_link frame
+ req.constraints.set_pose_size(1);
+ req.constraints.pose[0].type = robot_msgs::PoseConstraint::POSITION_XYZ;
+ req.constraints.pose[0].robot_link = "r_gripper_palm_link";
+ req.constraints.pose[0].x = 1;
+ req.constraints.pose[0].y = 1;
+ req.constraints.pose[0].z = 1;
+ req.constraints.pose[0].position_distance = 10.0; // L2Square
+
+ // allow 1 second computation time
+ req.allowed_time = 1.0;
+
+ // define the service messages
+ robot_srvs::KinematicPlanState::Request s_req;
+ robot_srvs::KinematicPlanState::Response s_res;
+ s_req.value = req;
+
+ if (ros::service::call("plan_kinematic_path_state", s_req, s_res))
+ {
+ // print the path on screen
+ printPath(s_res.value.path);
+
+ // send the path to the visualizer
+ sendDisplay(req.start_state, s_res.value.path, GROUPNAME);
+
+ // check if the straight line path would have been valid
+ verifyDirectPath(req.start_state, req.constraints, req.goal_state, GROUPNAME);
+
+ // send the path to the controller
+ if (use_topic_)
+ sendArmCommand(s_res.value.path, GROUPNAME);
+ else
+ sendArmCommandAndWait(s_res.value.path, GROUPNAME);
+ }
+ else
+ ROS_ERROR("Service 'plan_kinematic_path_state' failed");
+ }
+
+ void run(void)
+ {
+ loadRobotDescription();
+ if (loadedRobot())
+ {
+ sleep(1);
+ runExample();
+ }
+ sleep(1);
+ }
+
+protected:
+
+ // get the current state from the StateParams instance monitored by the KinematicStateMonitor
+ void currentState(robot_msgs::KinematicState &state)
+ {
+ state.set_vals_size(m_kmodel->getModelInfo().stateDimension);
+ for (unsigned int i = 0 ; i < state.get_vals_size() ; ++i)
+ state.vals[i] = m_robotState->getParams()[i];
+ }
+
+ // convert a kinematic path message to a trajectory for the controller
+ void getTrajectoryMsg(robot_msgs::KinematicPath &path, robot_msgs::JointTraj &traj)
+ {
+ traj.set_points_size(path.get_states_size());
+ for (unsigned int i = 0 ; i < path.get_states_size() ; ++i)
+ {
+ traj.points[i].set_positions_size(path.states[i].get_vals_size());
+ for (unsigned int j = 0 ; j < path.states[i].get_vals_size() ; ++j)
+ traj.points[i].positions[j] = path.states[i].vals[j];
+ traj.points[i].time = 0.0;
+ }
+ }
+
+ // send a command to the trajectory controller using a topic
+ void sendArmCommand(robot_msgs::KinematicPath &path, const std::string &model)
+ {
+ robot_msgs::JointTraj traj;
+ getTrajectoryMsg(path, traj);
+ m_node->publish("right_arm_trajectory_command", traj);
+ ROS_INFO("Sent trajectory to controller (using a topic)");
+ }
+
+ // send a command to the trajectory controller using a service;
+ // also demo how to wait for the trajectory to finish executing
+ void sendArmCommandAndWait(robot_msgs::KinematicPath &path, const std::string &model)
+ {
+ pr2_mechanism_controllers::TrajectoryStart::Request send_traj_start_req;
+ pr2_mechanism_controllers::TrajectoryStart::Response send_traj_start_res;
+ pr2_mechanism_controllers::TrajectoryQuery::Request send_traj_query_req;
+ pr2_mechanism_controllers::TrajectoryQuery::Response send_traj_query_res;
+
+ robot_msgs::JointTraj traj;
+ getTrajectoryMsg(path, traj);
+ send_traj_start_req.traj = traj;
+
+ int traj_done = -1;
+ if (ros::service::call("right_arm_trajectory_controller/TrajectoryStart", send_traj_start_req, send_traj_start_res))
+ {
+ ROS_INFO("Sent trajectory to controller (using a service)");
+ send_traj_query_req.trajectoryid = send_traj_start_res.trajectoryid;
+ while(!(traj_done == send_traj_query_res.State_Done || traj_done == send_traj_query_res.State_Failed))
+ {
+ if(ros::service::call("right_arm_trajectory_controller/TrajectoryQuery", send_traj_query_req, send_traj_query_res))
+ {
+ traj_done = send_traj_query_res.done;
+ }
+ else
+ {
+ ROS_ERROR("Trajectory query failed");
+ }
+ }
+ ROS_INFO("Trajectory execution is complete");
+ }
+ }
+
+ // send a message that can be used to display the kinematic path
+ void sendDisplay(robot_msgs::KinematicState &start, robot_msgs::KinematicPath &path, const std::string &model)
+ {
+ robot_msgs::DisplayKinematicPath dpath;
+ dpath.frame_id = "base_link";
+ dpath.model_name = model;
+ dpath.start_state = start;
+ dpath.path = path;
+ m_node->publish("display_kinematic_path", dpath);
+ ROS_INFO("Sent planned path to display");
+ }
+
+ void printPath(robot_msgs::KinematicPath &path)
+ {
+ printf("Path with %d states\n", (int)path.states.size());
+ for (unsigned int i = 0 ; i < path.states.size() ; ++i)
+ {
+ for (unsigned int j = 0 ; j < path.states[i].vals.size() ; ++j)
+ printf("%f ", path.states[i].vals[j]);
+ printf("\n");
+ }
+ printf("\n");
+ }
+
+ // check if straight line path is valid (motion_validator node)
+ bool verifyDirectPath(robot_msgs::KinematicState &start, robot_msgs::KinematicConstraints &cstrs,
+ robot_msgs::KinematicState &goal, const std::string &model)
+ {
+ robot_srvs::ValidateKinematicPath::Request req;
+ robot_srvs::ValidateKinematicPath::Response res;
+ req.model_id = model;
+ req.start_state = start;
+ req.constraints = cstrs;
+ req.goal_state = goal;
+ if (ros::service::call("validate_path", req, res))
+ {
+ if (res.valid)
+ ROS_INFO("Direct path is valid");
+ else
+ ROS_INFO("Direct path is not valid");
+ }
+ else
+ ROS_INFO("Service 'validate_path' not available (or failed)");
+
+ return res.valid;
+ }
+
+private:
+
+ bool use_topic_;
+};
+
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv);
+
+ ros::Node node("example_execute_plan_to_state");
+ Example plan(&node);
+ plan.run();
+
+ return 0;
+}
Added: pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_plan_to_state_minimal.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_plan_to_state_minimal.cpp (rev 0)
+++ pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_plan_to_state_minimal.cpp 2009-03-03 00:38:05 UTC (rev 12010)
@@ -0,0 +1,144 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+
+/** \author Ioan Sucan */
+
+/** This is a simple program that shows how to plan a motion to a state.
+ The minimum number of things to get the arm to move is done.
+ */
+
+#include <kinematic_planning/KinematicStateMonitor.h>
+
+// service for planning to a state
+#include <robot_srvs/KinematicPlanState.h>
+
+// messages to interact with the trajectory controller
+#include <robot_msgs/JointTraj.h>
+
+static const std::string GROUPNAME = "pr2::right_arm";
+
+class Example : public kinematic_planning::KinematicStateMonitor
+{
+public:
+
+ Example(ros::Node *node) : kinematic_planning::KinematicStateMonitor(node)
+ {
+ // we use the topic for sending commands to the controller, so we need to advertise it
+ m_node->advertise<robot_msgs::JointTraj>("right_arm_trajectory_command", 1);
+ }
+
+ void runExample(void)
+ {
+ // construct the request for the motion planner
+ robot_msgs::KinematicPlanStateRequest req;
+
+ req.params.model_id = GROUPNAME;
+ req.params.distance_metric = "L2Square";
+ req.params.planner_id = "SBL";
+ req.threshold = 0.1;
+ req.interpolate = 1;
+ req.times = 1;
+
+ // skip setting the start state
+
+ // 7 DOF for the arm; pick a goal state (joint angles)
+ req.goal_state.set_vals_size(7);
+ for (unsigned int i = 0 ; i < req.goal_state.get_vals_size(); ++i)
+ req.goal_state.vals[i] = 0.0;
+ req.goal_state.vals[0] = -1.5;
+ req.goal_state.vals[1] = -0.2;
+
+ // allow 1 second computation time
+ req.allowed_time = 1.0;
+
+ // define the service messages
+ robot_srvs::KinematicPlanState::Request s_req;
+ robot_srvs::KinematicPlanState::Response s_res;
+ s_req.value = req;
+
+ if (ros::service::call("plan_kinematic_path_state", s_req, s_res))
+ sendArmCommand(s_res.value.path, GROUPNAME);
+ else
+ ROS_ERROR("Service 'plan_kinematic_path_state' failed");
+ }
+
+ void run(void)
+ {
+ loadRobotDescription();
+ if (loadedRobot())
+ {
+ sleep(1);
+ runExample();
+ }
+ sleep(1);
+ }
+
+protected:
+
+ // convert a kinematic path message to a trajectory for the controller
+ void getTrajectoryMsg(robot_msgs::KinematicPath &path, robot_msgs::JointTraj &traj)
+ {
+ traj.set_points_size(path.get_states_size());
+ for (unsigned int i = 0 ; i < path.get_states_size() ; ++i)
+ {
+ traj.points[i].set_positions_size(path.states[i].get_vals_size());
+ for (unsigned int j = 0 ; j < path.states[i].get_vals_size() ; ++j)
+ traj.points[i].positions[j] = path.states[i].vals[j];
+ traj.points[i].time = 0.0;
+ }
+ }
+
+ // send a command to the trajectory controller using a topic
+ void sendArmCommand(robot_msgs::KinematicPath &path, const std::string &model)
+ {
+ robot_msgs::JointTraj traj;
+ getTrajectoryMsg(path, traj);
+ m_node->publish("right_arm_trajectory_command", traj);
+ ROS_INFO("Sent trajectory to controller (using a topic)");
+ }
+
+};
+
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv);
+
+ ros::Node node("example_execute_plan_to_state_minimal");
+ Example plan(&node);
+ plan.run();
+
+ return 0;
+}
Added: pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp (rev 0)
+++ pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/execute_replan_to_state.cpp 2009-03-03 00:38:05 UTC (rev 12010)
@@ -0,0 +1,206 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+
+/** \author Ioan Sucan */
+
+/** This is a simple program that shows how to (re)plan a motion to a state.
+ The minimum number of things to get the arm to move (and replan) is done.
+ */
+
+#include <kinematic_planning/KinematicStateMonitor.h>
+
+// service for (re)planning to a state
+#include <robot_srvs/KinematicReplanState.h>
+
+// message for receiving the planning status
+#include <robot_msgs/KinematicPlanStatus.h>
+
+// messages to interact with the trajectory controller
+#include <robot_msgs/JointTraj.h>
+
+static const std::string GROUPNAME = "pr2::right_arm";
+
+class Example : public kinematic_planning::KinematicStateMonitor
+{
+public:
+
+ Example(ros::Node *node) : kinematic_planning::KinematicStateMonitor(node)
+ {
+ plan_id_ = -1;
+ robot_stopped_ = true;
+
+ // we use the topic for sending commands to the controller, so we need to advertise it
+ m_node->advertise<robot_msgs::JointTraj>("right_arm_trajectory_command", 1);
+ m_node->subscribe("kinematic_planning_status", plan_status_, &Example::receiveStatus, this, 1);
+ }
+
+ void runExample(void)
+ {
+ // construct the request for the motion planner
+ robot_msgs::KinematicPlanStateRequest req;
+
+ req.params.model_id = GROUPNAME;
+ req.params.distance_metric = "L2Square";
+ req.params.planner_id = "SBL";
+ req.threshold = 0.1;
+ req.interpolate = 1;
+ req.times = 1;
+
+ // skip setting the start state
+
+ // 7 DOF for the arm; pick a goal state (joint angles)
+ req.goal_state.set_vals_size(7);
+ for (unsigned int i = 0 ; i < req.goal_state.get_vals_size(); ++i)
+ req.goal_state.vals[i] = 0.0;
+ req.goal_state.vals[0] = -1.5;
+ req.goal_state.vals[1] = -0.2;
+
+ // allow 1 second computation time
+ req.allowed_time = 1.0;
+
+ // define the service messages
+ robot_srvs::KinematicReplanState::Request s_req;
+ robot_srvs::KinematicReplanState::Response s_res;
+ s_req.value = req;
+
+ if (ros::service::call("replan_kinematic_path_state", s_req, s_res))
+ plan_id_ = s_res.id;
+ else
+ ROS_ERROR("Service 'replan_kinematic_path_state' failed");
+ }
+
+ void run(void)
+ {
+ loadRobotDescription();
+ if (loadedRobot())
+ {
+ sleep(1);
+ runExample();
+ m_node->spin();
+ }
+ }
+
+protected:
+
+ // handle new status message
+ void receiveStatus(void)
+ {
+ if (plan_id_ >= 0 && plan_status_.id == plan_id_)
+ {
+ if (plan_status_.valid)
+ {
+ if (!plan_status_.path.states.empty())
+ {
+ robot_stopped_ = false;
+ sendArmCommand(plan_status_.path, GROUPNAME);
+ }
+ }
+ else
+ stopRobot();
+ if (plan_status_.done)
+ {
+ plan_id_ = -1;
+ robot_stopped_ = true;
+ ROS_INFO("Execution is complete");
+ }
+ }
+ }
+
+ void stopRobot(void)
+ {
+ if (robot_stopped_)
+ return;
+ robot_stopped_ = true;
+
+ // get the current params for the robot's right arm
+ double cmd[7];
+ m_robotState->copyParamsGroup(cmd, GROUPNAME);
+
+ robot_msgs::KinematicPath stop_path;
+ stop_path.set_states_size(1);
+ stop_path.states[0].set_vals_size(7);
+ for (unsigned int i = 0 ; i < 7 ; ++i)
+ stop_path.states[0].vals[i] = cmd[i];
+
+ sendArmCommand(stop_path, GROUPNAME);
+ }
+
+ // get the current state from the StateParams instance monitored by the KinematicStateMonitor
+ void currentState(robot_msgs::KinematicState &state)
+ {
+ state.set_vals_size(m_kmodel->getModelInfo().stateDimension);
+ for (unsigned int i = 0 ; i < state.get_vals_size() ; ++i)
+ state.vals[i] = m_robotState->getParams()[i];
+ }
+
+ // convert a kinematic path message to a trajectory for the controller
+ void getTrajectoryMsg(robot_msgs::KinematicPath &path, robot_msgs::JointTraj &traj)
+ {
+ traj.set_points_size(path.get_states_size());
+ for (unsigned int i = 0 ; i < path.get_states_size() ; ++i)
+ {
+ traj.points[i].set_positions_size(path.states[i].get_vals_size());
+ for (unsigned int j = 0 ; j < path.states[i].get_vals_size() ; ++j)
+ traj.points[i].positions[j] = path.states[i].vals[j];
+ traj.points[i].time = 0.0;
+ }
+ }
+
+ // send a command to the trajectory controller using a topic
+ void sendArmCommand(robot_msgs::KinematicPath &path, const std::string &model)
+ {
+ robot_msgs::JointTraj traj;
+ getTrajectoryMsg(path, traj);
+ m_node->publish("right_arm_trajectory_command", traj);
+ ROS_INFO("Sent trajectory to controller (using a topic)");
+ }
+
+ robot_msgs::KinematicPlanStatus plan_status_;
+ int plan_id_;
+ bool robot_stopped_;
+
+};
+
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv);
+
+ ros::Node node("example_execute_replan_to_state");
+ Example plan(&node);
+ plan.run();
+
+ return 0;
+}
Added: pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/test_path_execution.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/test_path_execution.cpp (rev 0)
+++ pkg/trunk/motion_planning/plan_ompl_path/src/right_arm/test_path_execution.cpp 2009-03-03 00:38:05 UTC (rev 12010)
@@ -0,0 +1,185 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+
+/** \author Ioan Sucan */
+
+/** This is a simple program for testing how the joints are moved
+ between the defined limits. Agreement with the mechanism control
+ is looked at as well. */
+
+#include <kinematic_planning/KinematicStateMonitor.h>
+#include <robot_msgs/JointTraj.h>
+#include <pr2_mechanism_controllers/TrajectoryStart.h>
+#include <pr2_mechanism_controllers/TrajectoryQuery.h>
+#include <cassert>
+
+class TestExecutionPath : public kinematic_planning::KinematicStateMonitor
+{
+public:
+
+ TestExecutionPath(ros::Node *node) : kinematic_planning::KinematicStateMonitor(node)
+ {
+ m_node->advertise<robot_msgs::JointTraj>("right_arm_trajectory_command", 1);
+ sleep_duration_ = 4;
+ use_topic_ = false;
+ }
+
+ void testJointLimitsRightArm(const std::string& jname = "")
+ {
+ // we send a trajectory with one state
+ robot_msgs::JointTraj traj;
+ const int controllerDim = 7;
+ std::string groupName = "pr2::right_arm";
+ traj.set_points_size(1);
+ traj.points[0].set_positions_size(controllerDim);
+
+
+ std::vector<std::string> joints;
+ if (jname.empty())
+ {
+ // get the joints in the group
+ m_kmodel->getJointsInGroup(joints, m_kmodel->getGroupID(groupName));
+ }
+ else
+ {
+ joints.push_back(jname);
+ }
+
+ for (unsigned int j = 0 ; j < joints.size() ; ++j)
+ {
+ // we only test revolute joints
+ planning_models::KinematicModel::RevoluteJoint *joint = dynamic_cast<planning_models::KinematicModel::RevoluteJoint*>(m_kmodel->getJoint(joints[j]));
+ if (!joint) continue;
+
+ printf("Testing '%s': [%f, %f]\n", joint->name.c_str(), joint->limit[0], joint->limit[1]);
+ fprintf(stderr, "%s:\n", joint->name.c_str());
+
+ // check the value of the joint at small increments
+ planning_models::KinematicModel::StateParams *sp = m_kmodel->newStateParams();
+ for (double val = joint->limit[0] ; val <= joint->limit[1] ; val += 0.1)
+ {
+ double to_send[controllerDim];
+ for (int i = 0 ; i < controllerDim ; ++i)
+ to_send[i] = 0.0;
+ sp->setParamsJoint(&val, joints[j]);
+ sp->copyParamsGroup(to_send, groupName);
+ int index = sp->getJointIndexInGroup(joints[j], groupName);
+
+ for (unsigned int i = 0 ; i < traj.points[0].get_positions_size() ; ++i)
+ traj.points[0].positions[i] = to_send[i];
+
+ if (use_topic_)
+ {
+ m_node->publish("right_arm_trajectory_command", traj);
+ sleep(sleep_duration_);
+ }
+ else
+ {
+ pr2_mechanism_controllers::TrajectoryStart::Request send_traj_start_req;
+ pr2_mechanism_controllers::TrajectoryStart::Response send_traj_start_res;
+
+ pr2_mechanism_controllers::TrajectoryQuery::Request send_traj_query_req;
+ pr2_mechanism_controllers::TrajectoryQuery::Response send_traj_query_res;
+
+ send_traj_start_req.traj = traj;
+ int traj_done = -1;
+ if (ros::service::call("right_arm_trajectory_controller/TrajectoryStart", send_traj_start_req, send_traj_start_res))
+ {
+ ROS_INFO("Sent trajectory to controller");
+
+ send_traj_query_req.trajectoryid = send_traj_start_res.trajectoryid;
+ while(!(traj_done == send_traj_query_res.State_Done || traj_done == send_traj_query_res.State_Failed))
+ {
+ if(ros::service::call("right_arm_trajectory_controller/TrajectoryQuery", send_traj_query_req, send_traj_query_res))
+ traj_done = send_traj_query_res.done;
+ else
+ {
+ ROS_ERROR("Trajectory query failed");
+ }
+ }
+ ROS_INFO("Trajectory execution is complete");
+ }
+ }
+
+ printf("Sent: ");
+ for (unsigned int i = 0 ; i < traj.points[0].get_positions_size() ; ++i)
+ printf("%f ", traj.points[0].positions[i]);
+ printf("\n");
+
+ m_robotState->copyParamsGroup(to_send, groupName);
+ printf("Achieved: ");
+ for (unsigned int i = 0 ; i < traj.points[0].get_positions_size() ; ++i)
+ printf("%f ", to_send[i]);
+ printf("\n\n");
+
+ fprintf(stderr, "...
[truncated message content] |
