tiago_controller_configuration: home

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import rospy
00004 import actionlib
00005 from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
00006 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
00007 from sensor_msgs.msg import JointState
00008 
00009 joint_names = ["gripper_left_finger_joint", "gripper_right_finger_joint"]
00010 closed  = [0.045, 0.045]
00011 
00012 if __name__ == "__main__":
00013   rospy.init_node("home_gripper")
00014   rospy.loginfo("Waiting for gripper_controller...")
00015   client = actionlib.SimpleActionClient("gripper_controller/follow_joint_trajectory", FollowJointTrajectoryAction)
00016   client.wait_for_server()
00017   rospy.loginfo("...connected.")
00018 
00019   rospy.wait_for_message("/joint_states", JointState)
00020 
00021   trajectory = JointTrajectory()
00022   trajectory.joint_names = joint_names
00023   trajectory.points.append(JointTrajectoryPoint())
00024   trajectory.points[0].positions = closed
00025   trajectory.points[0].velocities = [0.0 for i in joint_names]
00026   trajectory.points[0].accelerations = [0.0 for i in joint_names]
00027   trajectory.points[0].time_from_start = rospy.Duration(2.0)
00028 
00029   rospy.loginfo("Opening gripper...")
00030   goal = FollowJointTrajectoryGoal()
00031   goal.trajectory = trajectory
00032   goal.goal_time_tolerance = rospy.Duration(0.0)
00033 
00034   client.send_goal(goal)
00035   client.wait_for_result(rospy.Duration(3.0))
00036   rospy.loginfo("Gripper opened.")