MoveIt在rbx2上的学习

一.查看速度信息
Joint Trajectory Action Controller设计用来接受整个关节轨迹作为输入，关节轨迹是一个关于关节位置速度加速度和结果的序列集合，可以通过rqt_graph画出关于/joint_states的消息查看:

$ rqt_plot /joint_states/position[2], /joint_states/position[3], /joint_states/position[4], /joint_states/position[5], /joint_states/position[6], /joint_states/position[8]

 



也可以用下面语句查看速度信息：

$ rqt_plot /joint_states/velocity[2], /joint_states/velocity[3], /joint_states/velocity[4], /joint_states/velocity[5], /joint_states/velocity[6], /joint_states/velocity[8]

或者直接查看消息：

</pre><pre name="code" class="plain">$ rosmsg show JointTrajectorysunfc@ubuntu001:~$ rosmsg show JointTrajectory [trajectory_msgs/JointTrajectory]: std_msgs/Header header   uint32 seq   time stamp   string frame_id string[] joint_names trajectory_msgs/JointTrajectoryPoint[] points   float64[] positions   float64[] velocities   float64[] accelerations   float64[] effort   duration time_from_start 

Both the arbotix and dynamixel_motor packages provide a joint trajectory action controller that works with Dynamixel servos.
使用moveit时会产生运动解决器robot's kinematic solvers,，关节控制器joint controllers ，关节限制oint limits，运动规划和传感器motion planners and sensors，这些都在config文件夹包含，主要包括：kinematics.yaml , controllers.yaml , joint_limits.yaml and sensors_rgbd.yaml，除此之外还会产生一系列launch文件和运动控制模型：

◦ the planning scene pipeline including collision checking and an occupancy grid using an Octomap ◦ motion planning using OMPL (Open Motion Planning Library)◦ the move_group node that provides topics, services and actions used for kinematics, planning and execution, pick and place, state validation and planning scene queries and updates ◦ a joint trajectory controller manager for launching the appropriate trajectory controllers for the given robot

二.运用moveit配置

要使用moveit，首先安装：

$ sudo apt-get update$ sudo apt-get install ros-indigo-moveit-full$ sudo apt-get install ros-indigo-moveit-ikfast

这里我们安装学习的东西：

$ sudo apt-get install ros-indigo-moveit-full-pr2

首先要明白moveit需要的是urdf，所以需要转化xacro：

$ roscd rbx2_description/urdf/pi_robot $ rosrun xacro xacro.py pi_robot_with_arm.xacro > pi_robot.urdf

查看转换是否成功：

$ check_urdf pi_robot.urdf | less

显示如下就是成功了：

robot name is: pi_robot ---------- Successfully Parsed XML --------------- root Link: base_footprint has 1 child(ren) child(1): base_link child(1): base_l_wheel_link child(2): base_r_wheel_link child(3): torso_link child(1): head_base_link child(1): head_pan_servo_link child(1): head_pan_bracket_link child(1): head_tilt_servo_link child(1): head_tilt_bracket_link

下一步就是直接启动moveit配置文件：

$ roslaunch moveit_setup_assistant setup_assistant.launch

以上是选择chain，单独选择抓手如下，选择addlinks，选择组成的link
组件：

设置几个位姿用来模拟：

选择末端执行器：

• click the End Effectors tab on the left • click the Add End Effector button • enter right_end_effector for the End Effector Name • select right_gripper in the End Effector Group pull down menu • in the Parent Link pull down menu, select right_gripper_link • leave the Parent Group blank • click the Save button

结束后自动生成pi_robot_moveit_config/config文件夹
各项运行结束后执行：

$ rospack profile

以下是一些文件内容

The fake_controllers.yaml file：controller_list: - name: fake_right_arm_controller joints: - right_arm_shoulder_pan_joint - right_arm_shoulder_lift_joint - right_arm_shoulder_roll_joint - right_arm_elbow_flex_joint - right_arm_forearm_flex_joint - right_arm_wrist_flex_joint – name: fake_right_gripper_controller joints: - right_gripper_finger_joint

当运动轨迹规划好了以后fake_contrallers会将节点位置信息更新到/joint_states，fake_controllers里面是关于控制关节的信息，一般发表在/joint_states主题，换言之当路径规划结束后将各关节轨迹信息交由fake_controllers来在/joint_states上发布。

joint_limits.yaml：
文件存放各关节活动限制，包括最大最小速度加速度。当对当前设定不满意后可修改，再执行move_group.launch

joint_limits: right_arm_shoulder_pan_joint: has_velocity_limits: true max_velocity: 3.14 has_acceleration_limits: false max_acceleration: 0 right_arm_shoulder_lift_joint: has_velocity_limits: true max_velocity: 3.14 has_acceleration_limits: false max_acceleration: 0

The kinematics.yaml file：
当move_group.launch顺序执行时会加载planning_context.launch，这里kinematics.yaml 就在这里被加载，主要放置求解器等配置

right_arm: kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver_attempts: 3 kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.05

