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;
