[Personalrobots-commit] SF.net SVN: personalrobots:[17273] pkg/trunk

[<is...@us...>]

Revision: 17273
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17273&view=rev
Author:   isucan
Date:     2009-06-18 03:09:27 +0000 (Thu, 18 Jun 2009)

Log Message:
-----------
some bugfixes

Modified Paths:
--------------
    pkg/trunk/highlevel/move_arm/CMakeLists.txt
    pkg/trunk/highlevel/move_arm/src/move_arm.cpp
    pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
    pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h
    pkg/trunk/motion_planning/planning_models/src/kinematic.cpp

Added Paths:
-----------
    pkg/trunk/highlevel/move_arm/test/move_to_position.cpp
    pkg/trunk/highlevel/move_arm/test/oscillate_move.cpp

Removed Paths:
-------------
    pkg/trunk/highlevel/move_arm/test/test_move_arm.cpp

Modified: pkg/trunk/highlevel/move_arm/CMakeLists.txt
===================================================================
--- pkg/trunk/highlevel/move_arm/CMakeLists.txt	2009-06-18 01:55:03 UTC (rev 17272)
+++ pkg/trunk/highlevel/move_arm/CMakeLists.txt	2009-06-18 03:09:27 UTC (rev 17273)
@@ -4,6 +4,6 @@
 rospack(move_arm)
 
 rospack_add_executable(move_arm src/move_arm.cpp)
-rospack_add_executable(move_arm_action_test test/test_move_arm.cpp)
 
-
+rospack_add_executable(move_to_position test/move_to_position.cpp)
+rospack_add_executable(oscillate_move test/oscillate_move.cpp)

Modified: pkg/trunk/highlevel/move_arm/src/move_arm.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/move_arm.cpp	2009-06-18 01:55:03 UTC (rev 17272)
+++ pkg/trunk/highlevel/move_arm/src/move_arm.cpp	2009-06-18 03:09:27 UTC (rev 17273)
@@ -103,18 +103,26 @@
 	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";
 	
+	req.params.model_id = arm_;              // the model to plan for (should be defined in planning.yaml)
+	req.params.planner_id = "KPIECE";        // this is optional; the planning node should be able to pick a planner
+	req.params.distance_metric = "L2Square"; // the metric to be used in the robot's state space
+	
 	// 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;
 	
+	// forward the goal & path constraints
 	req.goal_constraints = goal.goal_constraints;
 	req.path_constraints = goal.path_constraints;
 	
-	
+	// compute the path once
+	req.times = 1;
+
+	// do not spend more than this amount of time
+	req.allowed_time = 0.5;
+
+	// change pose constraints to joint constraints, if possible and so desired
 	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
@@ -156,9 +164,6 @@
 	    }
 	}
 	
-	req.times = 1;
-	req.allowed_time = 0.5;
-	
 	ResultStatus result = robot_actions::SUCCESS;
 	
 	feedback = pr2_robot_actions::MoveArmState::PLANNING;
