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,:],
time_from_start = 3.0,
client = l_jt_client ) # left arm goes to first pos of trajectory
rospy.loginfo( 'playing \'%s\' motion ...'%name )
# initial call
last_call = rospy.Time().now() + rospy.Duration().from_sec( .5 );
dynutils.track_trajectory(pos, vel, acc, time, arm = 'l', client = l_jt_client, stamp = last_call )
# loop
while ( not rospy.is_shutdown() ):
dur = rospy.Time().now() - last_call;
req.ik_request.pose_stamped.pose = home_pose
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
except rospy.ServiceException, e:
print "service call failed: %s"%e
home_joint_positions = joint_positions
arm = 'l'
l_arm_client = dynutils.init_jt_client( arm )
n_points = 50;
i = 0;
home = True;
while i < n_points:
if not home:
x = center.position.x + abs(np.random.rand() / 5)
y = center.position.y + np.random.rand() / 2
z = center.position.z + np.random.rand() / 10.0
pose = Pose( position = Point( x, y, z ),
orientation = Quaternion( 0.0, 0.0, 0.0, 1.0 ) )
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;
if ( dur.to_sec() > time - (time / 2.0) ):
last_call += rospy.Duration().from_sec( time );
dynutils.track_trajectory(pos, vel, acc, dt, arm = 'l', client = l_jt_client, stamp = last_call )
