星期六, 14. 一月 2017 10:31上午
主要看看路径规划在moveit里到底咋作的
利用moveit进行机械臂路径规划很简单，利用 moveit_commander.move_group.MoveGroupCommander类即可， 类中有go方法和plan、execute两种选择，今天看一下深入plan里面到底怎么写。
plan方法
def moveit_commander.move_group.MoveGroupCommander.plan ( self,
joints = None
)
- Return a motion plan (a RobotTrajectory) to the set goal state (or specified by the joints argument)
- Definition at line 425 of file move_group.py.
进入代码发现写的是：
def plan(self, joints = None):
""" Return a motion plan (a RobotTrajectory) to the set goal state (or specified by the joints argument) """
if type(joints) is JointState:
self.set_joint_value_target(joints)
elif type(joints) is Pose:
self.set_pose_target(joints)
elif not joints == None:
try:
self.set_joint_value_target(self.get_remembered_joint_values()[joints])
except:
self.set_joint_value_target(joints)
plan = RobotTrajectory()
plan.deserialize(self._g.compute_plan())
return plan
发现最终运行的是plan = RobotTrajectory()来定义结果存放的变量，运行plan.deserialize(self._g.compute_plan())这条语句，应该就是他生成了路径，我们再看看这个函数self._g.compute_plan()：
定义self._g = _moveit_move_group_interface.MoveGroup(name, robot_description)是这样的，也就是说路径规划是在_moveit_move_group_interface下的MoveGroup中生成的。
接下来看看这里面是啥：
只找到moveit::planning_interface::MoveGroup，却发现里面也有 结构plan,再深入看结构plan，里面是：
//The amount of time it took to generate the plan.
planning_time_ moveit::planning_interface::MoveGroup::Plan double
// The full starting state used for planning.
start_state_ moveit::planning_interface::MoveGroup::Plan moveit_msgs::RobotState
The trajectory of the robot (may not contain joints that are the same as for the start_state_)
trajectory_ moveit::planning_interface::MoveGroup::Plan moveit_msgs::RobotTrajectory
写到这里突然理解，估计前面是python路线的，现在转到了c++路线，但是原理应该差不多。
继续上面的moveit::planning_interface::MoveGroup ，他的公共类里我发现了设置目标点方法：
- Setting a joint state target (goal)
There are 2 types of goal targets:
a JointValueTarget (aka JointStateTarget) specifies an absolute value for each joint (angle for rotational joints or position for prismatic joints).
a PoseTarget (Position, Orientation, or Pose) specifies the pose of one or more end effectors (and the planner can use any joint values that reaches the pose(s)).
Only one or the other is used for planning. Calling any of the set*JointValueTarget() functions sets the current goal target to the JointValueTarget. Calling any of the setPoseTarget(), setOrientationTarget(), setRPYTarget(), setPositionTarget() functions sets the current goal target to the Pose target.
bool setJointValueTarget (const std::vector< double > &group_variable_values)
Set the JointValueTarget and use it for future planning requests.
bool setJointValueTarget (const std::map< std::string, double > &variable_values)
Set the JointValueTarget and use it for future planning requests.
bool setJointValueTarget (const robot_state::RobotState &robot_state)
Set the JointValueTarget and use it for future planning requests.
bool setJointValueTarget (const std::string &joint_name, const std::vector< double > &values)
Set the JointValueTarget and use it for future planning requests.
bool setJointValueTarget (const std::string &joint_name, double value)
Set the JointValueTarget and use it for future planning requests.
bool setJointValueTarget (const sensor_msgs::JointState &state)
Set the JointValueTarget and use it for future planning requests.
bool setJointValueTarget (const geometry_msgs::Pose &eef_pose, const std::string &end_effector_link="")
Set the joint state goal for a particular joint by computing IK.
bool setJointValueTarget (const geometry_msgs::PoseStamped &eef_pose, const std::string &end_effector_link="")
Set the joint state goal for a particular joint by computing IK.
bool setJointValueTarget (const Eigen::Affine3d &eef_pose, const std::string &end_effector_link="")
Set the joint state goal for a particular joint by computing IK.
bool setApproximateJointValueTarget (const geometry_msgs::Pose &eef_pose, const std::string &end_effector_link="")
Set the joint state goal for a particular joint by computing IK.
bool setApproximateJointValueTarget (const geometry_msgs::PoseStamped &eef_pose, const std::string &end_effector_link="")
Set the joint state goal for a particular joint by computing IK.
bool setApproximateJointValueTarget (const Eigen::Affine3d &eef_pose, const std::string &end_effector_link="")
Set the joint state goal for a particular joint by computing IK.
void setRandomTarget ()
Set the joint state goal to a random joint configuration.
bool setNamedTarget (const std::string &name)
Set the current joint values to be ones previously remembered by rememberJointValues() or, if not found, that are specified in the SRDF under the name name as a group state.
const robot_state::RobotState & getJointValueTarget () const
Get the currently set joint state goal.
- Setting a pose target (goal)
Setting a Pose (or Position or Orientation) target disables any previously set JointValueTarget. For groups that have multiple end effectors, a pose can be set for each end effector in the group. End effectors which do not have a pose target set will end up in arbitrary positions.
- Planning a path from the start position to the Target (goal) position, and executing that plan.
Planning a path from the start position to the Target (goal) position, and executing that plan.
MoveItErrorCode asyncMove ()
Plan and execute a trajectory that takes the group of joints declared in the constructor to the specified target. This call is not blocking (does not wait for the execution of the trajectory to complete).
MoveItErrorCode move ()
Plan and execute a trajectory that takes the group of joints declared in the constructor to the specified target. This call is always blocking (waits for the execution of the trajectory to complete) and requires an asynchronous spinner to be started.
MoveItErrorCode plan (Plan &plan)
Compute a motion plan that takes the group declared in the constructor from the current state to the specified target. No execution is performed. The resulting plan is stored in plan.
MoveItErrorCode asyncExecute (const Plan &plan)
Given a plan, execute it without waiting for completion. Return true on success.
MoveItErrorCode execute (const Plan &plan)
Given a plan, execute it while waiting for completion. Return true on success.
double computeCartesianPath (const std::vector< geometry_msgs::Pose > &waypoints, double eef_step, double jump_threshold, moveit_msgs::RobotTrajectory &trajectory, bool avoid_collisions=true, moveit_msgs::MoveItErrorCodes *error_code=NULL)
Compute a Cartesian path that follows specified waypoints with a step size of at most eef_step meters between end effector configurations of consecutive points in the result trajectory. The reference frame for the waypoints is that specified by setPoseReferenceFrame(). No more than jump_threshold is allowed as change in distance in the configuration space of the robot (this is to prevent 'jumps' in IK solutions). Collisions are avoided if avoid_collisions is set to true. If collisions cannot be avoided, the function fails. Return a value that is between 0.0 and 1.0 indicating the fraction of the path achieved as described by the waypoints. Return -1.0 in case of error.
void stop ()
Stop any trajectory execution, if one is active.
void allowLooking (bool flag)
Specify whether the robot is allowed to look around before moving if it determines it should (default is true)
void allowReplanning (bool flag)
Specify whether the robot is allowed to replan if it detects changes in the environment.
- Query current robot state
- Manage named joint configurations
- Manage planning constraints
从上面的代码中可以看到具体的路径规划函数MoveItErrorCode plan (Plan &plan) ,进入代码看到：
moveit::planning_interface::MoveItErrorCode moveit::planning_interface::MoveGroup::plan(Plan &plan)
{
return impl_->plan(plan);
}
其具体是有MoveGroupImpl * impl_来实现的，继续挖掘moveit::planning_interface::MoveGroup::MoveGroupImpl 类往下看：
MoveItErrorCode move (bool wait)
MoveGroupImpl (const Options &opt, const boost::shared_ptr< tf::Transformer > &tf, const ros::Duration &wait_for_server)
MoveItErrorCode pick (const std::string &object, const std::vector< moveit_msgs::Grasp > &grasps)
MoveItErrorCode place (const std::string &object, const std::vector< geometry_msgs::PoseStamped > &poses)
Place an object at one of the specified possible locations.
MoveItErrorCode place (const std::string &object, const std::vector< moveit_msgs::PlaceLocation > &locations)
MoveItErrorCode plan (Plan &plan)
其还有私有属性std::string planner_id_用来指定规划算法名。
终于函数里面不是空的了，看到plan函数如下：
MoveItErrorCode plan(Plan &plan)
{
if (!move_action_client_)
{
return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
}
if (!move_action_client_->isServerConnected())
{
return MoveItErrorCode(moveit_msgs::MoveItErrorCodes::FAILURE);
}
moveit_msgs::MoveGroupGoal goal;
constructGoal(goal);
goal.planning_options.plan_only = true;
goal.planning_options.look_around = false;
goal.planning_options.replan = false;
goal.planning_options.planning_scene_diff.is_diff = true;
goal.planning_options.planning_scene_diff.robot_state.is_diff = true;
move_action_client_->sendGoal(goal);
if (!move_action_client_->waitForResult())
{
ROS_INFO_STREAM("MoveGroup action returned early");
}
if (move_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
{
plan.trajectory_ = move_action_client_->getResult()->planned_trajectory;
plan.start_state_ = move_action_client_->getResult()->trajectory_start;
plan.planning_time_ = move_action_client_->getResult()->planning_time;
return MoveItErrorCode(move_action_client_->getResult()->error_code);
}
else
{
ROS_WARN_STREAM("Fail: " << move_action_client_->getState().toString() << ": " << move_action_client_->getState().getText());
return MoveItErrorCode(move_action_client_->getResult()->error_code);
}
}
核心是：
move_action_client_->sendGoal(goal);
move_action_client_->waitForResult();
也就是说通过action方式进行规划，继续…
boost::scoped_ptr<actionlib::SimpleActionClient<moveit_msgs::MoveGroupAction> >
moveit::planning_interface::MoveGroup::MoveGroupImpl::move_action_client_
继续看actionlib::SimpleActionClient< ActionSpec >,来自这里
消息类型moveit_msgs::MoveGroupAction>；
里面果然有：
void cancelAllGoals ()
Cancel all goals currently running on the action server.
void cancelGoal ()
Cancel the goal that we are currently pursuing.
void cancelGoalsAtAndBeforeTime (const ros::Time &time)
Cancel all goals that were stamped at and before the specified time.
ResultConstPtr getResult () const
Get the Result of the current goal.
SimpleClientGoalState getState () const
Get the state information for this goal.
bool isServerConnected () const
Checks if the action client is successfully connected to the action server.
void sendGoal (const Goal &goal, SimpleDoneCallback done_cb=SimpleDoneCallback(), SimpleActiveCallback active_cb=SimpleActiveCallback(), SimpleFeedbackCallback feedback_cb=SimpleFeedbackCallback())
Sends a goal to the ActionServer, and also registers callbacks.
SimpleClientGoalState sendGoalAndWait (const Goal &goal, const ros::Duration &execute_timeout=ros::Duration(0, 0), const ros::Duration &preempt_timeout=ros::Duration(0, 0))
Sends a goal to the ActionServer, and waits until the goal completes or a timeout is exceeded.
SimpleActionClient (const std::string &name, bool spin_thread=true)
Simple constructor.
SimpleActionClient (ros::NodeHandle &n, const std::string &name, bool spin_thread=true)
Constructor with namespacing options.
void stopTrackingGoal ()
Stops tracking the state of the current goal. Unregisters this goal's callbacks.
bool waitForResult (const ros::Duration &timeout=ros::Duration(0, 0))
Blocks until this goal finishes.
bool waitForServer (const ros::Duration &timeout=ros::Duration(0, 0)) const
Waits for the ActionServer to connect to this client.
~SimpleActionClient ()
其中sendGoal (const Goal &goal, SimpleDoneCallback done_cb=SimpleDoneCallback(), SimpleActiveCallback active_cb=SimpleActiveCallback(), SimpleFeedbackCallback feedback_cb=SimpleFeedbackCallback())的函数主要意义是：
Sends a goal to the ActionServer, and also registers callbacks.
查看simple_action_client.h：