Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[23712] pkg/trunk
|
From: <is...@us...> - 2009-09-02 21:47:16
|
Revision: 23712
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23712&view=rev
Author: isucan
Date: 2009-09-02 21:46:41 +0000 (Wed, 02 Sep 2009)
Log Message:
-----------
switching the arm planning stack to the new parser; changing planning groups to contain joints rather than links; planning_models::StateParams becomes planning_models::KinematicState, added planning_models::KinematicModel::JointGroup, cleaner interface, no longer doing recursive FK
Modified Paths:
--------------
pkg/trunk/highlevel/move_arm/launch/arm_cmd_line.launch
pkg/trunk/highlevel/move_arm/launch/move_rarm.launch
pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp
pkg/trunk/highlevel/move_arm/src/move_arm_setup.cpp
pkg/trunk/highlevel/move_arm/src/record_path.cpp
pkg/trunk/highlevel/move_arm/src/record_state.cpp
pkg/trunk/highlevel/move_arm/src/teleop_arm.cpp
pkg/trunk/motion_planning/motion_planning_rviz_plugin/manifest.xml
pkg/trunk/motion_planning/motion_planning_rviz_plugin/src/planning_display.cpp
pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.cpp
pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.h
pkg/trunk/motion_planning/ompl_ros/include/ompl_ros/ModelBase.h
pkg/trunk/motion_planning/ompl_ros/include/ompl_ros/dynamic/SpaceInformation.h
pkg/trunk/motion_planning/ompl_ros/include/ompl_ros/dynamic/StateValidator.h
pkg/trunk/motion_planning/ompl_ros/include/ompl_ros/kinematic/SpaceInformation.h
pkg/trunk/motion_planning/ompl_ros/include/ompl_ros/kinematic/StateValidator.h
pkg/trunk/motion_planning/ompl_ros/manifest.xml
pkg/trunk/motion_planning/ompl_ros/src/ModelBase.cpp
pkg/trunk/motion_planning/ompl_ros/src/base/GoalDefinitions.cpp
pkg/trunk/motion_planning/ompl_ros/src/base/ProjectionEvaluators.cpp
pkg/trunk/motion_planning/ompl_ros/src/dynamic/SpaceInformation.cpp
pkg/trunk/motion_planning/ompl_ros/src/dynamic/StateValidator.cpp
pkg/trunk/motion_planning/ompl_ros/src/kinematic/SpaceInformation.cpp
pkg/trunk/motion_planning/ompl_ros/src/kinematic/StateValidator.cpp
pkg/trunk/motion_planning/planning_environment/include/planning_environment/models/robot_models.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/kinematic_model_state_monitor.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/monitors/planning_monitor.h
pkg/trunk/motion_planning/planning_environment/include/planning_environment/util/kinematic_state_constraint_evaluator.h
pkg/trunk/motion_planning/planning_environment/src/models/robot_models.cpp
pkg/trunk/motion_planning/planning_environment/src/monitors/kinematic_model_state_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/monitors/planning_monitor.cpp
pkg/trunk/motion_planning/planning_environment/src/tools/clear_known_objects.cpp
pkg/trunk/motion_planning/planning_environment/src/util/kinematic_state_constraint_evaluator.cpp
pkg/trunk/motion_planning/planning_environment/test/test_robot_models.cpp
pkg/trunk/motion_planning/planning_models/CMakeLists.txt
pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_model.h
pkg/trunk/motion_planning/planning_models/manifest.xml
pkg/trunk/motion_planning/planning_models/src/kinematic_model.cpp
pkg/trunk/motion_planning/planning_research/pm_wrapper/include/pm_wrapper/pm_wrapper.h
pkg/trunk/motion_planning/planning_research/pm_wrapper/src/pm_wrapper.cpp
pkg/trunk/motion_planning/self_watch/self_watch.cpp
pkg/trunk/sandbox/3dnav_pr2/launch/perception/bits/collision_map_self_occ.launch
pkg/trunk/sandbox/3dnav_pr2/launch/perception/bits/tilt_laser_self_filter.launch
pkg/trunk/sandbox/3dnav_pr2/launch/perception/laser-perception.launch
pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/ompl_planning.launch
pkg/trunk/sandbox/3dnav_pr2/launch/planning/bits/ompl_search.launch
pkg/trunk/sandbox/3dnav_pr2/launch/pr2_planning_environment.launch
pkg/trunk/sandbox/3dnav_pr2/params/planning/planning.yaml
pkg/trunk/sandbox/chomp_motion_planner/manifest.xml
pkg/trunk/sandbox/chomp_motion_planner/src/chomp_robot_model.cpp
pkg/trunk/sandbox/fake_motion_planning/fake_motion_planner.cpp
pkg/trunk/sandbox/fake_motion_planning/manifest.xml
pkg/trunk/sandbox/fk_node/src/fk_node.cpp
pkg/trunk/sandbox/ompl_search/src/SearchRequestHandler.cpp
pkg/trunk/sandbox/ompl_search/src/SearchRequestHandler.h
pkg/trunk/sandbox/ompl_search/src/search_valid_state.cpp
pkg/trunk/sandbox/robot_voxelizer/include/robot_voxelizer/robot_voxelizer.h
pkg/trunk/world_models/collision_space/include/collision_space/environment.h
pkg/trunk/world_models/collision_space/src/environmentODE.cpp
pkg/trunk/world_models/test_collision_space/src/collision_test_speed.cpp
Added Paths:
-----------
pkg/trunk/motion_planning/planning_models/test/test_kinematic.cpp
Removed Paths:
-------------
pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h
pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_state_params.h
pkg/trunk/motion_planning/planning_models/include/planning_models/output.h
pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
pkg/trunk/motion_planning/planning_models/src/kinematic_state_params.cpp
pkg/trunk/motion_planning/planning_models/src/output.cpp
pkg/trunk/motion_planning/planning_models/test/test_kinematic.cpp
pkg/trunk/motion_planning/planning_models/test/test_kinematic2.cpp
Modified: pkg/trunk/highlevel/move_arm/launch/arm_cmd_line.launch
===================================================================
--- pkg/trunk/highlevel/move_arm/launch/arm_cmd_line.launch 2009-09-02 21:42:16 UTC (rev 23711)
+++ pkg/trunk/highlevel/move_arm/launch/arm_cmd_line.launch 2009-09-02 21:46:41 UTC (rev 23712)
@@ -1,6 +1,6 @@
<launch>
<node pkg="move_arm" type="arm_cmd_line" output="screen" args="right">
- <remap from="robot_description" to="robot_description" />
+ <remap from="robot_description" to="robot_description_new" />
<remap from="arm_ik" to="pr2_ik_right_arm/ik_service" />
</node>
</launch>
Modified: pkg/trunk/highlevel/move_arm/launch/move_rarm.launch
===================================================================
--- pkg/trunk/highlevel/move_arm/launch/move_rarm.launch 2009-09-02 21:42:16 UTC (rev 23711)
+++ pkg/trunk/highlevel/move_arm/launch/move_rarm.launch 2009-09-02 21:46:41 UTC (rev 23712)
@@ -1,6 +1,7 @@
<launch>
<node pkg="move_arm" type="move_arm_action" output="screen" name="move_right_arm">
+ <remap from="robot_description" to="robot_description_new" />
<remap from="collision_map" to="collision_map_occ" />
<remap from="collision_map_update" to="collision_map_occ_update" />
@@ -15,7 +16,7 @@
<param name="group" type="string" value="right_arm" />
<param name="show_collisions" type="bool" value="true" />
- <param name="refresh_interval_collision_map" type="double" value="20.0" />
+ <param name="refresh_interval_collision_map" type="double" value="0.0" />
<param name="refresh_interval_kinematic_state" type="double" value="1.0" />
<param name="bounding_planes" type="string" value="0 0 1 -0.01" />
</node>
Modified: pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-09-02 21:42:16 UTC (rev 23711)
+++ pkg/trunk/highlevel/move_arm/src/arm_cmd_line.cpp 2009-09-02 21:46:41 UTC (rev 23712)
@@ -44,6 +44,7 @@
#include <mapping_msgs/AttachedObject.h>
#include <planning_environment/monitors/kinematic_model_state_monitor.h>
+#include <planning_models/kinematic_state.h>
#include <motion_planning_msgs/KinematicPath.h>
#include <manipulation_msgs/JointTraj.h>
#include <manipulation_srvs/IKService.h>
@@ -87,12 +88,12 @@
void printJoints(const planning_environment::KinematicModelStateMonitor &km, const std::vector<std::string> &names)
{
- const planning_models::KinematicModel::ModelInfo &mi = km.getKinematicModel()->getModelInfo();
+ const std::vector<double> &bounds = km.getKinematicModel()->getStateBounds();
for (unsigned int i = 0 ; i < names.size(); ++i)
{
- int idx = km.getKinematicModel()->getJointIndex(names[i]);
- std::cout << " " << i << " = " << names[i] << " [" << mi.stateBounds[idx * 2] << ", " << mi.stateBounds[idx * 2 + 1] << "]" << std::endl;
+ int idx = km.getKinematicModel()->getJoint(names[i])->stateIndex;
+ std::cout << " " << i << " = " << names[i] << " [" << bounds[idx * 2] << ", " << bounds[idx * 2 + 1] << "]" << std::endl;
}
}
@@ -103,7 +104,7 @@
std::cout << " -rotation [x, y, z, w] = [" << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w() << "]" << std::endl;
}
-void goalToState(const move_arm::MoveArmGoal &goal, planning_models::StateParams &sp)
+void goalToState(const move_arm::MoveArmGoal &goal, planning_models::KinematicState &sp)
{
for (unsigned int i = 0 ; i < goal.goal_constraints.joint_constraint.size() ; ++i)
{
@@ -114,7 +115,7 @@
btTransform effPosition(const planning_environment::KinematicModelStateMonitor &km, const move_arm::MoveArmGoal &goal)
{
- planning_models::StateParams sp(*km.getRobotState());
+ planning_models::KinematicState sp(*km.getRobotState());
goalToState(goal, sp);
km.getKinematicModel()->computeTransforms(sp.getParams());
return km.getKinematicModel()->getJoint(goal.goal_constraints.joint_constraint.back().joint_name)->after->globalTrans;
@@ -223,10 +224,10 @@
goal.contacts[1].pose = goal.goal_constraints.pose_constraint[0].pose;
}
-void setConfig(const planning_models::StateParams *_sp, const std::vector<std::string> &names, move_arm::MoveArmGoal &goal)
+void setConfig(const planning_models::KinematicState *_sp, const std::vector<std::string> &names, move_arm::MoveArmGoal &goal)
{
setupGoal(names, goal);
- planning_models::StateParams sp(*_sp);
+ planning_models::KinematicState sp(*_sp);
sp.enforceBounds();
for (unsigned int i = 0 ; i < names.size() ; ++i)
{
@@ -235,7 +236,7 @@
}
}
-void getIK(bool r, ros::NodeHandle &nh, planning_environment::KinematicModelStateMonitor &km, move_arm::MoveArmGoal &goal, planning_models::StateParams &sp,
+void getIK(bool r, ros::NodeHandle &nh, planning_environment::KinematicModelStateMonitor &km, move_arm::MoveArmGoal &goal, planning_models::KinematicState &sp,
const std::vector<std::string> &names, double x, double y, double z)
{
ros::ServiceClient client = nh.serviceClient<manipulation_srvs::IKService>("arm_ik");
@@ -252,7 +253,7 @@
request.data.pose_stamped.pose.orientation.w = 1;
request.data.joint_names = names;
- planning_models::StateParams rs(*km.getRobotState());
+ planning_models::KinematicState rs(*km.getRobotState());
if (r)
rs.randomState();
@@ -308,7 +309,7 @@
std::cout << " -rotation distance: " << angle << std::endl;
}
-void viewState(ros::Publisher &view, const planning_environment::KinematicModelStateMonitor &km, const planning_models::StateParams &st)
+void viewState(ros::Publisher &view, const planning_environment::KinematicModelStateMonitor &km, const planning_models::KinematicState &st)
{
motion_planning_msgs::KinematicPath kp;
@@ -316,7 +317,7 @@
kp.header.stamp = km.lastJointStateUpdate();
// fill in start state with current one
- std::vector<planning_models::KinematicModel::Joint*> joints;
+ std::vector<const planning_models::KinematicModel::Joint*> joints;
km.getKinematicModel()->getJoints(joints);
kp.start_state.resize(joints.size());
@@ -503,7 +504,7 @@
else
if (cmd == "view")
{
- planning_models::StateParams st(*km.getRobotState());
+ planning_models::KinematicState st(*km.getRobotState());
viewState(view, km, st);
}
else
@@ -528,7 +529,7 @@
setConfig(km.getRobotState(), names, temp);
btTransform p = effPosition(km, temp);
- planning_models::StateParams sp(*km.getRobotState());
+ planning_models::KinematicState sp(*km.getRobotState());
getIK(false, nh, km, temp, sp, names, p.getOrigin().x() + fwd, p.getOrigin().y(), p.getOrigin().z());
sp.copyParamsJoints(traj.points[1].positions, names);
@@ -571,7 +572,7 @@
setConfig(km.getRobotState(), names, temp);
btTransform p = effPosition(km, temp);
- planning_models::StateParams sp(*km.getRobotState());
+ planning_models::KinematicState sp(*km.getRobotState());
getIK(false, nh, km, temp, sp, names, p.getOrigin().x(), p.getOrigin().y(), p.getOrigin().z() + up);
sp.copyParamsJoints(traj.points[1].positions, names);
@@ -601,7 +602,7 @@
std::cout << "Configuration '" << config << "' not found" << std::endl;
else
{
- planning_models::StateParams st(*km.getRobotState());
+ planning_models::KinematicState st(*km.getRobotState());
goalToState(goals[config], st);
viewState(view, km, st);
}
@@ -643,7 +644,7 @@
{
std::string config = cmd.substr(5);
boost::trim(config);
- planning_models::StateParams *sp = km.getKinematicModel()->newStateParams();
+ planning_models::KinematicState *sp = new planning_models::KinematicState(km.getKinematicModel());
sp->randomState();
setConfig(sp, names, goals[config]);
delete sp;
@@ -675,7 +676,7 @@
ss >> z;
err = false;
std::cout << "Performing IK to " << x << ", " << y << ", " << z << ", 0, 0, 0, 1..." << std::endl;
- planning_models::StateParams sp(*km.getRobotState());
+ planning_models::KinematicState sp(*km.getRobotState());
getIK(true, nh, km, goals[config], sp, names, x, y, z);
}
}
@@ -692,7 +693,7 @@
std::string fnm = getenv("HOME") + ("/states/" + state);
std::ifstream in(fnm.c_str());
- planning_models::StateParams sp(*km.getRobotState());
+ planning_models::KinematicState sp(*km.getRobotState());
std::vector<double> params;
while (in.good() && !in.eof())
{
Modified: pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp 2009-09-02 21:42:16 UTC (rev 23711)
+++ pkg/trunk/highlevel/move_arm/src/move_arm_action.cpp 2009-09-02 21:46:41 UTC (rev 23712)
@@ -104,9 +104,9 @@
/// A state with a cost attached to it. Used internally to sort states by cost
struct CostState
{
- boost::shared_ptr<planning_models::StateParams> state;
- double cost;
- unsigned int index;
+ boost::shared_ptr<planning_models::KinematicState> state;
+ double cost;
+ unsigned int index;
};
/// Ordering function for states with cost attached
@@ -135,7 +135,7 @@
/// Representation of a state and its corresponding joint constraints
struct StateAndConstraint
{
- boost::shared_ptr<planning_models::StateParams> state;
+ boost::shared_ptr<planning_models::KinematicState> state;
std::vector<motion_planning_msgs::JointConstraint> constraints;
};
@@ -186,7 +186,7 @@
}
/** \brief Evaluate the cost of a state, in terms of collisions */
- double computeStateCollisionCost(const planning_models::StateParams *sp)
+ double computeStateCollisionCost(const planning_models::KinematicState *sp)
{
CollisionCost ccost;
@@ -207,7 +207,7 @@
bool findValidNearJointConstraint(const std::vector<motion_planning_msgs::KinematicJoint> &start_state,
const motion_planning_msgs::KinematicSpaceParameters ¶ms,
const motion_planning_msgs::KinematicConstraints &constraints,
- const std::vector< boost::shared_ptr<planning_models::StateParams> > &hint_states,
+ const std::vector< boost::shared_ptr<planning_models::KinematicState> > &hint_states,
StateAndConstraint &sac)
{
bool result = false;
@@ -233,7 +233,7 @@
if (!c_res.joint_constraint.empty())
{
// construct a state representation from our found joint constraints
- sac.state.reset(new planning_models::StateParams(*planningMonitor_->getRobotState()));
+ sac.state.reset(new planning_models::KinematicState(*planningMonitor_->getRobotState()));
for (unsigned int i = 0 ; i < c_res.joint_constraint.size() ; ++i)
{
@@ -252,7 +252,7 @@
}
/** \brief If we have a complex goal for which we have not yet found a valid goal state, we use this function*/
- ArmActionState solveGoalComplex(std::vector< boost::shared_ptr<planning_models::StateParams> > &states,
+ ArmActionState solveGoalComplex(std::vector< boost::shared_ptr<planning_models::KinematicState> > &states,
motion_planning_msgs::GetMotionPlan::Request &req)
{
ROS_DEBUG("Acting on goal with unknown valid goal state ...");
@@ -317,7 +317,7 @@
ROS_DEBUG("Acting on goal to set of joints ...");
// construct a state representation from our goal joint
- boost::shared_ptr<planning_models::StateParams> sp(new planning_models::StateParams(*planningMonitor_->getRobotState()));
+ boost::shared_ptr<planning_models::KinematicState> sp(new planning_models::KinematicState(*planningMonitor_->getRobotState()));
for (unsigned int i = 0 ; i < req.goal_constraints.joint_constraint.size() ; ++i)
{
@@ -332,13 +332,13 @@
else
{
// if we can't, go to the more generic joint solver
- std::vector< boost::shared_ptr<planning_models::StateParams> > states;
+ std::vector< boost::shared_ptr<planning_models::KinematicState> > states;
states.push_back(sp);
return solveGoalJoints(states, req);
}
}
- void updateRequest(motion_planning_msgs::GetMotionPlan::Request &req, const planning_models::StateParams *sp)
+ void updateRequest(motion_planning_msgs::GetMotionPlan::Request &req, const planning_models::KinematicState *sp)
{
// update request
for (unsigned int i = 0 ; i < setup_.groupJointNames_.size() ; ++i)
@@ -359,7 +359,7 @@
}
/** \brief Find a plan to given request, given a set of hint states in the goal region */
- ArmActionState solveGoalJoints(std::vector< boost::shared_ptr<planning_models::StateParams> > &states,
+ ArmActionState solveGoalJoints(std::vector< boost::shared_ptr<planning_models::KinematicState> > &states,
motion_planning_msgs::GetMotionPlan::Request &req)
{
ROS_DEBUG("Acting on goal to set of joints pointing to potentially invalid state ...");
@@ -391,7 +391,7 @@
else
{
// order the states by cost before passing them forward
- std::vector< boost::shared_ptr<planning_models::StateParams> > backup = states;
+ std::vector< boost::shared_ptr<planning_models::KinematicState> > backup = states;
for (unsigned int i = 0 ; i < cstates.size() ; ++i)
states[i] = backup[cstates[i].index];
return solveGoalComplex(states, req);
@@ -414,7 +414,7 @@
// we do IK to find corresponding states
ros::ServiceClient ik_client = nh_.serviceClient<manipulation_srvs::IKService>(ARM_IK_NAME, true);
- std::vector< boost::shared_ptr<planning_models::StateParams> > states;
+ std::vector< boost::shared_ptr<planning_models::KinematicState> > states;
// find an IK solution
for (int step = 0 ; step < 10 ; ++step)
@@ -436,7 +436,7 @@
if (computeIK(ik_client, tpose, solution))
{
// check if it is a valid state
- boost::shared_ptr<planning_models::StateParams> spTest(new planning_models::StateParams(*planningMonitor_->getRobotState()));
+ boost::shared_ptr<planning_models::KinematicState> spTest(new planning_models::KinematicState(*planningMonitor_->getRobotState()));
spTest->setParamsJoints(solution, setup_.groupJointNames_);
spTest->enforceBounds();
@@ -472,17 +472,17 @@
return solveGoalJoints(req);
// otherwise, more complex constraints, run a generic method; we have no hint states
- std::vector< boost::shared_ptr<planning_models::StateParams> > states;
+ std::vector< boost::shared_ptr<planning_models::KinematicState> > states;
return solveGoalComplex(states, req);
}
ArmActionState runLRplanner(motion_planning_msgs::GetMotionPlan::Request &req)
{
- std::vector< boost::shared_ptr<planning_models::StateParams> > states;
+ std::vector< boost::shared_ptr<planning_models::KinematicState> > states;
return runLRplanner(states, req);
}
- ArmActionState runLRplanner(std::vector< boost::shared_ptr<planning_models::StateParams> > &states,
+ ArmActionState runLRplanner(std::vector< boost::shared_ptr<planning_models::KinematicState> > &states,
motion_planning_msgs::GetMotionPlan::Request &req)
{
ROS_DEBUG("Running long range planner...");
@@ -507,11 +507,11 @@
ArmActionState runSRplanner(motion_planning_msgs::GetMotionPlan::Request &req)
{
- std::vector< boost::shared_ptr<planning_models::StateParams> > states;
+ std::vector< boost::shared_ptr<planning_models::KinematicState> > states;
return runSRplanner(states, req);
}
- ArmActionState runSRplanner(std::vector< boost::shared_ptr<planning_models::StateParams> > &states,
+ ArmActionState runSRplanner(std::vector< boost::shared_ptr<planning_models::KinematicState> > &states,
motion_planning_msgs::GetMotionPlan::Request &req)
{
ROS_DEBUG("Running short range planner...");
@@ -803,7 +803,7 @@
ROS_DEBUG("Trying to use short range planner to move to a valid state...");
// construct a state representation + kinematic constraints from our start state
- boost::shared_ptr<planning_models::StateParams> sp(new planning_models::StateParams(*planningMonitor_->getRobotState()));
+ boost::shared_ptr<planning_models::KinematicState> sp(new planning_models::KinematicState(*planningMonitor_->getRobotState()));
motion_planning_msgs::KinematicConstraints constraints;
for (unsigned int i = 0 ; i < req.start_state.size() ; ++i)
@@ -819,7 +819,7 @@
jc.tolerance_below.resize(jc.value.size(), 0.0);
constraints.joint_constraint.push_back(jc);
}
- std::vector< boost::shared_ptr<planning_models::StateParams> > states;
+ std::vector< boost::shared_ptr<planning_models::KinematicState> > states;
states.push_back(sp);
// find valid state near by
@@ -941,7 +941,7 @@
}
// add the actual path
- planning_models::StateParams *sp = planningMonitor_->getKinematicModel()->newStateParams();
+ planning_models::KinematicState *sp = new planning_models::KinematicState(planningMonitor_->getKinematicModel());
for (unsigned int i = 0 ; i < path.states.size() ; ++i)
{
traj.points[i + includeFirst].time = offset + path.times[i];
@@ -958,11 +958,11 @@
std::stringstream ss;
for (unsigned int j = 0 ; j < path.states[i].vals.size() ; ++j)
ss << path.states[i].vals[j] << " ";
- ROS_DEBUG(ss.str().c_str());
+ ROS_DEBUG("%s", ss.str().c_str());
}
}
- bool fixStartState(planning_models::StateParams &st)
+ bool fixStartState(planning_models::KinematicState &st)
{
bool result = true;
@@ -973,7 +973,7 @@
if (!planningMonitor_->isStateValidOnPath(&st))
{
// try 2% change in each component
- planning_models::StateParams temp(st);
+ planning_models::KinematicState temp(st);
int count = 0;
do
{
@@ -993,14 +993,14 @@
bool fillStartState(std::vector<motion_planning_msgs::KinematicJoint> &start_state)
{
// get the current state
- planning_models::StateParams st(*planningMonitor_->getRobotState());
+ planning_models::KinematicState st(*planningMonitor_->getRobotState());
bool result = fixStartState(st);
if (!result)
ROS_DEBUG("Starting state for the robot is in collision and attempting to fix it failed.");
// fill in start state with current one
- std::vector<planning_models::KinematicModel::Joint*> joints;
+ std::vector<const planning_models::KinematicModel::Joint*> joints;
planningMonitor_->getKinematicModel()->getJoints(joints);
start_state.resize(joints.size());
@@ -1024,7 +1024,7 @@
request.data.pose_stamped = pose_stamped_msg;
request.data.joint_names = setup_.groupJointNames_;
- planning_models::StateParams *sp = planningMonitor_->getKinematicModel()->newStateParams();
+ planning_models::KinematicState *sp = new planning_models::KinematicState(planningMonitor_->getKinematicModel());
sp->randomStateGroup(setup_.group_);
for(unsigned int i = 0; i < setup_.groupJointNames_.size() ; ++i)
{
Modified: pkg/trunk/highlevel/move_arm/src/move_arm_setup.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/move_arm_setup.cpp 2009-09-02 21:42:16 UTC (rev 23711)
+++ pkg/trunk/highlevel/move_arm/src/move_arm_setup.cpp 2009-09-02 21:46:41 UTC (rev 23712)
@@ -55,7 +55,7 @@
if (!collisionModels_->loadedModels())
return false;
- if (collisionModels_->getKinematicModel()->getGroupID(group_) < 0)
+ if (!collisionModels_->getKinematicModel()->hasGroup(group_))
{
ROS_ERROR("Group '%s' is not known", group_.c_str());
return false;
@@ -99,28 +99,38 @@
}
joint_names = res_query.jointnames;
-
+
+ planning_models::KinematicModel::JointGroup *jg = planningMonitor_->getKinematicModel()->getGroup(group_);
+
// 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]))
{
+ if (!jg->hasJoint(j->name))
+ {
+ ROS_ERROR("Joint '%s' is not in group '%s'", j->name.c_str(), group_.c_str());
+ return false;
+ }
ROS_DEBUG("Using joing '%s' with %u parameters", j->name.c_str(), j->usedParams);
- if (planningMonitor_->getKinematicModel()->getJointIndexInGroup(j->name, group_) < 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, group_);
- if (groupNames.size() != joint_names.size())
+ if (jg->jointNames.size() != joint_names.size())
{
ROS_ERROR("The group '%s' does not have the same number of joints as the controller can handle", group_.c_str());
+ std::stringstream ss1;
+ for (unsigned int i = 0 ; i < jg->jointNames.size() ; ++i)
+ ss1 << jg->jointNames[i] << " ";
+ ROS_ERROR("Group '%s': %s", group_.c_str(), ss1.str().c_str());
+
+ std::stringstream ss2;
+ for (unsigned int i = 0 ; i < joint_names.size() ; ++i)
+ ss2 << joint_names[i] << " ";
+ ROS_ERROR("Controller joints: %s", ss2.str().c_str());
+
return false;
}
Modified: pkg/trunk/highlevel/move_arm/src/record_path.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/record_path.cpp 2009-09-02 21:42:16 UTC (rev 23711)
+++ pkg/trunk/highlevel/move_arm/src/record_path.cpp 2009-09-02 21:46:41 UTC (rev 23712)
@@ -57,16 +57,16 @@
void printJoints(const planning_environment::KinematicModelStateMonitor &km, const std::vector<std::string> &names)
{
- const planning_models::KinematicModel::ModelInfo &mi = km.getKinematicModel()->getModelInfo();
-
+ const std::vector<double> &bounds = km.getKinematicModel()->getStateBounds();
+
for (unsigned int i = 0 ; i < names.size(); ++i)
{
- int idx = km.getKinematicModel()->getJointIndex(names[i]);
- std::cout << " " << i << " = " << names[i] << " [" << mi.stateBounds[idx * 2] << ", " << mi.stateBounds[idx * 2 + 1] << "]" << std::endl;
+ int idx = km.getKinematicModel()->getJoint(names[i])->stateIndex;
+ std::cout << " " << i << " = " << names[i] << " [" << bounds[idx * 2] << ", " << bounds[idx * 2 + 1] << "]" << std::endl;
}
}
-void recordThread(planning_environment::KinematicModelStateMonitor *km, bool *stop, std::vector<planning_models::StateParams> *states)
+void recordThread(planning_environment::KinematicModelStateMonitor *km, bool *stop, std::vector<planning_models::KinematicState> *states)
{
while (*stop == false)
{
@@ -121,7 +121,7 @@
std::cout << "Hit <Enter> to stop recording... ";
bool stop = false;
- std::vector<planning_models::StateParams> states;
+ std::vector<planning_models::KinematicState> states;
boost::thread rec(boost::bind(recordThread, &km, &stop, &states));
std::string dummy;
std::getline(std::cin, dummy);
Modified: pkg/trunk/highlevel/move_arm/src/record_state.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/record_state.cpp 2009-09-02 21:42:16 UTC (rev 23711)
+++ pkg/trunk/highlevel/move_arm/src/record_state.cpp 2009-09-02 21:46:41 UTC (rev 23712)
@@ -57,16 +57,15 @@
void printJoints(const planning_environment::KinematicModelStateMonitor &km, const std::vector<std::string> &names)
{
- const planning_models::KinematicModel::ModelInfo &mi = km.getKinematicModel()->getModelInfo();
-
+ const std::vector<double> &bounds = km.getKinematicModel()->getStateBounds();
+
for (unsigned int i = 0 ; i < names.size(); ++i)
{
- int idx = km.getKinematicModel()->getJointIndex(names[i]);
- std::cout << " " << i << " = " << names[i] << " [" << mi.stateBounds[idx * 2] << ", " << mi.stateBounds[idx * 2 + 1] << "]" << std::endl;
+ int idx = km.getKinematicModel()->getJoint(names[i])->stateIndex;
+ std::cout << " " << i << " = " << names[i] << " [" << bounds[idx * 2] << ", " << bounds[idx * 2 + 1] << "]" << std::endl;
}
}
-
int main(int argc, char **argv)
{
ros::init(argc, argv, "record_state", ros::init_options::NoSigintHandler | ros::init_options::AnonymousName);
Modified: pkg/trunk/highlevel/move_arm/src/teleop_arm.cpp
===================================================================
--- pkg/trunk/highlevel/move_arm/src/teleop_arm.cpp 2009-09-02 21:42:16 UTC (rev 23711)
+++ pkg/trunk/highlevel/move_arm/src/teleop_arm.cpp 2009-09-02 21:46:41 UTC (rev 23712)
@@ -73,15 +73,15 @@
private:
- bool tryTwist(const geometry_msgs::Twist &tw, double frac)
- {
- bool res = true;
- planning_models::KinematicModel *kmodel = planningMonitor_->getKinematicModel()->clone();
+ bool tryTwist(const geometry_msgs::Twist &tw, double frac)
+ {
+ bool res = true;
+ planning_models::KinematicModel *kmodel = new planning_models::KinematicModel(*planningMonitor_->getKinematicModel());
- boost::shared_ptr<planning_models::StateParams> start(new planning_models::StateParams(*planningMonitor_->getRobotState()));
+ boost::shared_ptr<planning_models::KinematicState> start(new planning_models::KinematicState(*planningMonitor_->getRobotState()));
start->enforceBounds();
kmodel->computeTransforms(start->getParams());
-
+
tf::Pose currentEff = kmodel->getJoint(setup_.groupJointNames_.back())->after->globalTrans;
geometry_msgs::Pose currentEffMsg;
tf::poseTFToMsg(currentEff, currentEffMsg);
@@ -100,7 +100,7 @@
if (computeIK(ik_client, destEffMsgStmp, solution))
{
ROS_INFO("Starting at %f, %f, %f", currentEff.getOrigin().x(), currentEff.getOrigin().y(), currentEff.getOrigin().z());
- boost::shared_ptr<planning_models::StateParams> goal(new planning_models::StateParams(*start));
+ boost::shared_ptr<planning_models::KinematicState> goal(new planning_models::KinematicState(*start));
goal->setParamsJoints(solution, setup_.groupJointNames_);
goal->enforceBounds();
@@ -108,7 +108,7 @@
tf::Pose goalEff = kmodel->getJoint(setup_.groupJointNames_.back())->after->globalTrans;
ROS_INFO("Going to %f, %f, %f", goalEff.getOrigin().x(), goalEff.getOrigin().y(), goalEff.getOrigin().z());
- std::vector< boost::shared_ptr<planning_models::StateParams> > path;
+ std::vector< boost::shared_ptr<planning_models::KinematicState> > path;
interpolatePath(start, goal, 20, path);
ROS_INFO("Generated path with %d states", (int)path.size());
@@ -213,11 +213,11 @@
vizPub_.publish(marker);
}
- void interpolatePath(boost::shared_ptr<planning_models::StateParams> &start, boost::shared_ptr<planning_models::StateParams> &goal, unsigned int count,
- std::vector< boost::shared_ptr<planning_models::StateParams> > &path)
+ void interpolatePath(boost::shared_ptr<planning_models::KinematicState> &start, boost::shared_ptr<planning_models::KinematicState> &goal, unsigned int count,
+ std::vector< boost::shared_ptr<planning_models::KinematicState> > &path)
{
path.clear();
- unsigned int dim = planningMonitor_->getKinematicModel()->getModelInfo().stateDimension;
+ unsigned int dim = planningMonitor_->getKinematicModel()->getDimension();
std::vector<double> startv;
start->copyParams(startv);
std::vector<double> goalv;
@@ -230,7 +230,7 @@
path.push_back(start);
for (unsigned int i = 1 ; i < count ; ++i)
{
- boost::shared_ptr<planning_models::StateParams> sp(planningMonitor_->getKinematicModel()->newStateParams());
+ boost::shared_ptr<planning_models::KinematicState> sp(new planning_models::KinematicState(planningMonitor_->getKinematicModel()));
std::vector<double> v(dim);
for (unsigned int j = 0 ; j < dim ; ++j)
v[j] = startv[j] + inc[j] * i;
@@ -240,7 +240,7 @@
path.push_back(goal);
}
- unsigned int findFirstInvalid(std::vector< boost::shared_ptr<planning_models::StateParams> > &path)
+ unsigned int findFirstInvalid(std::vector< boost::shared_ptr<planning_models::KinematicState> > &path)
{
for (unsigned int i = 0 ; i < path.size() ; ++i)
if (!planningMonitor_->isStateValid(path[i].get(), planning_environment::PlanningMonitor::COLLISION_TEST))
@@ -248,7 +248,7 @@
return path.size();
}
- void executePath(std::vector< boost::shared_ptr<planning_models::StateParams> > &path)
+ void executePath(std::vector< boost::shared_ptr<planning_models::KinematicState> > &path)
{
manipulation_msgs::JointTraj traj;
traj.names = setup_.groupJointNames_;
@@ -287,7 +287,7 @@
request.data.pose_stamped = pose_stamped_msg;
request.data.joint_names = setup_.groupJointNames_;
- planning_models::StateParams *sp = planningMonitor_->getKinematicModel()->newStateParams();
+ planning_models::KinematicState *sp = new planning_models::KinematicState(planningMonitor_->getKinematicModel());
sp->defaultState();
for(unsigned int i = 0; i < setup_.groupJointNames_.size() ; ++i)
{
Modified: pkg/trunk/motion_planning/motion_planning_rviz_plugin/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/motion_planning_rviz_plugin/manifest.xml 2009-09-02 21:42:16 UTC (rev 23711)
+++ pkg/trunk/motion_planning/motion_planning_rviz_plugin/manifest.xml 2009-09-02 21:46:41 UTC (rev 23712)
@@ -11,6 +11,7 @@
<depend package="rviz"/>
<depend package="roscpp"/>
<depend package="tf"/>
+ <depend package="planning_models"/>
<depend package="planning_environment"/>
<depend package="motion_planning_msgs"/>
Modified: pkg/trunk/motion_planning/motion_planning_rviz_plugin/src/planning_display.cpp
===================================================================
--- pkg/trunk/motion_planning/motion_planning_rviz_plugin/src/planning_display.cpp 2009-09-02 21:42:16 UTC (rev 23711)
+++ pkg/trunk/motion_planning/motion_planning_rviz_plugin/src/planning_display.cpp 2009-09-02 21:46:41 UTC (rev 23712)
@@ -41,7 +41,7 @@
#include <tf/transform_listener.h>
#include <planning_environment/models/robot_models.h>
-
+#include <planning_models/kinematic_state.h>
#include <OGRE/OgreSceneNode.h>
#include <OGRE/OgreSceneManager.h>
@@ -60,7 +60,7 @@
{
apply_offset_transforms = false;
- planning_models::KinematicModel::Link* link = kinematic_model_->getLink( link_name );
+ const planning_models::KinematicModel::Link* link = kinematic_model_->getLink( link_name );
if ( !link )
{
@@ -271,7 +271,7 @@
current_state_ = -1;
current_state_time_ = state_display_time_ + 1.0f;
- planning_models::StateParams *sp = kinematic_model_->newStateParams();
+ planning_models::KinematicState *sp = new planning_models::KinematicState(kinematic_model_);
for (unsigned int i = 0; i < displaying_kinematic_path_message_->start_state.size(); ++i)
if (displaying_kinematic_path_message_->start_state[i].header.frame_id == displaying_kinematic_path_message_->header.frame_id)
sp->setParamsJoint(displaying_kinematic_path_message_->start_state[i].value, displaying_kinematic_path_message_->start_state[i].joint_name);
@@ -294,12 +294,12 @@
if ((size_t) current_state_ < displaying_kinematic_path_message_->get_states_size())
{
- int group_id = kinematic_model_->getGroupID(displaying_kinematic_path_message_->model_id);
- if (group_id >= 0)
+ planning_models::KinematicModel::JointGroup *jg = kinematic_model_->getGroup(displaying_kinematic_path_message_->model_id);
+ if (jg)
{
- unsigned int dim = kinematic_model_->getGroupDimension(group_id);
- if (displaying_kinematic_path_message_->states[current_state_].vals.size() == dim)
- kinematic_model_->computeTransformsGroup(&displaying_kinematic_path_message_->states[current_state_].vals[0], group_id);
+ unsigned int dim = jg->dimension;
+ if (displaying_kinematic_path_message_->states[current_state_].vals.size() == dim)
+ jg->computeTransforms(&displaying_kinematic_path_message_->states[current_state_].vals[0]);
robot_->update(PlanningLinkUpdater(kinematic_model_));
}
causeRender();
Modified: pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-09-02 21:42:16 UTC (rev 23711)
+++ pkg/trunk/motion_planning/ompl_planning/src/motion_planner.cpp 2009-09-02 21:46:41 UTC (rev 23712)
@@ -109,9 +109,9 @@
private:
- planning_models::StateParams* fillStartState(const std::vector<motion_planning_msgs::KinematicJoint> &given)
+ planning_models::KinematicState* fillStartState(const std::vector<motion_planning_msgs::KinematicJoint> &given)
{
- planning_models::StateParams *s = planningMonitor_->getKinematicModel()->newStateParams();
+ planning_models::KinematicState *s = new planning_models::KinematicState(planningMonitor_->getKinematicModel());
for (unsigned int i = 0 ; i < given.size() ; ++i)
{
if (!planningMonitor_->getTransformListener()->frameExists(given[i].header.frame_id))
@@ -131,7 +131,7 @@
if (planningMonitor_->haveState())
{
ROS_INFO("Using the current state to fill in the starting state for the motion plan");
- std::vector<planning_models::KinematicModel::Joint*> joints;
+ std::vector<const planning_models::KinematicModel::Joint*> joints;
planningMonitor_->getKinematicModel()->getJoints(joints);
for (unsigned int i = 0 ; i < joints.size() ; ++i)
if (!s->seenJoint(joints[i]->name))
@@ -158,7 +158,7 @@
res.distance = -1.0;
res.approximate = 0;
- planning_models::StateParams *startState = fillStartState(req.start_state);
+ planning_models::KinematicState *startState = fillStartState(req.start_state);
if (startState)
{
Modified: pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.cpp 2009-09-02 21:42:16 UTC (rev 23711)
+++ pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.cpp 2009-09-02 21:46:41 UTC (rev 23712)
@@ -169,7 +169,7 @@
ROS_DEBUG("No workspace bounding volume was set");
}
-void ompl_planning::RequestHandler::configure(const planning_models::StateParams *startState, motion_planning_msgs::GetMotionPlan::Request &req, PlannerSetup *psetup)
+void ompl_planning::RequestHandler::configure(const planning_models::KinematicState *startState, motion_planning_msgs::GetMotionPlan::Request &req, PlannerSetup *psetup)
{
/* clear memory */
psetup->ompl_model->si->clearGoal(); // goal definitions
@@ -191,20 +191,14 @@
const unsigned int dim = psetup->ompl_model->si->getStateDimension();
ompl::base::State *start = new ompl::base::State(dim);
- if (psetup->ompl_model->groupID >= 0)
- {
- /* set the pose of the whole robot */
- ompl_ros::EnvironmentDescription *ed = psetup->ompl_model->getEnvironmentDescription();
- ed->kmodel->computeTransforms(startState->getParams());
- ed->collisionSpace->updateRobotModel();
-
- /* extract the components needed for the start state of the desired group */
- startState->copyParamsGroup(start->values, psetup->ompl_model->groupID);
- }
- else
- /* the start state is the complete state */
- startState->copyParams(start->values);
+ /* set the pose of the whole robot */
+ ompl_ros::EnvironmentDescription *ed = psetup->ompl_model->getEnvironmentDescription();
+ ed->kmodel->computeTransforms(startState->getParams());
+ ed->collisionSpace->updateRobotModel();
+ /* extract the components needed for the start state of the desired group */
+ startState->copyParamsGroup(start->values, psetup->ompl_model->group);
+
psetup->ompl_model->si->addStartState(start);
/* add goal state */
@@ -255,7 +249,7 @@
return result;
}
-bool ompl_planning::RequestHandler::computePlan(ModelMap &models, const planning_models::StateParams *start, double stateDelay,
+bool ompl_planning::RequestHandler::computePlan(ModelMap &models, const planning_models::KinematicState *start, double stateDelay,
motion_planning_msgs::GetMotionPlan::Request &req, motion_planning_msgs::GetMotionPlan::Response &res)
{
if (!isRequestValid(models, req))
@@ -311,10 +305,10 @@
return true;
}
-void ompl_planning::RequestHandler::fillResult(PlannerSetup *psetup, const planning_models::StateParams *start, double stateDelay,
+void ompl_planning::RequestHandler::fillResult(PlannerSetup *psetup, const planning_models::KinematicState *start, double stateDelay,
motion_planning_msgs::GetMotionPlan::Response &res, const Solution &sol)
{
- std::vector<planning_models::KinematicModel::Joint*> joints;
+ std::vector<const planning_models::KinematicModel::Joint*> joints;
psetup->ompl_model->planningMonitor->getKinematicModel()->getJoints(joints);
res.path.start_state.resize(joints.size());
for (unsigned int i = 0 ; i < joints.size() ; ++i)
@@ -329,8 +323,7 @@
{
res.path.states.resize(kpath->states.size());
res.path.times.resize(kpath->states.size());
- res.path.names.clear();
- psetup->ompl_model->planningMonitor->getKinematicModel()->getJointsInGroup(res.path.names, psetup->ompl_model->groupID);
+ res.path.names = psetup->ompl_model->group->jointNames;
const unsigned int dim = psetup->ompl_model->si->getStateDimension();
for (unsigned int i = 0 ; i < kpath->states.size() ; ++i)
@@ -347,8 +340,7 @@
{
res.path.states.resize(dpath->states.size());
res.path.times.resize(dpath->states.size());
- res.path.names.clear();
- psetup->ompl_model->planningMonitor->getKinematicModel()->getJointsInGroup(res.path.names, psetup->ompl_model->groupID);
+ res.path.names = psetup->ompl_model->group->jointNames;
const unsigned int dim = psetup->ompl_model->si->getStateDimension();
for (unsigned int i = 0 ; i < dpath->states.size() ; ++i)
Modified: pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.h
===================================================================
--- pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.h 2009-09-02 21:42:16 UTC (rev 23711)
+++ pkg/trunk/motion_planning/ompl_planning/src/request_handler/RequestHandler.h 2009-09-02 21:46:41 UTC (rev 23712)
@@ -67,7 +67,7 @@
bool isRequestValid(ModelMap &models, motion_planning_msgs::GetMotionPlan::Request &req);
/** \brief Check and compute a motion plan. Return true if the plan was succesfully computed */
- bool computePlan(ModelMap &models, const planning_models::StateParams *start, double stateDelay,
+ bool computePlan(ModelMap &models, const planning_models::KinematicState *start, double stateDelay,
motion_planning_msgs::GetMotionPlan::Request &req, motion_planning_msgs::GetMotionPlan::Response &res);
/** \brief Enable callback for when a motion plan computation is completed */
@@ -83,7 +83,7 @@
};
/** \brief Set up all the data needed by motion planning based on a request */
- void configure(const planning_models::StateParams *startState, motion_planning_msgs::GetMotionPlan::Request &req, PlannerSetup *psetup);
+ void configure(const planning_models::KinematicState *startState, motion_planning_msgs::GetMotionPlan::Request &req, PlannerSetup *psetup);
/** \brief Compute the actual motion plan. Return true if computed plan was trivial (start state already in goal region) */
bool callPlanner(PlannerSetup *psetup, int times, double allowed_time, Solution &sol);
@@ -92,7 +92,7 @@
void setWorkspaceBounds(motion_planning_msgs::KinematicSpaceParameters ¶ms, ompl_ros::ModelBase *ompl_model);
/** \brief Fill the response with solution data */
- void fillResult(PlannerSetup *psetup, const planning_models::StateParams *start, double stateDelay,
+ void fillResult(PlannerSetup *psetup, const planning_models::KinematicState *start, double stateDelay,
motion_planning_msgs::GetMotionPlan::Response &res, const Solution &sol);
/** \brief Fix the input states, if they are not valid */
Modified: pkg/trunk/motion_planning/ompl_ros/include/ompl_ros/ModelBase.h
===================================================================
--- pkg/trunk/motion_planning/ompl_ros/include/ompl_ros/ModelBase.h 2009-09-02 21:42:16 UTC (rev 23711)
+++ pkg/trunk/motion_planning/ompl_ros/include/ompl_ros/ModelBase.h 2009-09-02 21:46:41 UTC (rev 23712)
@@ -53,6 +53,9 @@
{
collision_space::EnvironmentModel *collisionSpace;
planning_models::KinematicModel *kmodel;
+
+ /** \brief The group instance */
+ planning_models::KinematicModel::JointGroup *group;
const planning_environment::KinematicConstraintEvaluatorSet *constraintEvaluator;
};
@@ -79,10 +82,10 @@
/** \brief The group name */
std::string groupName;
-
- /** \brief The group ID */
- int groupID;
+ /** \brief The group instance */
+ planning_models::KinematicModel::JointGroup *group;
+
/** \brief The instance of the space information maintained for this group. si->setup() will need to be called after configure() */
ompl::base::SpaceInformation *si;
std::map<std::string, ompl::base::StateDistanceEvaluator*> sde; // list of available distance evaluators
Modified: pkg/trunk/motion_planning/ompl_ros/include/ompl_ros/dynamic/SpaceInformation.h
===================================================================
--- pkg/trunk/motion_planning/ompl_ros/include/ompl_ros/dynamic/SpaceInformation.h 2009-09-02 21:42:16 UTC (rev 23711)
+++ pkg/trunk/motion_planning/ompl_ros/include/ompl_ros/dynamic/SpaceInformation.h 2009-09-02 21:46:41 UTC (rev 23712)
@@ -85,7 +85,7 @@
ForwardPropagationModel *propagationModel_;
std::vector<ompl::base::StateComponent> basicStateComponent_;
planning_models::KinematicModel *kmodel_;
- int groupID_;
+ std::string groupName_;
std::vector<int> floatingJoints_;
std::vector<int> planarJoints_;
};
Modified: pkg/trunk/motion_planning/ompl_ros/include/ompl_ros/dynamic/StateValidator.h
===================================================================
--- pkg/trunk/motion_planning/ompl_ros/include/ompl_ros/dynamic/StateValidator.h 2009-09-02 21:42:16 UTC (rev 23711)
+++ pkg/trunk/motion_planning/ompl_ros/include/ompl_ros/dynamic/StateValidator.h 2009-09-02 21:46:41 UTC (rev 23712)
@@ -73,7 +73,7 @@
protected:
- bool check(const ompl::base::State *s, collision_space::EnvironmentModel *em, planning_models::KinematicModel *km,
+ bool check(const ompl::base::State *s, collision_space::EnvironmentModel *em, planning_models::KinematicModel::JointGroup *jg,
const planning_environment::KinematicConstraintEvaluatorSet *kce) const;
ModelBase *model_;
Modified: pkg/trunk/motion_planning/ompl_ros/include/ompl_ros/kinematic/SpaceInformation.h
===================================================================
--- pkg/trunk/motion_planning/ompl_ros/include/ompl_ros/kinematic/SpaceInformation.h 2009-09-02 21:42:16 UTC (rev 23711)
+++ pkg/trunk/motion_planning/ompl_ros/include/ompl_ros/kinematic/SpaceInformation.h 2009-09-02 21:46:41 UTC (rev 23712)
@@ -87,7 +87,7 @@
std::vector<ompl::base::StateComponent> basicStateComponent_;
planning_models::KinematicModel *kmodel_;
- int groupID_;
+ std::string groupName_;
std::vector<int> floatingJoints_;
std::vector<int> planarJoints_;
double divisions_;
Modified: pkg/trunk/motion_planning/ompl_ros/include/ompl_ros/kinematic/StateValidator.h
===================================================================
--- pkg/trunk/motion_planning/ompl_ros/include/ompl_ros/kinematic/StateValidator.h 2009-09-02 21:42:16 UTC (rev 23711)
+++ pkg/trunk/motion_planning/ompl_ros/include/ompl_ros/kinematic/StateValidator.h 2009-09-02 21:46:41 UTC (rev 23712)
@@ -73,7 +73,7 @@
protected:
- bool check(const ompl::base::State *s, collision_space::EnvironmentModel *em, planning_models::KinematicModel *km,
+ bool check(const ompl::base::State *s, collision_space::EnvironmentModel *em, planning_models::KinematicModel::JointGroup *jg,
const planning_environment::KinematicConstraintEvaluatorSet *kce) const;
ModelBase *model_;
Modified: pkg/trunk/motion_planning/ompl_ros/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/ompl_ros/manifest.xml 2009-09-02 21:42:16 UTC (rev 23711)
+++ pkg/trunk/motion_planning/ompl_ros/manifest.xml 2009-09-02 21:46:41 UTC (rev 23712)
@@ -7,6 +7,8 @@
<depend package="rosconsole" />
<depend package="motion_planning_msgs" />
<depend package="planning_environment"/>
+ <depend package="collision_space"/>
+ <depend package="planning_models"/>
<depend package="ompl" />
<export>
Modified: pkg/trunk/motion_planning/ompl_ros/src/ModelBase.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_ros/src/ModelBase.cpp 2009-09-02 21:42:16 UTC (rev 23711)
+++ pkg/trunk/motion_planning/ompl_ros/src/ModelBase.cpp 2009-09-02 21:46:41 UTC (rev 23712)
@@ -49,7 +49,7 @@
si = NULL;
groupName = gName;
planningMonitor = pMonitor;
- groupID = planningMonitor->getKinematicModel()->getGroupID(groupName);
+ group = planningMonitor->getKinematicModel()->getGroup(groupName);
}
ompl_ros::ModelBase::~ModelBase(void)
@@ -79,6 +79,7 @@
result->collisionSpace = planningMonitor->getEnvironmentModel();
result->kmodel = result->collisionSpace->getRobotModel().get();
result->constraintEvaluator = &constraintEvaluator;
+ result->group = group;
}
else
{
@@ -86,6 +87,7 @@
result = new EnvironmentDescription();
result->collisionSpace = planningMonitor->getEnvironmentModel()->clone();
result->kmodel = result->collisionSpace->getRobotModel().get();
+ result->group = result->kmodel->getGroup(groupName);
planning_environment::KinematicConstraintEvaluatorSet *kce = new planning_environment::KinematicConstraintEvaluatorSet();
kce->add(result->kmodel, constraintEvaluator.getPoseConstraints());
kce->add(result->kmodel, constraintEvaluator.getJointConstraints());
Modified: pkg/trunk/motion_planning/ompl_ros/src/base/GoalDefinitions.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_ros/src/base/GoalDefinitions.cpp 2009-09-02 21:42:16 UTC (rev 23711)
+++ pkg/trunk/motion_planning/ompl_ros/src/base/GoalDefinitions.cpp 2009-09-02 21:46:41 UTC (rev 23712)
@@ -83,10 +83,11 @@
for (unsigned int i = 0 ; i < jc.size() ; ++i)
{
// get the index at which the joint parameters start
- int idx = model->planningMonitor->getKinematicModel()->getJointIndexInGroup(jc[i].joint_name, model->groupID);
- if (idx >= 0)
+ const planning_models::KinematicModel::Joint *jnt = model->planningMonitor->getKinematicModel()->getJoint(jc[i].joint_name);
+ if (jnt)
{
- unsigned int usedParams = model->planningMonitor->getKinematicModel()->getJoint(jc[i].joint_name)->usedParams;
+ unsigned int usedParams = jnt->usedParams;
+ int idx = model->group->getJointPosition(jnt->name);
if (jc[i].tolerance_above.size() != jc[i].tolerance_below.size() || jc[i].value.size() != jc[i].tolerance_below.size() || jc[i].value.size() != usedParams)
ROS_ERROR("Constraint on joint %s has incorrect number of parameters. Expected %u.", jc[i].joint_name.c_str(), usedParams);
@@ -169,7 +170,7 @@
double ompl_ros::GoalToPosition::evaluateGoalAux(const ompl::base::State *state, std::vector<bool> *decision) const
{
EnvironmentDescription *ed = model_->getEnvironmentDescription();
- ed->kmodel->computeTransformsGroup(state->values, model_->groupID);
+ ed->group->computeTransforms(state->values);
if (decision)
decision->resize(pce_.size());
Modified: pkg/trunk/motion_planning/ompl_ros/src/base/ProjectionEvaluators.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_ros/src/base/ProjectionEvaluators.cpp 2009-09-02 21:42:16 UTC (rev 23711)
+++ pkg/trunk/motion_planning/ompl_ros/src/base/ProjectionEvaluators.cpp 2009-09-02 21:46:41 UTC (rev 23712)
@@ -53,7 +53,7 @@
void ompl_ros::LinkPositionProjectionEvaluator::operator()(const ompl::base::State *state, double *projection) const
{
EnvironmentDescription *ed = model_->getEnvironmentDescription();
- ed->kmodel->computeTransformsGroup(state->values, model_->groupID);
+ ed->group->computeTransforms(state->values);
const btVector3 &origin = ed->kmodel->getLink(linkName_)->globalTrans.getOrigin();
projection[0] = origin.x();
projection[1] = origin.y();
Modified: pkg/trunk/motion_planning/ompl_ros/src/dynamic/SpaceInformation.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_ros/src/dynamic/SpaceInformation.cpp 2009-09-02 21:42:16 UTC (rev 23711)
+++ pkg/trunk/motion_planning/ompl_ros/src/dynamic/SpaceInformation.cpp 2009-09-02 21:46:41 UTC (rev 23712)
@@ -42,48 +42,51 @@
void ompl_ros::ROSSpaceInformationDynamic::configureOMPLSpace(ModelBase *model)
{
kmodel_ = model->planningMonitor->getKinematicModel();
- groupID_ = model->groupID;
+ groupName_ = model->groupName;
propagationModel_ = NULL;
/* compute the state space for this group */
- m_stateDimension = kmodel_->getModelInfo().groupStateIndexList[groupID_].size();
+ m_stateDimension = model->group->dimension;
m_stateComponent.resize(m_stateDimension);
for (unsigned int i = 0 ; i < m_stateDimension ; ++i)
{
- int p = kmodel_->getModelInfo().groupStateIndexList[groupID_][i] * 2;
-
+ int p = model->group->stateIndex[i] * 2;
+ m_stateComponent[i].minValue = kmodel_->getStateBounds()[p ];
+ m_stateComponent[i].maxValue = kmodel_->getStateBounds()[p + 1];
+ }
+
+ for (unsigned int i = 0 ; i < model->group->joints.size() ; ++i)
+ {
if (m_stateComponent[i].type == ompl::base::StateComponent::UNKNOWN)
{
+ unsigned int k = model->group->jointIndex[i];
planning_models::KinematicModel::RevoluteJoint *rj =
- dynamic_cast<planning_models::KinematicModel::RevoluteJoint*>(kmodel_->getJoint(kmodel_->getModelInfo().parameterName[p]));
+ dynamic_cast<planning_models::KinematicModel::RevoluteJoint*>(model->group->joints[i]);
if (rj && rj->continuous)
- m_stateComponent[i].type = ompl::base::StateComponent::WRAPPING_ANGLE;
+ m_stateComponent[k].type = ompl::base::StateComponent::WRAPPING_ANGLE;
else
- m_stateComponent[i].type = ompl::base::StateComponent::LINEAR;
+ m_stateComponent[k].type = ompl::base::StateComponent::LINEAR;
}
- m_stateComponent[i].minValue = kmodel_->getModelInfo().stateBounds[p ];
- m_stateComponent[i].maxValue = kmodel_->getModelInfo().stateBounds[p + 1];
+ if (dynamic_cast<planning_models::KinematicModel::FloatingJoint*>(model->group->joints[i]))
+ {
+ unsigned int k = model->group->jointIndex[i];
+ floatingJoints_.push_back(k);
+ m_stateComponent[k + 3].type = ompl::base::StateComponent::QUATERNION;
+ m_stateComponent[k + 4].type = ompl::base::StateComponent::QUATERNION;
+ m_stateComponent[k + 5].type = ompl::base::StateComponent::QUATERNION;
+ m_stateComponent[k + 6].type = ompl::base::StateComponent::QUATERNION;
+ break;
+ }
- for (unsigned int j = 0 ; j < kmodel_->getModelInfo().floatingJoints.size() ; ++j)
- if (kmodel_->getModelInfo().floatingJoints[j] == p)
- {
- floatingJoints_.push_back(i);
- m_stateComponent[i + 3].type = ompl::base::StateComponent::QUATERNION;
- m_stateComponent[i + 4].type = ompl::base::StateComponent::QUATERNION;
- m_stateComponent[i + 5].type = ompl::base::StateComponent::QUATERNION;
- m_stateComponent[i + 6].type = ompl::base::StateComponent::QUATERNION;
- break;
- }
-
- for (unsigned int j = 0 ; j < kmodel_->getModelInfo().planarJoints.size() ; ++j)
- if (kmodel_->getModelInfo().planarJoints[j] == p)
- {
- planarJoints_.push_back(i);
- m_stateComponent[i + 2].type = ompl::base::StateComponent::WRAPPING_ANGLE;
- break;
- }
+ if (dynamic_cast<planning_models::KinematicModel::PlanarJoint*>(model->group->joints[i]))
+ {
+ unsigned int k = model->group->jointIndex[i];
+ planarJoints_.push_back(k);
+ m_stateComponent[k + 2].type = ompl::base::StateComponent::WRAPPING_ANGLE;
+ break;
+ }
}
// create a backup of this, in case it gets bound by joint constraints
@@ -143,7 +146,7 @@
for (unsigned int i = 0 ; i < jc.size() ; ++i)
{
// get the index at which the joint parameters start
- int idx = kmodel_->getJointIndexInGroup(jc[i].joint_name, groupID_);
+ int idx = kmodel_->getGroup(groupName_)->getJointPosition(jc[i].joint_name);
if (idx >= 0)
{
unsigned int usedParams = kmodel_->getJoint(jc[i].joint_name)->usedParams;
Modified: pkg/trunk/motion_planning/ompl_ros/src/dynamic/StateValidator.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl_ros/src/dynamic/StateValidator.cpp 2009-09-02 21:42:16 UTC (rev 23711)
+++ pkg/trunk/motion_planning/ompl_ros/src/dynamic/StateValidator.cpp 2009-09-02 21:46:41 UTC (rev 23712)
@@ -42,7 +42,7 @@
return false;
EnvironmentDescription *ed = model_->getEnvironmentDescription();
- return check(s, ed->collisionSpace, ed->kmodel, ed->constraintEvaluator);
+ return check(s, ed->collisionSpace, ed->group, ed->constraintEvaluator);
}
void ompl_ros::ROSStateValidityPredicateDynamic::setConstraints(const motion_planning_msgs::KinematicConstraints &kc)
@@ -62,12 +62,12 @@
model_->constraintEvaluator.print(out);
}
-bool ompl_ros::ROSStateValidityPredicateDynamic::check(const ompl::base::State *s, collision_space::EnvironmentModel *em, planning_m...
[truncated message content] |
