nextage_ros_playpattern.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 
00004 # Software License Agreement (BSD License)
00005 #
00006 # Copyright (c) 2017, Tokyo Opensource Robotics Kyokai Association
00007 # All rights reserved.
00008 #
00009 # Redistribution and use in source and binary forms, with or without
00010 # modification, are permitted provided that the following conditions
00011 # are met:
00012 #
00013 #  * Redistributions of source code must retain the above copyright
00014 #    notice, this list of conditions and the following disclaimer.
00015 #  * Redistributions in binary form must reproduce the above
00016 #    copyright notice, this list of conditions and the following
00017 #    disclaimer in the documentation and/or other materials provided
00018 #    with the distribution.
00019 #  * Neither the name of Tokyo Opensource Robotics Kyokai Association. nor the
00020 #    names of its contributors may be used to endorse or promote products
00021 #    derived from this software without specific prior written permission.
00022 #
00023 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034 # POSSIBILITY OF SUCH DAMAGE.
00035 #
00036 # Author: Yosuke Yamamoto
00037 
00038 import rospy
00039 import geometry_msgs.msg
00040 import copy
00041 import tf
00042 
00043 from moveit_commander import MoveGroupCommander
00044 
00045 import nextage_rtm_playpattern as nxtpp
00046 
00047 
00048 def setTargetPoseSequenceMoveIt(limb, pos_list, rpy_list, tm_list):
00049     '''
00050     Create a array of limb joint angles for the each waypoints
00051     from data of the end effector postions and postures
00052     using MoveIt!.
00053     
00054     @type  limb     : str
00055     @param limb     : limb to create a pattern, right arm 'rarm' or left arm 'larm'
00056     @type  pos_list : [[float,float,float],[float,float,float],...]
00057     @param pos_list : Array of end effector positions (x,y,z) [m]
00058     @type  rpy_list : [[float,float,float],[float,float,float],...]
00059     @param rpy_list : Array of end effector postures (r,p,y) [m]
00060     @type  tm_list  : [float,float,...]
00061     @param tm_list  : Array of motion time lengths of the each pose [s]
00062     
00063     @rtype  : RobotTrajectory, float
00064     @return : Plan of limb waypoints motion and fraction of the path achieved as described by the waypoints
00065     '''
00066     wpose = geometry_msgs.msg.Pose()
00067     waypoints = []
00068     
00069     for pos, rpy, tm in zip(pos_list, rpy_list, tm_list):
00070         quaternion = tf.transformations.quaternion_from_euler(rpy[0], rpy[1], rpy[2])
00071         wpose.position.x = pos[0]
00072         wpose.position.y = pos[1]
00073         wpose.position.z = pos[2]
00074         wpose.orientation.x = quaternion[0]
00075         wpose.orientation.y = quaternion[1]
00076         wpose.orientation.z = quaternion[2]
00077         wpose.orientation.w = quaternion[3]
00078         waypoints.append(copy.deepcopy(wpose))
00079     
00080     (pln, frc) = limb.compute_cartesian_path(
00081                              waypoints,   # waypoints to follow
00082                              0.01,        # eef_step
00083                              0.0)         # jump_threshold
00084     
00085     return pln, frc
00086 
00087 
00088 if __name__ == '__main__':
00089     '''
00090     Main sequence for moving at waypoints
00091     using MoveIt!.
00092     '''
00093     rospy.init_node('commander_example', anonymous=True)
00094 
00095     limb_name = "left_arm"  # Limb 'right_arm', 'left_arm'
00096     group = MoveGroupCommander(limb_name)
00097   
00098     rarm_initial_pose = group.get_current_pose().pose
00099     rospy.loginfo("Got Initial Pose \n{}".format(rarm_initial_pose))
00100     
00101     pose_target = geometry_msgs.msg.Pose()
00102     pose_target = copy.deepcopy(rarm_initial_pose)
00103     
00104     positions_arm = []  # All elements are lists. E.g. pos1 = [x1, y1, z1]
00105     rpys_arm = []       # All elements are lists. E.g. rpy1 = [r1, p1, y1]
00106     time_list = []      # Time list [t1, t2, t3,...]
00107     
00108     # Rectangular Positions, RPYs, Time List
00109     rect_xyzs = nxtpp.rectangularPositions(dp_a=[0.25, 0.0, 0.1], dp_b=[0.45, 0.2, 0.1])
00110     rect_rpys = nxtpp.samePostureRPY([-3.073437, -1.569023, 3.073247], len(rect_xyzs))
00111     rect_time = nxtpp.equalTimeList(10.0, len(rect_xyzs))
00112     
00113     # Circular Positions, RPYs, Time List
00114     circ_xyzs = nxtpp.circularPositions(center=[0.35, 0.1, 0.1], radius=0.1 ,steps=12)
00115     circ_rpys = nxtpp.samePostureRPY([-3.073437, -1.569023, 3.073247], len(circ_xyzs))
00116     circ_time = nxtpp.equalTimeList(10.0, len(circ_xyzs))
00117 
00118     # Adjust Transition Time
00119     rect_time[0] += 1.0
00120     circ_time[0] += 1.0
00121     
00122     # Combine Patterns
00123     positions_arm = rect_xyzs + circ_xyzs
00124     rpys_arm      = rect_rpys + circ_rpys
00125     time_list     = rect_time + circ_time
00126 
00127     print('Limb: %s' % limb_name)
00128     print('Positions (%d): %s' % (len(positions_arm), positions_arm))
00129     print('RPY Pattern (%d): %s' % (len(rpys_arm), rpys_arm))
00130     print('Time List (%d): %s' % (len(time_list), time_list))    
00131     
00132     # plan waypoints
00133     rospy.loginfo("Planing Waypoints")
00134     plan, flaction = setTargetPoseSequenceMoveIt(group, positions_arm, rpys_arm, time_list)
00135     
00136     # wait for playing the plan on MoveIt
00137     rospy.loginfo("Waiting while the plan is visualized on RViz")
00138     rospy.sleep(20)
00139     
00140     # execute the plan
00141     rospy.loginfo("Execute!!")
00142     group.execute(plan)
00143     rospy.loginfo("Executiion Done")
00144     rospy.sleep(5)
00145 
00146     # move to the initial pose
00147     rospy.loginfo("Move to Initial Pose")
00148     pose_target = copy.deepcopy(rarm_initial_pose)
00149     rospy.loginfo("set target to {}".format(pose_target))
00150     group.set_pose_target(pose_target)
00151     plan = group.plan()
00152     ret = group.go()


nextage_ros_bridge
Author(s): Isaac Isao Saito , Akio Ochiai
autogenerated on Wed May 15 2019 04:45:36