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.

## 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

TBD

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:])```

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]]

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_list```

Then 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:]

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_list```

Then 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.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```

Wiki: rtmros_nextage/Tutorials/Task-oriented programming (last edited 2017-05-07 08:16:12 by Yosuke Yamamoto)