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 )