exit(-1) fname = sys.argv[1]; node_name = "play_motion" rospy.loginfo( 'register node %s ...'%node_name ) rospy.init_node( node_name ) rospy.loginfo( 'node %s up and running!'%node_name ) print dynutils.get_joint_names( 'l_arm_controller' ) # move right arm out of the way dynutils.move_arm( [ -np.pi / 3, np.pi / 3, 0, -3 * np.pi/4, 0, 0, 0], arm = 'r' ) # load motion ( name, time, pos, vel, acc ) = dynutils.load_motion( fname ); dt = time[1] - time[0] total_time = dt * len( time ) # start client for left arm l_jt_client = dynutils.init_jt_client(arm = 'l') # move left arm to starting posiiton dynutils.move_arm( pos[0,:],
req.ik_request.pose_stamped.pose = pose # make the call try: # print "send request :" #, req res = ik_service( req ) if res.error_code.val == res.error_code.SUCCESS: joint_positions = np.asarray( res.solution.joint_state.position ) req.ik_request.ik_seed_state.joint_state.position = joint_positions else: print "IK failed, error code : ", res.error_code.val break except rospy.ServiceException, e: print "service call failed: %s"%e break dynutils.move_arm(joint_positions, time_from_start = 1.0, client = l_arm_client) else: dynutils.move_arm(home_joint_positions, time_from_start = 1.0, client = l_arm_client) home = not home dynutils.move_arm(home_joint_positions, time_from_start = 1.0, client = l_arm_client) # waypoints = [] # while len( waypoints ) < n_points: # x = center.position.x; # y = center.position.y + np.random.rand() / 5.0; # z = center.position.z + np.random.rand() / 5.0;
rospy.loginfo( 'done!' ) pos = np.asarray( trajectory ) dynutils.wrap_trajectory( pos ) vel = dynutils.compute_derivative( pos, dt ) acc = dynutils.compute_derivative( vel, dt ) # plt.plot( acc ) # plt.show() # move_arms to neutral positions dynutils.move_arm( [ -np.pi / 3, np.pi / 3, 0, -3 * np.pi/4, 0, 0, 0], arm = 'r' ) l_jt_client = dynutils.init_jt_client(arm = 'l') dynutils.move_arm( pos[0,:], time_from_start = 5.0, client = l_jt_client ) # left arm goes to first pos of trajectory # loop last_call = rospy.Time().now() + rospy.Duration().from_sec( .5 ); dynutils.track_trajectory(pos, vel, acc, dt, arm = 'l', client = l_jt_client, stamp = last_call ) while ( not rospy.is_shutdown() ): dur = rospy.Time().now() - last_call;