orientation = Quaternion( 0.0, 0.0, 0.0, 1.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 )
else:
print "IK failed, error code : ", res.error_code.val
break
except rospy.ServiceException, e:
print "service call failed: %s"%e
break
trajectory.append( joint_positions )
req.ik_request.ik_seed_state.joint_state.position = joint_positions
rospy.loginfo( 'done!' )
t = np.cumsum( [dt] * n ) - dt
pos = np.asarray( trajectory )
dynutils.wrap_trajectory( pos )
vel = dynutils.compute_derivative( pos, dt )
acc = dynutils.compute_derivative( vel, dt )
dynutils.save_motion( 'circle.pkl', 'circle', t, pos, vel, acc )
orientation=Quaternion(0.0, 0.0, 0.0, 1.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)
else:
print "IK failed, error code : ", res.error_code.val
break
except rospy.ServiceException, e:
print "service call failed: %s" % e
break
trajectory.append(joint_positions)
req.ik_request.ik_seed_state.joint_state.position = joint_positions
rospy.loginfo('done!')
t = np.cumsum([dt] * n) - dt
pos = np.asarray(trajectory)
dynutils.wrap_trajectory(pos)
vel = dynutils.compute_derivative(pos, dt)
acc = dynutils.compute_derivative(vel, dt)
dynutils.save_motion('circle.pkl', 'circle', t, pos, vel, acc)
#!/usr/bin/env python
import dynamics_utils as dynutils
(name, time, pos, vel, acc) = dynutils.load_motion_ascii("/u/dhennes/Desktop/reaching.txt")
dynutils.save_motion("../motions/reaching.pkl", name, time, pos, vel, acc)
orientation = Quaternion( 0.0, 0.0, 0.0, 1.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 )
else:
print "IK failed, error code : ", res.error_code.val
break
except rospy.ServiceException, e:
print "service call failed: %s"%e
break
trajectory.append( joint_positions )
req.ik_request.ik_seed_state.joint_state.position = joint_positions
rospy.loginfo( 'done!' )
t = np.cumsum( [dt] * n ) - dt
pos = np.asarray( trajectory )
dynutils.wrap_trajectory( pos )
vel = dynutils.compute_derivative( pos, dt )
acc = dynutils.compute_derivative( vel, dt )
dynutils.save_motion( 'figure8.pkl', 'figure8', t, pos, vel, acc )