Merge pull request tork-a#323 from y-yosuke/add-yosuke_from_k-okada_2

add comments to functions in playPattern example codes

nextage_ros_bridge/script/nextage_ros_playpattern.py

@@ -0,0 +1,152 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2017, Tokyo Opensource Robotics Kyokai Association
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of Tokyo Opensource Robotics Kyokai Association. nor the
+#    names of its contributors may be used to endorse or promote products
+#    derived from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+# Author: Yosuke Yamamoto
+
+import rospy
+import geometry_msgs.msg
+import copy
+import tf
+
+from moveit_commander import MoveGroupCommander
+
+import nextage_rtm_playpattern as nxtpp
+
+
+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
+
+
+if __name__ == '__main__':
+    '''
+    Main sequence for moving at waypoints
+    using MoveIt!.
+    '''
+    rospy.init_node('commander_example', anonymous=True)
+
+    limb_name = "left_arm"  # Limb 'right_arm', 'left_arm'
+    group = MoveGroupCommander(limb_name)
+
+    rarm_initial_pose = group.get_current_pose().pose
+    rospy.loginfo("Got Initial Pose \n{}".format(rarm_initial_pose))
+
+    pose_target = geometry_msgs.msg.Pose()
+    pose_target = copy.deepcopy(rarm_initial_pose)
+
+    positions_arm = []  # All elements are lists. E.g. pos1 = [x1, y1, z1]
+    rpys_arm = []       # All elements are lists. E.g. rpy1 = [r1, p1, y1]
+    time_list = []      # Time list [t1, t2, t3,...]
+
+    # Rectangular Positions, RPYs, Time List
+    rect_xyzs = nxtpp.rectangularPositions(dp_a=[0.25, 0.0, 0.1], dp_b=[0.45, 0.2, 0.1])
+    rect_rpys = nxtpp.samePostureRPY([-3.073437, -1.569023, 3.073247], len(rect_xyzs))
+    rect_time = nxtpp.equalTimeList(10.0, len(rect_xyzs))
+
+    # Circular Positions, RPYs, Time List
+    circ_xyzs = nxtpp.circularPositions(center=[0.35, 0.1, 0.1], radius=0.1 ,steps=12)
+    circ_rpys = nxtpp.samePostureRPY([-3.073437, -1.569023, 3.073247], len(circ_xyzs))
+    circ_time = nxtpp.equalTimeList(10.0, len(circ_xyzs))
+
+    # Adjust Transition Time
+    rect_time[0] += 1.0
+    circ_time[0] += 1.0
+
+    # Combine Patterns
+    positions_arm = rect_xyzs + circ_xyzs
+    rpys_arm      = rect_rpys + circ_rpys
+    time_list     = rect_time + circ_time
+
+    print('Limb: %s' % limb_name)
+    print('Positions (%d): %s' % (len(positions_arm), positions_arm))
+    print('RPY Pattern (%d): %s' % (len(rpys_arm), rpys_arm))
+    print('Time List (%d): %s' % (len(time_list), time_list))    
+
+    # plan waypoints
+    rospy.loginfo("Planing Waypoints")
+    plan, flaction = setTargetPoseSequenceMoveIt(group, positions_arm, rpys_arm, time_list)
+
+    # wait for playing the plan on MoveIt
+    rospy.loginfo("Waiting while the plan is visualized on RViz")
+    rospy.sleep(20)
+
+    # execute the plan
+    rospy.loginfo("Execute!!")
+    group.execute(plan)
+    rospy.loginfo("Executiion Done")
+    rospy.sleep(5)
+
+    # move to the initial pose
+    rospy.loginfo("Move to Initial Pose")
+    pose_target = copy.deepcopy(rarm_initial_pose)
+    rospy.loginfo("set target to {}".format(pose_target))
+    group.set_pose_target(pose_target)
+    plan = group.plan()
+    ret = group.go()

nextage_ros_bridge/script/nextage_rtm_playpattern.py