@@ -180,6 +185,20 @@
 	    // if we have to plan, do so
 	    if (result == robot_actions::SUCCESS && feedback == pr2_robot_actions::MoveArmState::PLANNING)
 	    {
+		
+		// fill in start state with current one
+		std::vector<planning_models::KinematicModel::Joint*> joints;
+		planningMonitor_->getKinematicModel()->getJoints(joints);
+		
+		req.start_state.resize(joints.size());
+		for (unsigned int i = 0 ; i < joints.size() ; ++i)
+		{
+		    req.start_state[i].header.frame_id = planningMonitor_->getFrameId();
+		    req.start_state[i].header.stamp = planningMonitor_->lastStateUpdate();
+		    req.start_state[i].joint_name = joints[i]->name;
+		    planningMonitor_->getRobotState()->copyParamsJoint(req.start_state[i].value, joints[i]->name);
+		}
+		
 		// call the planner and decide whether to use the path 
 		if (clientPlan.call(req, res))
 		{

Copied: pkg/trunk/highlevel/move_arm/test/move_to_position.cpp (from rev 17262, pkg/trunk/highlevel/move_arm/test/test_move_arm.cpp)
===================================================================
--- pkg/trunk/highlevel/move_arm/test/move_to_position.cpp	                        (rev 0)
+++ pkg/trunk/highlevel/move_arm/test/move_to_position.cpp	2009-06-18 03:09:27 UTC (rev 17273)
@@ -0,0 +1,100 @@
+/*********************************************************************
+ * 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 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.
+ *
+ * $Id: test_executive.cpp 16122 2009-05-27 00:28:28Z meeussen $
+ *
+ *********************************************************************/
+
+/* Author: Sachin Chitta */
+
+
+#include <ros/node.h>
+#include <boost/thread/thread.hpp>
+#include <robot_actions/action_client.h>
+
+#include <pr2_robot_actions/MoveArmGoal.h>
+#include <pr2_robot_actions/MoveArmState.h>
+#include <pr2_robot_actions/SwitchControllersState.h>
+
+using namespace ros;
+using namespace std;
+
+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].link_name = "r_wrist_roll_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;
+    goal.goal_constraints.pose_constraint[0].position_distance = 0.01;
+    goal.goal_constraints.pose_constraint[0].orientation_distance = 0.01;
+    goal.goal_constraints.pose_constraint[0].orientation_importance = 0.1;
+    goal.goal_constraints.pose_constraint[0].type = motion_planning_msgs::PoseConstraint::POSITION_XYZ + 
+	motion_planning_msgs::PoseConstraint::ORIENTATION_RPY;
+    
+    //  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);
+}


Property changes on: pkg/trunk/highlevel/move_arm/test/move_to_position.cpp
___________________________________________________________________
Added: svn:mergeinfo
   + /pkg/branches/gazebo-branch-merge/highlevel/move_arm/test/test_move_arm.cpp:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334

Copied: pkg/trunk/highlevel/move_arm/test/oscillate_move.cpp (from rev 17262, pkg/trunk/highlevel/move_arm/test/test_move_arm.cpp)
===================================================================
--- pkg/trunk/highlevel/move_arm/test/oscillate_move.cpp	                        (rev 0)
+++ pkg/trunk/highlevel/move_arm/test/oscillate_move.cpp	2009-06-18 03:09:27 UTC (rev 17273)
@@ -0,0 +1,122 @@
+/*********************************************************************
+ * 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 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 */
+
+#include <ros/ros.h>
+#include <robot_actions/action_client.h>
+
+#include <pr2_robot_actions/MoveArmGoal.h>
+#include <pr2_robot_actions/MoveArmState.h>
+
+#include <vector>
+#include <string>
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "test_move_arm");
+    
+    ros::Node hack; // hack
+    
+    robot_actions::ActionClient<pr2_robot_actions::MoveArmGoal, pr2_robot_actions::MoveArmState, int32_t> move_arm("move_arm");
+    
+    int32_t                         feedback;
+    pr2_robot_actions::MoveArmGoal  goalA;
+    pr2_robot_actions::MoveArmGoal  goalB;
+    pr2_robot_actions::MoveArmState state;
+    
+    goalA.goal_constraints.set_pose_constraint_size(1);
+    goalA.goal_constraints.pose_constraint[0].pose.header.stamp = ros::Time::now();
+    goalA.goal_constraints.pose_constraint[0].pose.header.frame_id = "torso_lift_link";
+    
+    goalA.goal_constraints.pose_constraint[0].link_name = "r_wrist_roll_link";
+    goalA.goal_constraints.pose_constraint[0].pose.pose.position.x = 0.75;
+    goalA.goal_constraints.pose_constraint[0].pose.pose.position.y = -0.188;
+    goalA.goal_constraints.pose_constraint[0].pose.pose.position.z = 0;
+    
+    goalA.goal_constraints.pose_constraint[0].pose.pose.orientation.x = 0;
+    goalA.goal_constraints.pose_constraint[0].pose.pose.orientation.y = 0;
+    goalA.goal_constraints.pose_constraint[0].pose.pose.orientation.z = 0;
+    goalA.goal_constraints.pose_constraint[0].pose.pose.orientation.w = 1;
+    goalA.goal_constraints.pose_constraint[0].position_distance = 0.01;
+    goalA.goal_constraints.pose_constraint[0].orientation_distance = 0.01;
+    goalA.goal_constraints.pose_constraint[0].orientation_importance = 0.1;
+    goalA.goal_constraints.pose_constraint[0].type =
+	motion_planning_msgs::PoseConstraint::POSITION_XYZ + 
+	motion_planning_msgs::PoseConstraint::ORIENTATION_RPY;
+    
+    
+    std::vector<std::string> names(7);
+    names[0] = "r_shoulder_pan_joint";
+    names[1] = "r_shoulder_lift_joint";
+    names[2] = "r_upper_arm_roll_joint";
+    names[3] = "r_elbow_flex_joint";
+    names[4] = "r_forearm_roll_joint";
+    names[5] = "r_wrist_flex_joint";
+    names[6] = "r_wrist_roll_joint";
+    
+    goalB.goal_constraints.joint_constraint.resize(names.size());
+    for (unsigned int i = 0 ; i < goalB.goal_constraints.joint_constraint.size(); ++i)
+    {
+	goalB.goal_constraints.joint_constraint[i].header.stamp = ros::Time::now();
+	goalB.goal_constraints.joint_constraint[i].header.frame_id = "base_link";
+	goalB.goal_constraints.joint_constraint[i].joint_name = names[i];
+	goalB.goal_constraints.joint_constraint[i].value.resize(1);
+	goalB.goal_constraints.joint_constraint[i].toleranceAbove.resize(1);
+	goalB.goal_constraints.joint_constraint[i].toleranceBelow.resize(1);
+	goalB.goal_constraints.joint_constraint[i].value[0] = 0.0;
+	goalB.goal_constraints.joint_constraint[i].toleranceBelow[0] = 0.0;
+	goalB.goal_constraints.joint_constraint[i].toleranceAbove[0] = 0.0;
+    }
+    
+    goalB.goal_constraints.joint_constraint[0].value[0] = -2.0;
+    goalB.goal_constraints.joint_constraint[3].value[0] = -0.1;
+    goalB.goal_constraints.joint_constraint[5].value[0] = 0.15;
+
+    ros::NodeHandle nh;
+    
+    while (nh.ok())
+    {
+	ros::spinOnce();
+	if (move_arm.execute(goalA, feedback, ros::Duration(10.0)) != robot_actions::SUCCESS)
+	    return -1;
+	ros::spinOnce();
+	if (move_arm.execute(goalB, feedback, ros::Duration(10.0)) != robot_actions::SUCCESS)
+	    return -1;
+    }
+    
+    return 0;
+}


