 Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Task-oriented programming for HiroNXO
Description: Programming examples for more complicated task. Unlike any previous tutorials, we now use all APIs available for HiroNXO to achieve the tasks.Keywords: ROS, MoveIt!, hrpsys
Tutorial Level: INTERMEDIATE
NOTE: All the tutorials available under the URL http://wiki.ros.org/rtmros_nextage/Tutorials are applicable to the multiple products of Kawada Industries; Hiro (only with the one that opensource software is installed) and NEXTAGE OPEN. To simplify the notation in the rest of the tutorials, we use HiroNXO to appoint the aforementioned robots.
Contents
Introduction
Up to previous tutorials, we've taken "bottom-up" approach, mostly to focus on familliarizing each API available with HiroNXO. That approach hopefully worked to find each API's dis/advantage.
Now let's move on looking at more complicated tasks that may require more than a single command to achieve. We 'may' use all available APIs mixed for a single task, depending on situations. We're also aware that some users may prefer sticking to a single API (e.g. some like RTM while others are interested in nothing but ROS) to multiple APIs. API-specific solution will be provided as much as possible.
Offline teaching
Using RViz
TBD
Using Fermi, GUI for offline planning
TBD
Using RTM interface commands
If you already know the exact pose values and just want to get the joint angles (e.g. particularly so that you can pass the sequence of joint values at the waypoints to playPattern as described in this tutorial), you can convert to the joint values by batch by do ng the following for instance:
# For larm for example.
# Define the sequence of positions and RPYs of the waypoints.
positions_larm = [pos1, pos2,...,posn] # All elements are lists. E.g. pos1 = [x1, y1, z1]
rpys_larm = [rpy1, rpy2,...,rpyn] # All elements are lists. E.g. rpy1 = [r1, p1, y1]
play_pattern_larm = []
for pos, rpy in zip(positions_larm, rpys_larm):
robot.setTargetPose('larm', pos, rpy, tm=5) # Pass necessary args.
# Example for left arm. For right arm use [3:9] instead.
play_pattern_larm.append(robot.getJointAngles()[10:])
Arm navigation with waypoints
Move arms to the destination pose with specific waypoints on the way, without slowing down between each.
Using RTM
Using HIRONX.playPatternOfGroup, you can send the poses per robot's kinematic group in sequence.
robot.goInitial(tm=3)
import math
# Pass a list of angles in degree, then convert to a list of radian.
LARM_POSE_1_RAD = [math.radians(angle_in_degree) for angle_in_degree in [-3.9945940432973823, -1.4044249851493191, -115.24381793377728, -16.782942807469155, 26.6005628548603, -3.5943405145596574]]
LARM_POSE_2_RAD = [math.radians(angle_in_degree) for angle_in_degree in [-12.065806685460588, -12.47323848974745, -129.85336438013067, -24.77359453901959, 52.801921527692414, -7.7433048931694515]]
robot.playPatternOfGroup('larm', [LARM_POSE_1_RAD, LARM_POSE_2_RAD], [1.5, 2])With the example above, the left arm executes 2 trajectories in order from the initial pose. Each trajectory runs within the specified time.
Note that we did cumbersome deg-to-rad conversion above, because we got the joint values by HIRONX.getJointAngles, which returns in degree, but playPatternOfGroup takes radian. If you already know all positions and RPYs of the waypoints, you can convert by batch by the way described in the previous section.
You can also find an example of how to aquire joint angles using RTM interface from known positions and RPYs of the waypoints, in nextage_rtm_playpattern.py file in nextage_ros_bridge .
First, move Nextage robot/simulation to aquire the joint angles of a limb. The function setTargetPoseSwquence() is defined like at the end of this subsection.
play_pattern_arm = setTargetPoseSequence(limb_name, positions_arm, rpys_arm, time_list)
play_pattern_time = time_listThen move Nextage robot/simulation with the aquired joint angles pattern, using playPatternOfGroup() of RTM interface.
robot.playPatternOfGroup(limb_name, play_pattern_arm, play_pattern_time)
robot.waitInterpolationOfGroup(limb_name)The function setTargetPoseSequence()
def setTargetPoseSequence(limb, pos_list, rpy_list, tm_list):
'''
Create a array of limb joint angles for the each waypoints
from data of the end effector postions and postures
using robot moving using RTM interface.
@type limb : str
@param limb : limb to create a pattern, right arm 'rarm' or left arm 'larm'
@type pos_list : [[float,float,float],[float,float,float],...]
@param pos_list : Array of end effector positions (x,y,z) [m]
@type rpy_list : [[float,float,float],[float,float,float],...]
@param rpy_list : Array of end effector postures (r,p,y) [m]
@type tm_list : [float,float,...]
@param tm_list : Array of motion time lengths of the each pose [s]
@rtype : [[float,float,...],[float,float,...],...]
@return : Array of limb joint angles [rad]
'''
pattern_arm = []
for pos, rpy, tm in zip(pos_list, rpy_list, tm_list):
robot.setTargetPose(limb, pos, rpy, tm)
robot.waitInterpolationOfGroup(limb)
if limb == 'rarm':
joint_angles_deg = robot.getJointAngles()[3:9]
else:
joint_angles_deg = robot.getJointAngles()[9:]
joint_angles_rad = [math.radians(angle_in_degree) for angle_in_degree in joint_angles_deg]
pattern_arm.append(joint_angles_rad)
return pattern_arm
Using MoveIt!
Aquireing joint angles using MoveIt! IK request
Using MoveIt! you can aquire joint angles of limbs without robot/simulaiton moving. You can use PositionIKRequest() to aquire joint agnles from known positins and orientations of the waypoints.
As an example, you can find a file nextage_rtm_playpattern2.py of the nextage_ros_bridge package.
First, generate Joint Angle Pattern with the MoveIt! IK request used in the function setTargetPoseSequenceMoveIt().
play_pattern_arm = setTargetPoseSequenceMoveIt(limb_name, positions_arm, rpys_arm, time_list)
play_pattern_time = time_listThen move Nextage robot/simulation with the aquired joint angles pattern, using playPatternOfGroup() of RTM interface.
robot.playPatternOfGroup(limb_name, play_pattern_arm, play_pattern_time)
robot.waitInterpolationOfGroup(limb_name)The function setTargetPoseSequenceMoveIt() in nextage_rtm_playpattern2.py
def setTargetPoseSequenceMoveIt(limb, pos_list, rpy_list, tm_list):
'''
Create a array of limb joint angles for the each waypoints
from data of the end effector postions and postures
using MoveIt! IK for computing joint angles.
@type limb : str
@param limb : limb to create a pattern, right arm 'rarm' or left arm 'larm'
@type pos_list : [[float,float,float],[float,float,float],...]
@param pos_list : Array of end effector positions (x,y,z) [m]
@type rpy_list : [[float,float,float],[float,float,float],...]
@param rpy_list : Array of end effector postures (r,p,y) [m]
@type tm_list : [float,float,...]
@param tm_list : Array of motion time lengths of the each pose [s]
@rtype : [[float,float,...],[float,float,...],...]
@return : Array of limb joint angles [rad]
'''
pattern_arm = []
wpose = geometry_msgs.msg.Pose()
rospy.wait_for_service('compute_ik')
compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
if limb == 'rarm':
limb_name = "right_arm" # Limb 'right_arm', 'left_arm'
else:
limb_name = "left_arm"
joint_name = filter(lambda n:n[0] == limb, robot.Groups)[0][1]
for pos, rpy, tm in zip(pos_list, rpy_list, tm_list):
quaternion = tf.transformations.quaternion_from_euler(rpy[0], rpy[1], rpy[2])
wpose.position.x = pos[0]
wpose.position.y = pos[1]
wpose.position.z = pos[2]
wpose.orientation.x = quaternion[0]
wpose.orientation.y = quaternion[1]
wpose.orientation.z = quaternion[2]
wpose.orientation.w = quaternion[3]
req = PositionIKRequest()
req.group_name = limb_name
#req.ik_link_name = ''
req.pose_stamped.pose = wpose
ret = compute_ik(req)
if ret.error_code.val != 1:
error(ret)
pattern_arm.append(map(lambda n: n[1],filter(lambda n:n[0] in joint_name, zip(ret.solution.joint_state.name, ret.solution.joint_state.position))))
return pattern_arm
Creating a motion plan of waypoints in MoveIt!
You can use compute_cartesian_path() to create a motion plan from known positins and orientations of the waypoints.
As an example, you can find a file nextage_ros_playpattern.py of the nextage_ros_bridge package.
First, generate a motion plan of a limb with MoveIt!.
plan, flaction = setTargetPoseSequenceMoveIt(group, positions_arm, rpys_arm, time_list)
Then execute the motion plan.
group.execute(plan)
The function setTargetPoseSequenceMoveIt() in nextage_rtm_playpattern.py
def setTargetPoseSequenceMoveIt(limb, pos_list, rpy_list, tm_list):
'''
Create a array of limb joint angles for the each waypoints
from data of the end effector postions and postures
using MoveIt!.
@type limb : str
@param limb : limb to create a pattern, right arm 'rarm' or left arm 'larm'
@type pos_list : [[float,float,float],[float,float,float],...]
@param pos_list : Array of end effector positions (x,y,z) [m]
@type rpy_list : [[float,float,float],[float,float,float],...]
@param rpy_list : Array of end effector postures (r,p,y) [m]
@type tm_list : [float,float,...]
@param tm_list : Array of motion time lengths of the each pose [s]
@rtype : RobotTrajectory, float
@return : Plan of limb waypoints motion and fraction of the path achieved as described by the waypoints
'''
wpose = geometry_msgs.msg.Pose()
waypoints = []
for pos, rpy, tm in zip(pos_list, rpy_list, tm_list):
quaternion = tf.transformations.quaternion_from_euler(rpy[0], rpy[1], rpy[2])
wpose.position.x = pos[0]
wpose.position.y = pos[1]
wpose.position.z = pos[2]
wpose.orientation.x = quaternion[0]
wpose.orientation.y = quaternion[1]
wpose.orientation.z = quaternion[2]
wpose.orientation.w = quaternion[3]
waypoints.append(copy.deepcopy(wpose))
(pln, frc) = limb.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
return pln, frc