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 )