Property changes on: pkg/trunk/highlevel/move_arm/test/oscillate_move.cpp
___________________________________________________________________
Added: svn:mergeinfo
   + /pkg/branches/gazebo-branch-merge/highlevel/move_arm/test/test_move_arm.cpp:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334

Deleted: pkg/trunk/highlevel/move_arm/test/test_move_arm.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/test/test_move_arm.cpp	2009-06-18 01:55:03 UTC (rev 17272)
+++ pkg/trunk/highlevel/move_arm/test/test_move_arm.cpp	2009-06-18 03:09:27 UTC (rev 17273)
@@ -1,100 +0,0 @@
-/*********************************************************************
- * 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 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.
- *
- * $Id: test_executive.cpp 16122 2009-05-27 00:28:28Z meeussen $
- *
- *********************************************************************/
-
-/* Author: Sachin Chitta */
-
-
-#include <ros/node.h>
-#include <boost/thread/thread.hpp>
-#include <robot_actions/action_client.h>
-
-#include <pr2_robot_actions/MoveArmGoal.h>
-#include <pr2_robot_actions/MoveArmState.h>
-#include <pr2_robot_actions/SwitchControllersState.h>
-
-using namespace ros;
-using namespace std;
-
-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].link_name = "r_wrist_roll_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;
-    goal.goal_constraints.pose_constraint[0].position_distance = 0.01;
-    goal.goal_constraints.pose_constraint[0].orientation_distance = 0.01;
-    goal.goal_constraints.pose_constraint[0].orientation_importance = 0.1;
-    goal.goal_constraints.pose_constraint[0].type = motion_planning_msgs::PoseConstraint::POSITION_XYZ + 
-	motion_planning_msgs::PoseConstraint::ORIENTATION_RPY;
-    
-    //  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/src/motion_planner.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp	2009-06-18 01:55:03 UTC (rev 17272)
+++ pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp	2009-06-18 03:09:27 UTC (rev 17273)
@@ -117,7 +117,7 @@
 	    planningMonitor_->getRobotState()->copyParams(res.path.start_state.vals);
 
 	    // apply changes as indicated in request
