Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[17169] pkg/trunk
|
From: <is...@us...> - 2009-06-16 18:53:48
|
Revision: 17169
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17169&view=rev
Author: isucan
Date: 2009-06-16 18:53:46 +0000 (Tue, 16 Jun 2009)
Log Message:
-----------
work on move_arm
Modified Paths:
--------------
pkg/trunk/highlevel/move_arm/CMakeLists.txt
pkg/trunk/highlevel/move_arm/include/move_arm/move_arm.h
pkg/trunk/highlevel/move_arm/manifest.xml
pkg/trunk/highlevel/move_arm/move_arm.launch
pkg/trunk/highlevel/move_arm/src/move_arm.cpp
pkg/trunk/highlevel/move_arm/test/test_move_arm.cpp
pkg/trunk/motion_planning/ompl_planning/motion_planning.launch
Modified: pkg/trunk/highlevel/move_arm/CMakeLists.txt
===================================================================
--- pkg/trunk/highlevel/move_arm/CMakeLists.txt 2009-06-16 18:51:08 UTC (rev 17168)
+++ pkg/trunk/highlevel/move_arm/CMakeLists.txt 2009-06-16 18:53:46 UTC (rev 17169)
@@ -1,33 +1,9 @@
cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
-# Set the build type. Options are:
-# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
-# Debug : w/ debug symbols, w/o optimization
-# Release : w/o debug symbols, w/ optimization
-# RelWithDebInfo : w/ debug symbols, w/ optimization
-# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
-#set(ROS_BUILD_TYPE RelWithDebInfo)
-
rospack(move_arm)
-#set the default path for built executables to the "bin" directory
-set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
-#set the default path for built libraries to the "lib" directory
-set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
-
-#uncomment if you have defined messages
-#genmsg()
-#uncomment if you have defined services
-#gensrv()
-
rospack_add_executable(move_arm src/move_arm.cpp)
rospack_add_executable(move_arm_action_test test/test_move_arm.cpp)
-#common commands for building c++ executables and libraries
-#rospack_add_library(${PROJECT_NAME} src/example.cpp)
-#target_link_libraries(${PROJECT_NAME} another_library)
-#rospack_add_boost_directories()
-#rospack_link_boost(${PROJECT_NAME} thread)
-#rospack_add_executable(example examples/example.cpp)
-#target_link_libraries(example ${PROJECT_NAME})
+
Modified: pkg/trunk/highlevel/move_arm/include/move_arm/move_arm.h
===================================================================
--- pkg/trunk/highlevel/move_arm/include/move_arm/move_arm.h 2009-06-16 18:51:08 UTC (rev 17168)
+++ pkg/trunk/highlevel/move_arm/include/move_arm/move_arm.h 2009-06-16 18:53:46 UTC (rev 17169)
@@ -41,70 +41,79 @@
#include <robot_actions/action.h>
#include <robot_actions/action_runner.h>
+#include <robot_msgs/JointTraj.h>
#include <pr2_robot_actions/MoveArmState.h>
#include <pr2_robot_actions/MoveArmGoal.h>
+#include <motion_planning_msgs/KinematicPath.h>
-/** ROS */
#include <ros/ros.h>
-#include <vector>
-#include <string>
+#include <planning_environment/planning_monitor.h>
-/** IK services */
-#include <manipulation_srvs/IKService.h>
-#include <manipulation_srvs/IKQuery.h>
-
namespace move_arm
-{
- /**
- * @class MoveArm
- * @brief A class adhering to the robot_actions::Action interface that moves the robot base to a goal location.
- */
- class MoveArm : public robot_actions::Action<pr2_robot_actions::MoveArmGoal, int32_t>
- {
- public:
+{
+
+ enum ArmType
+ {
+ LEFT, RIGHT
+ };
+
/**
- * @brief Constructor for the actions
+ * @class MoveArm
+ * @brief A class adhering to the robot_actions::Action interface that moves the robot base to a goal location.
*/
- MoveArm();
-
- /**
- * @brief Destructor - Cleans up
- */
- virtual ~MoveArm();
-
- /**
- * @brief Runs whenever a new goal is sent to the move_base
- * @param goal The goal to pursue
- * @param feedback Feedback that the action gives to a higher-level monitor, in this case, the position of the robot
- * @return The result of the execution, ie: Success, Preempted, Aborted, etc.
- */
- virtual robot_actions::ResultStatus execute(const pr2_robot_actions::MoveArmGoal& goal, int32_t& feedback);
-
+ class MoveArm : public robot_actions::Action<pr2_robot_actions::MoveArmGoal, int32_t>
+ {
+ public:
+ /**
+ * @brief Constructor for the actions
+ */
+ MoveArm(const ArmType &arm);
+
+ /**
+ * @brief Destructor - Cleans up
+ */
+ virtual ~MoveArm(void);
+
+ /**
+ * @brief Runs whenever a new goal is sent to the move_base
+ * @param goal The goal to pursue
+ * @param feedback Feedback that the action gives to a higher-level monitor, in this case, the position of the robot
+ * @return The result of the execution, ie: Success, Preempted, Aborted, etc.
+ */
+ virtual robot_actions::ResultStatus execute(const pr2_robot_actions::MoveArmGoal& goal, int32_t& feedback);
+
private:
+
+ // the state of the action; this should be true if initialized properly
+ bool valid_;
+
+ // the arm we are planning for
+ std::string arm_;
+ std::vector<std::string> arm_joint_names_;
- std::string ik_service_name_; /**< Name of the service that provides IK */
+
+ bool perform_ik_; /**< Flag that enables the option of IK */
+ std::string ik_service_name_; /**< Name of the service that provides IK */
+ std::string ik_query_name_; /**< Name of the service that allows you to query the joint names that IK uses */
- std::string ik_query_name_; /**< Name of the service that allows you to query the joint names that IK uses */
+ // service names
+ std::string motion_plan_name_;
+ std::string control_start_name_;
+ std::string control_query_name_;
+ std::string control_cancel_name_;
+
+ ros::NodeHandle node_handle_;
+ ros::Publisher displayPathPublisher_;
+
+ motion_planning_msgs::KinematicPath currentPath_;
+ planning_environment::CollisionModels *collisionModels_;
+ planning_environment::PlanningMonitor *planningMonitor_;
- std::string control_query_name_;
+ void fillTrajectoryPath(const motion_planning_msgs::KinematicPath &path, robot_msgs::JointTraj &traj);
+ bool computeIK(const robot_msgs::PoseStamped &pose_stamped_msg, std::vector<double> &solution);
+ bool getControlJointNames(std::vector<std::string> &joint_names);
+ };
+}
- std::string control_topic_name_;
-
- std::string control_service_name_;
-
- ros::NodeHandle node_handle_;
-
- bool computeIK(const robot_msgs::PoseStamped &pose_stamped_msg, std::vector<std::pair<std::string, double> > &solution);
-
- int arm_number_joints_;
-
- std::vector<std::string> arm_joint_names_;
-
- bool sendControl(const std::vector<std::pair<std::string, double> > &solution);
-
- bool getControlJointNames(std::vector<std::string> &joint_names);
- };
-};
-
#endif
Modified: pkg/trunk/highlevel/move_arm/manifest.xml
===================================================================
--- pkg/trunk/highlevel/move_arm/manifest.xml 2009-06-16 18:51:08 UTC (rev 17168)
+++ pkg/trunk/highlevel/move_arm/manifest.xml 2009-06-16 18:53:46 UTC (rev 17169)
@@ -15,9 +15,9 @@
<depend package="robot_msgs"/>
<depend package="motion_planning_msgs"/>
<depend package="motion_planning_srvs"/>
+ <depend package="manipulation_srvs"/>
+ <depend package="planning_environment" />
+ <depend package="robot_actions"/>
<depend package="pr2_robot_actions"/>
- <depend package="robot_actions"/>
- <depend package="manipulation_srvs"/>
<depend package="pr2_mechanism_controllers"/>
-
</package>
Modified: pkg/trunk/highlevel/move_arm/move_arm.launch
===================================================================
--- pkg/trunk/highlevel/move_arm/move_arm.launch 2009-06-16 18:51:08 UTC (rev 17168)
+++ pkg/trunk/highlevel/move_arm/move_arm.launch 2009-06-16 18:53:46 UTC (rev 17169)
@@ -1,6 +1,6 @@
<launch>
- <node pkg="move_arm" type="move_arm" name="move_arm">
+ <node pkg="move_arm" type="move_arm" name="move_arm" output="screen">
<param name="ik_service_name" value="pr2_ik_node/ik_service" />
- <param name="ik_service" value="pr2_ik_node/ik_query" />
+ <param name="ik_query_name" value="pr2_ik_node/ik_query" />
</node>
-</launch>
\ No newline at end of file
+</launch>
Modified: pkg/trunk/highlevel/move_arm/src/move_arm.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/move_arm.cpp 2009-06-16 18:51:08 UTC (rev 17168)
+++ pkg/trunk/highlevel/move_arm/src/move_arm.cpp 2009-06-16 18:53:46 UTC (rev 17169)
@@ -36,143 +36,401 @@
* Authors: Sachin Chitta, Ioan Sucan
*********************************************************************/
#include <move_arm/move_arm.h>
-#include <robot_msgs/JointTraj.h>
+
+#include <motion_planning_srvs/KinematicPlan.h>
#include <pr2_mechanism_controllers/TrajectoryStart.h>
#include <pr2_mechanism_controllers/TrajectoryQuery.h>
+#include <pr2_mechanism_controllers/TrajectoryCancel.h>
+#include <manipulation_srvs/IKService.h>
+#include <manipulation_srvs/IKQuery.h>
using namespace robot_actions;
namespace move_arm
{
- MoveArm::MoveArm() : Action<pr2_robot_actions::MoveArmGoal, int32_t>("move_arm")
- {
- node_handle_.param<int>("~arm_number_joints", arm_number_joints_,7);
- node_handle_.param<std::string>("~ik_service_name", ik_service_name_,"pr2_ik");
- node_handle_.param<std::string>("~ik_query_name", ik_query_name_, "pr2_query");
- node_handle_.param<std::string>("~control_topic_name",control_service_name_,"r_arm_joint_trajectory_controller/TrajectoryStart");
- node_handle_.param<std::string>("~control_query_name",control_query_name_,"r_arm_joint_trajectory_controller/TrajectoryQuery");
- }
+ MoveArm::MoveArm(const ArmType &arm) : Action<pr2_robot_actions::MoveArmGoal, int32_t>("move_arm")
+ {
+ if (arm == LEFT)
+ arm_ = "left_arm";
+ else
+ arm_ = "right_arm";
+
+ ROS_INFO("Starting move_arm for '%s'", arm_.c_str());
+
+ // monitor robot
+ collisionModels_ = new planning_environment::CollisionModels("robot_description");
+ planningMonitor_ = new planning_environment::PlanningMonitor(collisionModels_);
- MoveArm::~MoveArm()
- {
- }
-
- robot_actions::ResultStatus MoveArm::execute(const pr2_robot_actions::MoveArmGoal& goal, int32_t& feedback)
- {
- robot_msgs::PoseStamped arm_cartesian_goal = goal.goal_constraints.pose_constraint[0].pose;
- std::vector<std::pair<std::string, double> > solution;
- if(!computeIK(arm_cartesian_goal,solution))
- {
- ROS_ERROR("MoveArm:: IK failed");
- return robot_actions::ABORTED;
+ node_handle_.param<bool> ("~perform_ik", perform_ik_, true);
+ node_handle_.param<std::string>("~ik_service_name", ik_service_name_, "pr2_ik");
+ node_handle_.param<std::string>("~ik_query_name", ik_query_name_, "pr2_ik_query");
+
+ node_handle_.param<std::string>("~motion_plan_name", motion_plan_name_, "plan_kinematic_path");
+ node_handle_.param<std::string>("~control_start_name", control_start_name_, "right_arm/trajectory_controller/TrajectoryStart");
+ node_handle_.param<std::string>("~control_query_name", control_query_name_, "right_arm/trajectory_controller/TrajectoryQuery");
+ node_handle_.param<std::string>("~control_cancel_name", control_cancel_name_, "right_arm/trajectory_controller/TrajectoryCancel");
+
+ if (!(valid_ = collisionModels_->loadedModels() && getControlJointNames(arm_joint_names_)))
+ ROS_ERROR("Move arm action is invalid");
+
+ // advertise the topic for displaying kinematic plans
+ displayPathPublisher_ = node_handle_.advertise<motion_planning_msgs::KinematicPath>("display_kinematic_path", 10);
}
- if(!sendControl(solution))
+
+ MoveArm::~MoveArm()
{
- ROS_ERROR("MoveArm:: Control failed");
- return robot_actions::ABORTED;
+ delete planningMonitor_;
+ delete collisionModels_;
}
-
- return robot_actions::SUCCESS;
- }
+
+ robot_actions::ResultStatus MoveArm::execute(const pr2_robot_actions::MoveArmGoal& goal, int32_t& feedback)
+ {
+ if (!valid_)
+ {
+ ROS_ERROR("Move arm action has not been initialized properly");
+ return robot_actions::ABORTED;
+ }
+
+ motion_planning_srvs::KinematicPlan::Request req;
+ motion_planning_srvs::KinematicPlan::Response res;
+
+ req.params.model_id = arm_;
+ req.params.planner_id = "KPIECE";
+ req.params.distance_metric = "L2Square";
+
+ // this volume is only needed if planar or floating joints move in the space
+ req.params.volumeMin.x = req.params.volumeMin.y = req.params.volumeMin.z = 0.0;
+ req.params.volumeMax.x = req.params.volumeMax.y = req.params.volumeMax.z = 0.0;
+
+ req.goal_constraints = goal.goal_constraints;
+ req.path_constraints = goal.path_constraints;
+
+
+ if (perform_ik_ && // IK is enabled,
+ req.goal_constraints.joint_constraint.empty() && // we have no joint constraints on the goal,
+ req.goal_constraints.pose_constraint.size() == 1 && // we have a single pose constraint on the goal
+ req.goal_constraints.pose_constraint[0].type == motion_planning_msgs::PoseConstraint::POSITION_XYZ +
+ motion_planning_msgs::PoseConstraint::ORIENTATION_RPY && // that is active on all 6 DOFs
+ req.goal_constraints.pose_constraint[0].position_distance < 0.1 && // and the desired position and
+ req.goal_constraints.pose_constraint[0].orientation_distance < 0.1 && // orientation distances are small,
+ ((arm_ == "left_arm" && req.goal_constraints.pose_constraint[0].link_name == arm_joint_names_.back())
+ || (arm_ == "right_arm" && req.goal_constraints.pose_constraint[0].link_name == arm_joint_names_.back()))) // and acts on the last link of the arm
+ {
+ // we can do ik can turn the pose constraint into a joint one
+ ROS_INFO("Converting pose constraint to joint constraint using IK...");
+
+ std::vector<double> solution;
+ if (computeIK(req.goal_constraints.pose_constraint[0].pose, solution))
+ {
+ req.goal_constraints.joint_constraint.resize(1);
+ req.goal_constraints.joint_constraint[0].header.frame_id = req.goal_constraints.pose_constraint[0].pose.header.frame_id;
+ req.goal_constraints.joint_constraint[0].header.stamp = planningMonitor_->lastStateUpdate();
+ unsigned int n = 0;
+ for (unsigned int i = 0 ; i < arm_joint_names_.size() ; ++i)
+ {
+ motion_planning_msgs::JointConstraint jc;
+ jc.joint_name = arm_joint_names_[i];
+ unsigned int u = planningMonitor_->getKinematicModel()->getJoint(arm_joint_names_[i])->usedParams;
+ for (unsigned int j = 0 ; j < u ; ++j)
+ {
+ jc.value.push_back(solution[n + j]);
+ jc.toleranceAbove.push_back(0.0);
+ jc.toleranceBelow.push_back(0.0);
+ }
+ n += u;
+ req.goal_constraints.joint_constraint.push_back(jc);
+ }
+ req.goal_constraints.pose_constraint.clear();
+ }
+ else
+ ROS_WARN("Unable to compute IK");
+ }
+
+ req.times = 1;
+ req.allowed_time = 0.5;
+
+ ResultStatus result = robot_actions::SUCCESS;
+
+ feedback = pr2_robot_actions::MoveArmState::PLANNING;
+ update(feedback);
+
+ ros::ServiceClient clientPlan = node_handle_.serviceClient<motion_planning_srvs::KinematicPlan>("plan_kinematic_path", true);
+ ros::ServiceClient clientStart = node_handle_.serviceClient<pr2_mechanism_controllers::TrajectoryStart>("right_arm/trajectory_controller/TrajectoryStart", true);
+ ros::ServiceClient clientQuery = node_handle_.serviceClient<pr2_mechanism_controllers::TrajectoryQuery>("right_arm/trajectory_controller/TrajectoryQuery", true);
+ ros::ServiceClient clientCancel = node_handle_.serviceClient<pr2_mechanism_controllers::TrajectoryCancel>("right_arm/trajectory_controller/TrajectoryCancel", true);
- bool MoveArm::sendControl(const std::vector<std::pair<std::string, double> > &solution)
- {
- robot_msgs::JointTraj joint_traj;
- joint_traj.set_points_size(1);
- joint_traj.points[0].set_positions_size(solution.size());
+ int trajectoryId = -1;
+ ros::Duration eps(0.01);
+ while (true)
+ {
+ // if we have to stop, do so
+ if (isPreemptRequested() || !node_handle_.ok())
+ result = robot_actions::PREEMPTED;
+
+ // if we have to plan, do so
+ if (result == robot_actions::SUCCESS && feedback == pr2_robot_actions::MoveArmState::PLANNING)
+ {
+ // call the planner and decide whether to use the path
+ if (clientPlan.call(req, res))
+ {
+ if (res.path.states.empty())
+ ROS_WARN("Unable to plan path to desired goal");
+ else
+ {
+ if (res.unsafe)
+ ROS_WARN("Received path is unsafe (planning data was out of date). Ignoring");
+ else
+ {
+ if (res.path.model_id != req.params.model_id)
+ ROS_ERROR("Received path for incorrect model: expected '%s', received '%s'", req.params.model_id.c_str(), res.path.model_id.c_str());
+ else
+ {
+ if (!planningMonitor_->getTransformListener()->frameExists(res.path.header.frame_id))
+ ROS_ERROR("Received path in unknown frame: '%s'", res.path.header.frame_id.c_str());
+ else
+ {
+ if (res.approximate)
+ ROS_INFO("Approximate path was found. Distance to goal is: %f", res.distance);
+ currentPath_ = res.path;
+ feedback = pr2_robot_actions::MoveArmState::MOVING;
+ update(feedback);
+ }
+ }
+ }
+ }
+ }
+ else
+ {
+ ROS_ERROR("Service 'plan_kinematic_path' failed");
+ result = robot_actions::ABORTED;
+ break;
+ }
+ }
+
+ // if we have to stop, do so
+ if (isPreemptRequested() || !node_handle_.ok())
+ result = robot_actions::PREEMPTED;
- for(int j =0; j < (int) solution.size(); j++)
- {
- joint_traj.points[0].positions[j] = solution[j].second;
- }
- ros::ServiceClient client = node_handle_.serviceClient<pr2_mechanism_controllers::TrajectoryStart>(control_service_name_);
- pr2_mechanism_controllers::TrajectoryStart::Request req;
- pr2_mechanism_controllers::TrajectoryStart::Response res;
- req.traj = joint_traj;
- req.requesttiming = 1;
+ // stop the robot if we need to
+ if (feedback == pr2_robot_actions::MoveArmState::MOVING)
+ {
+ if (result == robot_actions::PREEMPTED || !planningMonitor_->isPathValid(currentPath_))
+ {
+ if (result == robot_actions::PREEMPTED)
+ ROS_INFO("Preempt requested. Stopping arm.");
+ else
+ ROS_INFO("Current path is no longer valid. Stopping & replanning...");
- ROS_DEBUG("MoveArm:: Sending out control");
- if(!client.call(req,res))
- return false;
- return true;
- }
+ if (trajectoryId != -1)
+ {
+ // we are already executing the path; we need to stop it
+ pr2_mechanism_controllers::TrajectoryCancel::Request send_traj_cancel_req;
+ pr2_mechanism_controllers::TrajectoryCancel::Response send_traj_cancel_res;
+ send_traj_cancel_req.trajectoryid = trajectoryId;
+ if (clientCancel.call(send_traj_cancel_req, send_traj_cancel_res))
+ {
+ // check if indeed the trajectory was canceled
+ pr2_mechanism_controllers::TrajectoryQuery::Request send_traj_query_req;
+ pr2_mechanism_controllers::TrajectoryQuery::Response send_traj_query_res;
+ send_traj_query_req.trajectoryid = trajectoryId;
+ if (clientQuery.call(send_traj_query_req, send_traj_query_res))
+ {
+ if (send_traj_query_res.done == pr2_mechanism_controllers::TrajectoryQuery::Response::State_Active)
+ ROS_WARN("Unable to confirm canceling trajectory %d. Continuing...", trajectoryId);
+ }
+ else
+ ROS_ERROR("Unable to query trajectory %d. Continuing...", trajectoryId);
+ }
+ else
+ ROS_ERROR("Unable to cancel trajectory %d. Continuing...", trajectoryId);
+ trajectoryId = -1;
+ }
+
+ if (result != robot_actions::PREEMPTED)
+ {
+ // if we were not preempted
+ feedback = pr2_robot_actions::MoveArmState::PLANNING;
+ update(feedback);
+ continue;
+ }
+ else
+ break;
+ }
+ }
+
+ // execute & monitor a path if we need to
+ if (result == robot_actions::SUCCESS && feedback == pr2_robot_actions::MoveArmState::MOVING)
+ {
+ // start the controller if we have to, using trajectory start
+ if (trajectoryId == -1)
+ {
+ pr2_mechanism_controllers::TrajectoryStart::Request send_traj_start_req;
+ pr2_mechanism_controllers::TrajectoryStart::Response send_traj_start_res;
+
+ fillTrajectoryPath(currentPath_, send_traj_start_req.traj);
+ send_traj_start_req.hastiming = 0;
+ send_traj_start_req.requesttiming = 0;
- bool MoveArm::getControlJointNames(std::vector<std::string> &joint_names)
- {
- ros::ServiceClient client_query = node_handle_.serviceClient<pr2_mechanism_controllers::TrajectoryQuery>(control_query_name_);
- pr2_mechanism_controllers::TrajectoryQuery::Request req_query;
- pr2_mechanism_controllers::TrajectoryQuery::Response res_query;
- req_query.trajectoryid = -1;
+ if (clientStart.call(send_traj_start_req, send_traj_start_res))
+ {
+ trajectoryId = send_traj_start_res.trajectoryid;
+ if (trajectoryId < 0)
+ {
+ ROS_ERROR("Invalid trajectory id: %d", trajectoryId);
+ result = robot_actions::ABORTED;
+ break;
+ }
+ ROS_INFO("Sent trajectory to controller");
+ }
+ else
+ {
+ ROS_ERROR("Service 'right_arm/trajectory_controller/TrajectoryStart' failed");
+ result = robot_actions::ABORTED;
+ break;
+ }
+ }
+
+ // monitor controller execution by calling trajectory query
- if(!client_query.call(req_query,res_query))
- return false;
- joint_names.resize(res_query.get_jointnames_size());
+ pr2_mechanism_controllers::TrajectoryQuery::Request send_traj_query_req;
+ pr2_mechanism_controllers::TrajectoryQuery::Response send_traj_query_res;
+ send_traj_query_req.trajectoryid = trajectoryId;
+ if (clientQuery.call(send_traj_query_req, send_traj_query_res))
+ {
+ // we are done; exit with success
+ if (send_traj_query_res.done == pr2_mechanism_controllers::TrajectoryQuery::Response::State_Done)
+ break;
+ // something bad happened in the execution
+ if (send_traj_query_res.done != pr2_mechanism_controllers::TrajectoryQuery::Response::State_Active &&
+ send_traj_query_res.done != pr2_mechanism_controllers::TrajectoryQuery::Response::State_Queued)
+ {
+ result = robot_actions::ABORTED;
+ ROS_ERROR("Unable to execute trajectory %d: query returned status %d", trajectoryId, (int)send_traj_query_res.done);
+ break;
+ }
+ }
+ else
+ {
+ ROS_ERROR("Unable to query trajectory %d", trajectoryId);
+ result = robot_actions::ABORTED;
+ break;
+ }
+ }
+ eps.sleep();
+ }
- for(int i=0; i < (int)res_query.get_jointnames_size(); i++)
- {
- joint_names[i] = res_query.jointnames[i];
- }
- return true;
- }
+ return result;
- bool MoveArm::computeIK(const robot_msgs::PoseStamped &pose_stamped_msg, std::vector<std::pair<std::string, double> > &solution)
- {
- // define the service messages
- manipulation_srvs::IKService::Request request;
- manipulation_srvs::IKService::Response response;
+ /*
- request.data.pose_stamped = pose_stamped_msg;
- request.data.set_positions_size(arm_number_joints_);
- request.data.set_joint_names_size(arm_number_joints_);
+
+ return robot_actions::SUCCESS;
- for(int i=0; i < arm_number_joints_; i++)
+ */
+ }
+
+ void MoveArm::fillTrajectoryPath(const motion_planning_msgs::KinematicPath &path, robot_msgs::JointTraj &traj)
{
- request.data.positions[i] = 0.0; // TODO - FILL THIS UP
- }
- // BIG TODO - FILL THESE UP
- request.data.joint_names[0] = "r_shoulder_pan_joint";
- request.data.joint_names[1] = "r_shoulder_lift_joint";
- request.data.joint_names[2] = "r_upper_arm_roll_joint";
-
- request.data.joint_names[3] = "r_elbow_flex_joint";
- request.data.joint_names[4] = "r_forearm_roll_joint";
- request.data.joint_names[5] = "r_wrist_flex_joint";
- request.data.joint_names[6] = "r_wrist_roll_joint";
-
- ros::ServiceClient client = node_handle_.serviceClient<manipulation_srvs::IKService>(ik_service_name_);
-
- if (client.call(request, response))
- {
- ROS_DEBUG("MoveArm:: Got IK solution");
- solution.resize(arm_number_joints_);
- for(int i=0; i< arm_number_joints_; i++)
- {
- solution[i].first = request.data.joint_names[i];
- solution[i].second = response.solution[i];
- ROS_DEBUG("Joint %s: %f",solution[i].first.c_str(),solution[i].second);
- }
- return true;
+ traj.points.resize(path.states.size());
+ for (unsigned int i = 0 ; i < path.states.size() ; ++i)
+ {
+ traj.points[i].positions = path.states[i].vals;
+ traj.points[i].time = path.times[i];
+ }
}
- else
+
+ bool MoveArm::getControlJointNames(std::vector<std::string> &joint_names)
{
- ROS_ERROR("MoveArm:: Service %s failed",ik_service_name_.c_str());
- return false;
+ ros::ServiceClient client_query = node_handle_.serviceClient<pr2_mechanism_controllers::TrajectoryQuery>(control_query_name_);
+ pr2_mechanism_controllers::TrajectoryQuery::Request req_query;
+ pr2_mechanism_controllers::TrajectoryQuery::Response res_query;
+ req_query.trajectoryid = -1;
+
+ if (!client_query.call(req_query, res_query))
+ {
+ ROS_ERROR("Unable to retrieve controller joint names using '%s' service", control_query_name_.c_str());
+ return false;
+ }
+
+ joint_names = res_query.jointnames;
+
+ // make sure we have the right joint names
+ for(unsigned int i = 0; i < joint_names.size() ; ++i)
+ {
+ if (planning_models::KinematicModel::Joint *j = planningMonitor_->getKinematicModel()->getJoint(joint_names[i]))
+ {
+ ROS_DEBUG("Using joing '%s' with %u parameters", j->name.c_str(), j->usedParams);
+ if (planningMonitor_->getKinematicModel()->getJointIndexInGroup(j->name, arm_) < 0)
+ return false;
+ }
+ else
+ {
+ ROS_ERROR("Joint '%s' is not known", joint_names[i].c_str());
+ return false;
+ }
+ }
+
+ std::vector<std::string> groupNames;
+ planningMonitor_->getKinematicModel()->getJointsInGroup(groupNames, arm_);
+ if (groupNames.size() != joint_names.size())
+ {
+ ROS_ERROR("The group '%s' has more joints than the controller can handle", arm_.c_str());
+ return false;
+ }
+
+ if (groupNames != joint_names)
+ ROS_WARN("The group joints are not in the same order as the controller expects");
+
+ return true;
}
- }
-};
+
+ bool MoveArm::computeIK(const robot_msgs::PoseStamped &pose_stamped_msg, std::vector<double> &solution)
+ {
+ // define the service messages
+ manipulation_srvs::IKService::Request request;
+ manipulation_srvs::IKService::Response response;
+
+ request.data.pose_stamped = pose_stamped_msg;
+ request.data.joint_names = arm_joint_names_;
+ request.data.positions.clear();
+ for(unsigned int i = 0; i < arm_joint_names_.size() ; ++i)
+ {
+ const double *params = planningMonitor_->getRobotState()->getParamsJoint(arm_joint_names_[i]);
+ const unsigned int u = planningMonitor_->getKinematicModel()->getJoint(arm_joint_names_[i])->usedParams;
+ for (unsigned int j = 0 ; j < u ; ++j)
+ request.data.positions.push_back(params[j]);
+ }
+
+ ros::ServiceClient client = node_handle_.serviceClient<manipulation_srvs::IKService>(ik_service_name_);
+
+ if (client.call(request, response))
+ {
+ ROS_DEBUG("MoveArm:: Got IK solution");
+ solution = response.solution;
+ for(unsigned int i = 0; i < solution.size() ; ++i)
+ ROS_DEBUG("%f", solution[i]);
+ return true;
+ }
+ else
+ {
+ ROS_ERROR("MoveArm:: Service '%s' failed", ik_service_name_.c_str());
+ return false;
+ }
+ }
+
+}
-int main(int argc, char** argv){
-// ros::init(argc, argv, "move_arm");
- ros::init(argc,argv);
- ros::Node ros_node("move_arm");
-
- move_arm::MoveArm move_arm;
- robot_actions::ActionRunner runner(20.0);
- runner.connect<pr2_robot_actions::MoveArmGoal, pr2_robot_actions::MoveArmState, int32_t>(move_arm);
- runner.run();
- ros::spin();
- return(0);
-
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv, "move_arm");
+ ros::Node xx; // hack to get action to work
+
+ move_arm::MoveArm move_arm(move_arm::RIGHT);
+ robot_actions::ActionRunner runner(20.0);
+ runner.connect<pr2_robot_actions::MoveArmGoal, pr2_robot_actions::MoveArmState, int32_t>(move_arm);
+ runner.run();
+ ros::spin();
+ return 0;
}
Modified: pkg/trunk/highlevel/move_arm/test/test_move_arm.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/test/test_move_arm.cpp 2009-06-16 18:51:08 UTC (rev 17168)
+++ pkg/trunk/highlevel/move_arm/test/test_move_arm.cpp 2009-06-16 18:53:46 UTC (rev 17169)
@@ -51,44 +51,44 @@
int main(int argc, char **argv)
{
- ros::init(argc, argv);
- ros::Node node("test_move_arm");
-// boost::thread* thread;
-
- pr2_robot_actions::SwitchControllers switchlist;
- std_msgs::Empty empty;
-
- Duration timeout_short = Duration().fromSec(2.0);
- Duration timeout_medium = Duration().fromSec(10.0);
- Duration timeout_long = Duration().fromSec(40.0);
-
-// robot_actions::ActionClient<pr2_robot_actions::SwitchControllers, pr2_robot_actions::SwitchControllersState, std_msgs::Empty> switch_controllers("switch_controllers");
- robot_actions::ActionClient<pr2_robot_actions::MoveArmGoal, pr2_robot_actions::MoveArmState, int32_t> move_arm("move_arm");
-
- int32_t feedback;
- pr2_robot_actions::MoveArmGoal goal;
- pr2_robot_actions::MoveArmState state;
-
- goal.goal_constraints.set_pose_constraint_size(1);
- goal.goal_constraints.pose_constraint[0].pose.header.stamp = ros::Time::now();
- goal.goal_constraints.pose_constraint[0].pose.header.frame_id = "torso_lift_link";
-
- goal.goal_constraints.pose_constraint[0].pose.pose.position.x = 0.75;
- goal.goal_constraints.pose_constraint[0].pose.pose.position.y = -0.188;
- goal.goal_constraints.pose_constraint[0].pose.pose.position.z = 0;
-
- goal.goal_constraints.pose_constraint[0].pose.pose.orientation.x = 0;
- goal.goal_constraints.pose_constraint[0].pose.pose.orientation.y = 0;
- goal.goal_constraints.pose_constraint[0].pose.pose.orientation.z = 0;
- goal.goal_constraints.pose_constraint[0].pose.pose.orientation.w = 1;
-
-// switchlist.start_controllers.clear(); switchlist.stop_controllers.clear();
-// switchlist.start_controllers.push_back("r_arm_joint_trajectory_controller");
-
-// if(switch_controllers.execute(switchlist, empty, timeout_medium) != robot_actions::SUCCESS) return -1;
- ROS_INFO("Done switching controllers");
-
- if(move_arm.execute(goal,feedback,timeout_long) != robot_actions::SUCCESS) return -1;
-
- return (0);
+ ros::init(argc, argv);
+ ros::Node node("test_move_arm");
+ // boost::thread* thread;
+
+ pr2_robot_actions::SwitchControllers switchlist;
+ std_msgs::Empty empty;
+
+ Duration timeout_short = Duration().fromSec(2.0);
+ Duration timeout_medium = Duration().fromSec(10.0);
+ Duration timeout_long = Duration().fromSec(40.0);
+
+ // robot_actions::ActionClient<pr2_robot_actions::SwitchControllers, pr2_robot_actions::SwitchControllersState, std_msgs::Empty> switch_controllers("switch_controllers");
+ robot_actions::ActionClient<pr2_robot_actions::MoveArmGoal, pr2_robot_actions::MoveArmState, int32_t> move_arm("move_arm");
+
+ int32_t feedback;
+ pr2_robot_actions::MoveArmGoal goal;
+ pr2_robot_actions::MoveArmState state;
+
+ goal.goal_constraints.set_pose_constraint_size(1);
+ goal.goal_constraints.pose_constraint[0].pose.header.stamp = ros::Time::now();
+ goal.goal_constraints.pose_constraint[0].pose.header.frame_id = "torso_lift_link";
+
+ goal.goal_constraints.pose_constraint[0].pose.pose.position.x = 0.75;
+ goal.goal_constraints.pose_constraint[0].pose.pose.position.y = -0.188;
+ goal.goal_constraints.pose_constraint[0].pose.pose.position.z = 0;
+
+ goal.goal_constraints.pose_constraint[0].pose.pose.orientation.x = 0;
+ goal.goal_constraints.pose_constraint[0].pose.pose.orientation.y = 0;
+ goal.goal_constraints.pose_constraint[0].pose.pose.orientation.z = 0;
+ goal.goal_constraints.pose_constraint[0].pose.pose.orientation.w = 1;
+
+ // switchlist.start_controllers.clear(); switchlist.stop_controllers.clear();
+ // switchlist.start_controllers.push_back("r_arm_joint_trajectory_controller");
+
+ // if(switch_controllers.execute(switchlist, empty, timeout_medium) != robot_actions::SUCCESS) return -1;
+ ROS_INFO("Done switching controllers");
+
+ if(move_arm.execute(goal,feedback,timeout_long) != robot_actions::SUCCESS) return -1;
+
+ return (0);
}
Modified: pkg/trunk/motion_planning/ompl_planning/motion_planning.launch
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/motion_planning.launch 2009-06-16 18:51:08 UTC (rev 17168)
+++ pkg/trunk/motion_planning/ompl_planning/motion_planning.launch 2009-06-16 18:53:46 UTC (rev 17169)
@@ -1,14 +1,9 @@
<launch>
-<!--
- <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/pr2.xacro.xml'" />
- <rosparam command="load" ns="robotdesc/pr2_planning" file="$(find pr2_defs)/planning/planning.yaml" />
- <rosparam command="load" ns="robotdesc/pr2_collision" file="$(find pr2_defs)/collision/collision_checks.yaml" />
--->
<node pkg="ompl_planning" type="motion_planner" args="robot_description:=robotdesc/pr2 collision_map:=collision_map_buffer" respawn="true" output="screen">
-<!-- <param name="~refresh_interval_collision_map" type="double" value="0.0" />
+ <param name="~refresh_interval_collision_map" type="double" value="0.0" />
<param name="~refresh_interval_kinematic_state" type="double" value="0.0" />
<param name="~refresh_interval_base_pose" type="double" value="0.0" />
- <param name="~kinematic_planning_status_interval" type="double" value="0.02" /> -->
+ <param name="~kinematic_planning_status_interval" type="double" value="0.02" />
</node>
</launch>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