上面那个是单个手臂，下面提供个双臂的：

right_arm: kinematics_solver: pr2_arm_kinematics/PR2ArmKinematicsPlugin kinematics_solver_attempts: 3 kinematics_solver_search_resolution: 0.001 Arm Navigation using MoveIt! - 302kinematics_solver_timeout: 0.05 right_arm_and_torso:kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver_attempts: 3 kinematics_solver_search_resolution: 0.001 kinematics_solver_timeout: 0.05 left_arm: kinematics_solver: pr2_arm_kinematics/PR2ArmKinematicsPlugin kinematics_solver_attempts: 3 kinematics_solver_search_resolution: 0.001 kinematics_solver_timeout: 0.05 left_arm_and_torso: kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver_attempts: 3 kinematics_solver_search_resolution: 0.001 kinematics_solver_timeout: 0.05

在配置文件中最核心的是move_group文件夹，它为handle kinematics, the planning scene, collision checking, sensor input and joint ，trajectory control.启动了一系列的node及需要的所有文件消息服务等等
运用下面的命令可以初步了解moveit：

$ roslaunch pi_robot_moveit_config demo.launch

使用命令行模式控制机械臂：

$ rosrun moveit_commander moveit_commander_cmdline.py

可以随意指定位置

 use right_arm/ go rand/ record c/ goal = c/ c[1]=0.2/ go goal/ current(当前位置) 

查看当前机械臂joint信息和hand坐标等等信息

rosrun rbx2_arm_nav get_arm_pose.py   

之后阶段使用ArbotiX  controllers代替moveit的fake controlles，这个的好处是在使用真实驱动的时候不需要修改太多代码。
使用方法，先是启动相关控制器，然后放入路径：

$ roslaunch rbx2_bringup pi_robot_with_gripper.launch sim:=true

得到：

process[arbotix-1]: started with pid [11850] process[right_gripper_controller-2]: started with pid [11853] process[robot_state_publisher-3]: started with pid [11859] [INFO] [WallTime: 1401976945.363225] ArbotiX being simulated. [INFO] [WallTime: 1401976945.749586] Started FollowController (right_arm_controller). Joints: ['right_arm_shoulder_pan_joint', 'right_arm_shoulder_lift_joint', 'right_arm_shoulder_roll_joint', 'right_arm_elbow_flex_joint', 'right_arm_forearm_flex_joint', 'right_arm_wrist_flex_joint'] on C1 [INFO] [WallTime: 1401976945.761165] Started FollowController (head_controller). Joints: ['head_pan_joint', 'head_tilt_joint'] on C2

在rivz显示图像：

$ rosrun rviz rviz -d `rospack find rbx2_arm_nav`/config/arm_sim.rviz

驱动机械臂运动：

$ rosrun rbx2_arm_nav trajectory_demo.py _reset:=false _sync:=true

对现有机械臂设定目标位置进行规划：trajectory_demo.py ，此文件中将目标位置和速度信息包装在

arm_trajectory = JointTrajectory()

中，然后通过：