-	    for (unsigned int i = 0 ; req.start_state.size() ; ++i)
+	    for (unsigned int i = 0 ; i < req.start_state.size() ; ++i)
 	    {
 		if (!planningMonitor_->getTransformListener()->frameExists(req.start_state[i].header.frame_id))
 		{

Modified: pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h
===================================================================
--- pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h	2009-06-18 01:55:03 UTC (rev 17272)
+++ pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h	2009-06-18 03:09:27 UTC (rev 17273)
@@ -531,9 +531,12 @@
 	    /** Copy all parameters to a destination address */
 	    void copyParams(std::vector<double> &params) const;
 	    
-	    /** Copy the parameters describen a given joint */
+	    /** Copy the parameters describing a given joint */
 	    void copyParamsJoint(double *params, const std::string &name) const;
 	    
+	    /** Copy the parameters describing a given joint */
+	    void copyParamsJoint(std::vector<double> &params, const std::string &name) const;
+	    
 	    /** Check if all params in a group were seen */
 	    bool seenAllGroup(const std::string &group) const;
 

Modified: pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/kinematic.cpp	2009-06-18 01:55:03 UTC (rev 17272)
+++ pkg/trunk/motion_planning/planning_models/src/kinematic.cpp	2009-06-18 03:09:27 UTC (rev 17273)
@@ -472,13 +472,19 @@
 
 void planning_models::KinematicModel::getJoints(std::vector<Joint*> &joints) const
 {
-    joints.resize(m_jointMap.size());
+    std::vector<Joint*> jn(m_mi.stateDimension, NULL);
     for (std::map<std::string, Joint*>::const_iterator it = m_jointMap.begin() ; it != m_jointMap.end() ; ++it)
     {
+	if (it->second->usedParams == 0)
+	    continue;
 	std::map<std::string, unsigned int>::const_iterator p = m_mi.parameterIndex.find(it->first);
 	assert(p != m_mi.parameterIndex.end());
-	joints[p->second] = it->second;
+	jn[p->second] = it->second;
     }
+    joints.clear();
+    for (unsigned int i = 0 ; i < jn.size() ; ++i)
+	if (jn[i])
+	    joints.push_back(jn[i]);
 }
 
 unsigned int planning_models::KinematicModel::getGroupDimension(int groupID) const
@@ -504,10 +510,9 @@
     for (std::map<std::string, Joint*>::const_iterator it = m_jointMap.begin() ; it != m_jointMap.end() ; ++it)
 	if (it->second->inGroup[groupID] && it->second->usedParams > 0)
 	    nm.push_back(it->first);
-    unsigned int start = names.size();
-    names.resize(start + nm.size());
+    names.resize(nm.size());
     for (unsigned int i = 0 ; i < nm.size() ; ++i)
-	names[start + getJointIndexInGroup(nm[i], groupID)] = nm[i];
+	names[getJointIndexInGroup(nm[i], groupID)] = nm[i];
 }
 
 void planning_models::KinematicModel::buildChainJ(Robot *robot, Link *parent, Joint* joint, const robot_desc::URDF::Link* urdfLink, const robot_desc::URDF &model)
@@ -895,6 +900,24 @@
     m_msg.error("Unknown joint: '" + name + "'");
 }
 
+void planning_models::KinematicModel::StateParams::copyParamsJoint(std::vector<double> &params, const std::string &name) const
+{
+    Joint *joint = m_owner->getJoint(name);
+    if (joint)
+    {
+	params.resize(joint->usedParams);
+	std::map<std::string, unsigned int>::const_iterator it = m_mi.parameterIndex.find(name);
+	if (it != m_mi.parameterIndex.end())
+	{
+	    unsigned int pos = it->second;
+	    for (unsigned int i = 0 ; i < joint->usedParams ; ++i)
+		params[i] = m_params[pos + i];
+	    return;
+	}
+    }
+    m_msg.error("Unknown joint: '" + name + "'");
+}
+
 void planning_models::KinematicModel::StateParams::copyParams(double *params) const
 {
     for (unsigned int i = 0 ; i < m_mi.stateDimension ; ++i)


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.