nextage_moveit_sample.py

Go to the documentation of this file.

#!/usr/bin/env python
##########################################
# @file         nextage_moveit_sample.py         #
# @brief        Nextage Move it demo program #
# @author   Ryu Yamamoto                                 #
# @date         2015/05/26                                       #
##########################################
import moveit_commander
import rospy
import geometry_msgs.msg

def main():
        rospy.init_node("moveit_command_sender")

        robot = moveit_commander.RobotCommander()
    
        print "=" * 10, " Robot Groups:"
        print robot.get_group_names()

        print "=" * 10, " Printing robot state"
        print robot.get_current_state()
        print "=" * 10 

        rarm = moveit_commander.MoveGroupCommander("right_arm")
        larm = moveit_commander.MoveGroupCommander("left_arm")

        print "=" * 15, " Right arm ", "=" * 15
        print "=" * 10, " Reference frame: %s" % rarm.get_planning_frame()
        print "=" * 10, " Reference frame: %s" % rarm.get_end_effector_link()
    
        print "=" * 15, " Left ight arm ", "=" * 15
        print "=" * 10, " Reference frame: %s" % larm.get_planning_frame()
        print "=" * 10, " Reference frame: %s" % larm.get_end_effector_link()

        #Right Arm Initial Pose
        rarm_initial_pose = rarm.get_current_pose().pose
        print "=" * 10, " Printing Right Hand initial pose: "
        print rarm_initial_pose

        #Light Arm Initial Pose
        larm_initial_pose = larm.get_current_pose().pose    
        print "=" * 10, " Printing Left Hand initial pose: "
        print larm_initial_pose

        target_pose_r = geometry_msgs.msg.Pose()
        target_pose_r.position.x = 0.325471850974-0.01
        target_pose_r.position.y = -0.182271241593-0.3
        target_pose_r.position.z = 0.0676272396419+0.3
        target_pose_r.orientation.x = -0.000556712307053
        target_pose_r.orientation.y = -0.706576742941
        target_pose_r.orientation.z = -0.00102461782513
        target_pose_r.orientation.w = 0.707635461636
        rarm.set_pose_target(target_pose_r)

        print "=" * 10," plan1 ..."
        rarm.go()
        rospy.sleep(1)
        
        target_pose_l = [
                target_pose_r.position.x,
                -target_pose_r.position.y,
                target_pose_r.position.z,
                target_pose_r.orientation.x,
                target_pose_r.orientation.y,
                target_pose_r.orientation.z,
                target_pose_r.orientation.w
        ]
        larm.set_pose_target(target_pose_l)

        print "=" * 10," plan2 ..."
        larm.go()
        rospy.sleep(1)
        
        #Clear pose
        rarm.clear_pose_targets()

        #Right Hand
        target_pose_r.position.x = 0.221486843301
        target_pose_r.position.y = -0.0746407547512
        target_pose_r.position.z = 0.642545484602
        target_pose_r.orientation.x = 0.0669013615474
        target_pose_r.orientation.y = -0.993519060661
        target_pose_r.orientation.z = 0.00834224628291
        target_pose_r.orientation.w = 0.0915122442864
        rarm.set_pose_target(target_pose_r)
        
        print "=" * 10, " plan3..."
        rarm.go()
        rospy.sleep(1)

        print "=" * 10,"Initial pose ..."
        rarm.set_pose_target(rarm_initial_pose)
        larm.set_pose_target(larm_initial_pose)
        rarm.go()
        larm.go()
        rospy.sleep(2)
        
if __name__ == '__main__':
    try:
        main()
    except rospy.ROSInterruptException:
        pass