Exemplo n.º 1
0
        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,:], 
Exemplo n.º 2
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;
Exemplo n.º 3
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;