#!/usr/bin/env python

import rospy, sys
import moveit_commander
from control_msgs.msg import GripperCommand
import datetime

class MoveItSo:
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        # Initialize the ROS node
        rospy.init_node('moveit_demo', anonymous=True)

        GRIPPER_OPEN = [0.030, 0.030]
        GRIPPER_CLOSED = [0.002, 0.002]
        GRIPPER_NEUTRAL = [0.01, 0.01]

        # Connect to the arm move group
        arm = moveit_commander.MoveGroupCommander('arm')

        # Connect to the gripper move group
        gripper = moveit_commander.MoveGroupCommander('gripper')

        rospy.loginfo("mw gripper name= " + gripper.get_name())
        rospy.loginfo("mw gripper active joints= " + str(gripper.get_active_joints()))
        rospy.loginfo("mw gripper current joint values= " + str(gripper.get_current_joint_values()))

        # Get the name of the end-effector link
        end_effector_link = arm.get_end_effector_link()

        # Display the name of the end_effector link
        rospy.loginfo("The end effector link is: " + str(end_effector_link))

        # Set a small tolerance on joint angles
        arm.set_goal_joint_tolerance(0.001)
        gripper.set_goal_joint_tolerance(0.002)

        # Set the gripper target to neutal position using a joint value target
        gripper.set_joint_value_target(GRIPPER_OPEN)

        # Plan and execute the gripper motion
        gripper.go()
        rospy.sleep(0.1)
        rospy.loginfo("mw gripper current joint values= " + str(gripper.get_current_joint_values()))

        start_time = datetime.datetime.now()

        # gripper.set_joint_value_target(GRIPPER_CLOSED)
        # gripper.go()
        # rospy.sleep(0.1)

        stop_time = datetime.datetime.now()

        print "took: %s\n" % str(stop_time - start_time)

        # Cleanly shut down MoveIt
        moveit_commander.roscpp_shutdown()

        # Exit the script
        moveit_commander.os._exit(0)

if __name__ == "__main__":
    try:
        MoveItSo()
    except rospy.ROSInterruptException:
        pass

#!/usr/bin/env python

import rospy, sys
import moveit_commander
from control_msgs.msg import GripperCommand
import datetime

class MoveItSo:
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        # Initialize the ROS node
        rospy.init_node('moveit_demo', anonymous=True)

        GRIPPER_OPEN = [0.030, 0.030]
        GRIPPER_CLOSED = [0.002, 0.002]
        GRIPPER_NEUTRAL = [0.01, 0.01]

        # Connect to the arm move group
        arm = moveit_commander.MoveGroupCommander('arm')

        # Connect to the gripper move group
        gripper = moveit_commander.MoveGroupCommander('gripper')

        rospy.loginfo("mw gripper name= " + gripper.get_name())
        rospy.loginfo("mw gripper active joints= " + str(gripper.get_active_joints()))
        rospy.loginfo("mw gripper current joint values= " + str(gripper.get_current_joint_values()))

        # Get the name of the end-effector link
        end_effector_link = arm.get_end_effector_link()

        # Display the name of the end_effector link
        rospy.loginfo("The end effector link is: " + str(end_effector_link))

        # Set a small tolerance on joint angles
        arm.set_goal_joint_tolerance(0.001)
        gripper.set_goal_joint_tolerance(0.002)

        # Set the gripper target to neutal position using a joint value target
        gripper.set_joint_value_target(GRIPPER_OPEN)

        # Plan and execute the gripper motion
        gripper.go()
        rospy.sleep(0.1)
        rospy.loginfo("mw gripper current joint values= " + str(gripper.get_current_joint_values()))

        start_time = datetime.datetime.now()

        # gripper.set_joint_value_target(GRIPPER_CLOSED)
        # gripper.go()
        # rospy.sleep(0.1)

        stop_time = datetime.datetime.now()

        print "took: %s\n" % str(stop_time - start_time)

        # Cleanly shut down MoveIt
        moveit_commander.roscpp_shutdown()

        # Exit the script
        moveit_commander.os._exit(0)

if __name__ == "__main__":
    try:
        MoveItSo()
    except rospy.ROSInterruptException:
        pass

#!/usr/bin/env python # Initialize the move_group API # Initialize the ROS node # Connect to the arm move group # Connect to the gripper move group # Get the name of the end-effector link # Display the name of the end_effector link # Set a small tolerance on joint angles # Set the gripper target to neutal position using a joint value target # Plan and execute the gripper motion # gripper.set_joint_value_target(GRIPPER_CLOSED) # gripper.go() # rospy.sleep(0.1) # Cleanly shut down MoveIt # Exit the script