arm_client = actionlib.SimpleActionClient('right_arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction)

先构成发送信息：

arm_goal = FollowJointTrajectoryGoal()arm_goal.trajectory = arm_trajectory

然后发送目标信息：

arm_client.send_goal(arm_goal)

同理将head信息也打包发送。

查看速度图：

$ rqt_plot /joint_states/velocity[1]:velocity[7] 

补充：
根据各关节的joint数据，自动规划速度加速度：FollowJointTrajectoryAction

sunfc@ubuntu001:~$ rostopic type /head_controller/follow_joint_trajectory/goal control_msgs/FollowJointTrajectoryActionGoal     # Create a single-point arm trajectory with the arm_goal as the end-point         arm_trajectory = JointTrajectory()         arm_trajectory.joint_names = arm_joints         arm_trajectory.points.append(JointTrajectoryPoint())         arm_trajectory.points[0].positions = arm_goal         arm_trajectory.points[0].velocities = [0.0 for i in arm_joints]         arm_trajectory.points[0].accelerations = [0.0 for i in arm_joints]         arm_trajectory.points[0].time_from_start = rospy.Duration(3.0)         # Send the trajectory to the arm action server         rospy.loginfo('Moving the arm to goal position...')             # Create an empty trajectory goal         arm_goal = FollowJointTrajectoryGoal()              # Set the trajectory component to the goal trajectory created above         arm_goal.trajectory = arm_trajectory 

controller uses a ROS FollowJointTrajectoryAction action to accept a desired joint trajectory and
ensure that it is followed within a given set of tolerances.同样，在moveit里面有moveit_simple_controller_manager，它实现FollowJointTrajectoryAction接口，同时也有GripperCommandAction，


在moveit中使用真实的controller，要做到下面的两步骤：
1.创建controllers.yaml：

controller_list: - name: right_arm_controller action_ns: follow_joint_trajectory type:     FollowJointTrajectory default: true joints:     -     right_arm_shoulder_pan_joint     -     right_arm_shoulder_lift_joint     -     right_arm_shoulder_roll_joint     -     right_arm_elbow_flex_joint     -     right_arm_forearm_flex_joint     -     right_arm_wrist_flex_joint - name: right_gripper_controller action_ns: gripper_action type: GripperCommand default: true joints:     -     right_gripper_finger_joint

查看：type: -->  FollowJointTrajectory

$ rosmsg show control_msgs/FollowJointTrajectoryAction | less

显示：

control_msgs/FollowJointTrajectoryActionGoal action_goal std_msgs/Header header uint32 seq time stamp string frame_id actionlib_msgs/GoalID goal_id time stamp string id control_msgs/FollowJointTrajectoryGoal goal trajectory_msgs/JointTrajectory trajectory std_msgs/Header header uint32 seq time stamp string frame_id string[] joint_names trajectory_msgs/JointTrajectoryPoint[] points float64[] positions float64[] velocities float64[] accelerations float64[] effort duration time_from_start …

Putting it all together, MoveIt! expects our right_arm planning group to be controlled by FollowJointTrajectoryAction messages published on the  /right_arm_controller/follow_joint_trajectory action topics. This in  turns means that our actual joint controller for the right arm must implement a ROS action server that consumes these types of messages and implements a callback to map FollowJointTrajectoryGoal messages into actual joint trajectories. Both the  arbotix_ros and dynamixel_motor packages provide action servers that work with  Dynamixel servos.
总而言之，MoveIt！希望我们的right_arm规划组被followjointtrajectoryaction消息发表在/ right_arm_controller / follow_joint_trajectory话题控制行动。这就意味着我们的实际联合右手臂控制器必须实施ROS作用服务器消耗这些类型的信息并实现回调地图followjointtrajectorygoal信息转化为实际的关节轨迹。无论是arbotix_ros和dynamixel_motor包提供服务器与Dynamixel舵机动作。

2.创建controller manager launch file
moveit自带的是在launch文件夹下的pi_robot_moveit_controller_manager.launch.xml

<launch> <!-- Set the param that trajectory_execution_manager needs to find the controller plugin --> <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" /> <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/> <!-- load controller_list --> <rosparam file="$(find pi_robot_moveit_config)/config/controllers.yaml"/> </launch>

三.正逆运动学

关节空间的轨迹规划（前向运动学）
一般来说，在moveit里面只需要四步就可以完成，其他都已经自动运行：

    right_arm = MoveGroupCommander("right_arm")     joint_positions = [0.2, -0.5, 1.57, -1.0, -0.4, 0.5]     right_arm.set_joint_value_target(joint_positions)     right_arm.go()

运行：

$ roslaunch rbx2_bringup pi_robot_with_gripper.launch sim:=true$ roslaunch pi_robot_moveit_config move_group.launch$ rosrun rviz rviz -d `rospack find rbx2_arm_nav`/config/arm_nav.rviz$ rosrun rbx2_arm_nav moveit_fk_demo.py

其中：right_arm.go()
可以分为：

    traj = right_arm.plan()     right_arm.execuite(traj)

关节空间的轨迹规划（逆运动学）
一般来说，在moveit里面只需要四步就可以完成，其他都已经自动运行：

right_arm = MoveGroupCommander("right_arm") end_effector_link = right_arm.get_end_effector_link() target_pose = PoseStamped() target_pose.header.frame_id = 'base_footprint' target_pose.header.stamp = rospy.Time.now() target_pose.pose.position.x = 0.20 target_pose.pose.position.y = -0.1 target_pose.pose.position.z = 0.85 target_pose.pose.orientation.x = 0.0 target_pose.pose.orientation.y = 0.0 target_pose.pose.orientation.z = 0.0 target_pose.pose.orientation.w = 1.0 right_arm.set_pose_target(target_pose, end_effector_link) right_arm.go()

运行：

$roslaunch rbx2_bringup pi_robot_with_gripper.launch sim:=true$ roslaunch pi_robot_moveit_config move_group.launch$ rosrun rviz rviz -d `rospack find rbx2_arm_nav`/config/arm_nav.rviz$ rosrun rbx2_arm_nav moveit_ik_demo.py

IK算法可以只更新某一项：

 right_arm.shift_pose_target(1, -0.05, end_effector_link)

    其中    0,1,2,3,4,5 → x,y,z,r,p,y where r , p , y stand for roll, pitch and yaw.
保存：  

 saved_target_pose = right_arm.get_current_pose(end_effector_link)

视觉发现

$ roslaunch rbx2_bringup pi_robot_with_gripper.launch sim:=true$ roslaunch pi_robot_moveit_config move_group.launchrosrun rviz rviz -d `rospack find rbx2_utils`/fake_target.rviz$ roslaunch rbx2_utils pub_3d_target.launch speed:=0.2$ roslaunch rbx2_dynamixels head_tracker.launch sim:=true$ rosrun rbx2_arm_nav arm_tracker.py