@@ -0,0 +1,234 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2017, Tokyo Opensource Robotics Kyokai Association
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+#  * Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above
+#    copyright notice, this list of conditions and the following
+#    disclaimer in the documentation and/or other materials provided
+#    with the distribution.
+#  * Neither the name of Tokyo Opensource Robotics Kyokai Association. nor the
+#    names of its contributors may be used to endorse or promote products
+#    derived from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+# Author: Yosuke Yamamoto
+
+from nextage_ros_bridge import nextage_client
+import math
+###
+
+def circularPositions(center=[0.3, 0.2, 0.1], radius=0.1 ,steps=12):
+    '''
+    Create a array of circular position coordinates
+    from a center position and a radius divided by steps.
+    
+    @type  center : [float, float, float]
+    @param center : Position (x,y,z) [m] coordinates of a circle center
+    @type  radius : float
+    @param radius : Radius length [m]
+    @type  steps  : int
+    @param steps  : Number of steps for dividing a circle
+    
+    @rtype  : [ [float, float, float], [float, float, float], ...]
+    @return : Array of circular position coordinates (x,y,z) [m]
+              Length of steps+1 (including an end position same as the start position)
+    '''
+    positions_xyz = []
+    step_rad = 2 * math.pi / steps
+    for i in range(steps+1):
+        ang_rad = step_rad * i
+        px = center[0] - radius * math.cos(ang_rad)
+        py = center[1] + radius * math.sin(ang_rad)
+        pz = center[2]
+        positions_xyz.append([px,py,pz])
+
+    return positions_xyz
+
+
+def rectangularPositions(dp_a=[0.25, 0.0, 0.1], dp_b=[0.45, 0.2, 0.1]):
+    '''
+    Create a array of rectangular position coordinates
+    from diagonal points of A and B.
+    
+    @type  dp_a : [float, float, float]
+    @param dp_a : Position (x,y,z) [m] coordinates of a diagonal point A
+    @type  dp_a : [float, float, float]
+    @param dp_a : Position (x,y,z) [m] coordinates of a diagonal point B
+    
+    @rtype  : [ [float, float, float], [float, float, float], ...]
+    @return : Array of rectangular position coordinates (x,y,z) [m]
+              Length of 5 (including an end position same as the start position)
+    '''
+    positions_xyz = [
+                     [ dp_a[0], dp_a[1], dp_a[2] ],
+                     [ dp_a[0], dp_b[1], dp_a[2] ],
+                     [ dp_b[0], dp_b[1], dp_b[2] ],
+                     [ dp_b[0], dp_a[1], dp_b[2] ],
+                     [ dp_a[0], dp_a[1], dp_a[2] ]
+                    ]
+
+    return positions_xyz
+
+
+def samePostureRPY(rpy, pat_length):
+    '''
+    Create a array of the same posture Roll, Pitch, Yaw angles
+    from one set of Roll Pitch Yaw angles.
+    
+    @type  rpy        : [float, float, float]
+    @param rpy        : Posture angle Roll, Pitch, Yaw (r,p,y) [rad] to copy
+    @type  pat_length : int
+    @param pat_length : Array length of pat_length for the same posture angles
+    
+    @rtype  : [ [float, float, float], [float, float, float], ...]
+    @return : Array of the same posture angles (r,p,y) [rad]
+              Length of pat_length
+    '''
+    posture_rpy = []
+    for i in range(pat_length):
+        posture_rpy.append(rpy)
+
+    return posture_rpy
+
+
+def equalTimeList(whole_tm, pat_length):
+    '''
+    Create a array of the same time length
+    from whole motion time.
+    
+    @type  whole_tm   : float
+    @param whole_tm   : Whole time [s]
+    @type  pat_length : int
+    @param pat_length : Number to divide the whole time
+    
+    @rtype  : [ float, float, float, ... ]
+    @return : Array of the equally divided whole time [s]
+              Length of pat_length
+    '''
+    tm_list = []
+    tm = whole_tm / pat_length
+    for i in range(pat_length):
+        tm_list.append(tm)
+
+    return tm_list
+
+
+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
+
+###
+
+from nextage_ros_bridge import nextage_client
+
+if __name__ == '__main__':
+    '''
+    Main sequence for moving at waypoints
+    using RTM interface.
+    '''
+    robot = nextage_client.NextageClient()
+    # #robot.init(robotname=robotname, url=modelfile)
+    robot.init(robotname="HiroNX(Robot)0")#, url=modelfile)
+
+    # go initial
+    #robot.goInitial()
+
+    # playPatternOfGroup() Example
+    positions_arm = []  # All elements are lists. E.g. pos1 = [x1, y1, z1]
+    rpys_arm = []       # All elements are lists. E.g. rpy1 = [r1, p1, y1]
+    time_list = []      # Time list [t1, t2, t3,...]
+    limb_name = 'larm'  # Limb 'rarm', 'larm'
+
+    # Rectangular Positions, RPYs, Time List
+    rect_xyzs = rectangularPositions(dp_a=[0.25, 0.0, 0.1], dp_b=[0.45, 0.2, 0.1])
+    rect_rpys = samePostureRPY([-3.073437, -1.569023, 3.073247], len(rect_xyzs))
+    rect_time = equalTimeList(10.0, len(rect_xyzs))
+
+    # Circular Positions, RPYs, Time List
+    circ_xyzs = circularPositions(center=[0.35, 0.1, 0.1], radius=0.1 ,steps=12)
+    circ_rpys = samePostureRPY([-3.073437, -1.569023, 3.073247], len(circ_xyzs))
+    circ_time = equalTimeList(10.0, len(circ_xyzs))
+
+    # Adjust Transition Time
+    rect_time[0] += 1.0
+    circ_time[0] += 1.0
+
+    # Combine Patterns
+    positions_arm = rect_xyzs + circ_xyzs
+    rpys_arm      = rect_rpys + circ_rpys
+    time_list     = rect_time + circ_time
+
+    print('Limb: %s' % limb_name)
+    print('Positions (%d): %s' % (len(positions_arm), positions_arm))
+    print('RPY Pattern (%d): %s' % (len(rpys_arm), rpys_arm))
+    print('Time List (%d): %s' % (len(time_list), time_list))
+
+    # Generate Joint Angle Pattern with Moving
+    print('Generating Joint Angle Pattern')
+    play_pattern_arm = setTargetPoseSequence(limb_name, positions_arm, rpys_arm, time_list)
+    play_pattern_time = time_list
+
+    print('Generated')
+    print('Limb: %s' % limb_name)
+    print('Joint Angle Pattern [rad]: %s' % play_pattern_arm)
+    print('Motion Time Pattern [sec]: %s' % play_pattern_time)
+
+    robot.goInitial()
+
+    # Play Pattern - playPatternOfGroup()
+    print('Start: Play Pattern')
+    robot.playPatternOfGroup(limb_name, play_pattern_arm, play_pattern_time)
+    robot.waitInterpolationOfGroup(limb_name)
+    print('End: Play Pattern')
+
+    robot.goInitial()
+