motion_routine.py 文件源码

def get_ik_joints_linear(initial_x, initial_y, initial_z, initial_w2,
target_x, target_y, target_z, target_w2, n_steps):
    ns = "ExternalTools/left/PositionKinematicsNode/IKService"
    iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
    ikreq = SolvePositionIKRequest(seed_mode=SolvePositionIKRequest.SEED_CURRENT)
    hdr = Header(stamp=rospy.Time.now(), frame_id='base')

    # current_pose = arm.endpoint_pose()
    x0 = initial_x
    y0 = initial_y
    z0 = initial_z

    # linear interpolate between current pose and target pose
    for i in xrange(n_steps):
        t = (i + 1) * 1. / n_steps
        x = (1. - t) * x0 + t * target_x
        y = (1. - t) * y0 + t * target_y
        z = (1. - t) * z0 + t * target_z

        pose = PoseStamped(
            header=hdr,
            pose=Pose(
                position=Point( x=x, y=y, z=z, ),
                # endeffector pointing down
                orientation=Quaternion( x=1., y=0., z=0., w=0., ),
            ),
        )
        ikreq.pose_stamp.append(pose)
    try:
        rospy.wait_for_service(ns, 5.0)
        resp = iksvc(ikreq)
    except (rospy.ServiceException, rospy.ROSException), e:
        rospy.logerr("Service call failed: %s" % (e,))
        return []

    js = []
    # control w2 separately from other joints
    for i, (v, j) in enumerate(zip(resp.isValid, resp.joints)):
        t = (i + 1) * 1. / n_steps
        if v:
            w2 = (1. - t) * initial_w2 + t * target_w2
            j.position = j.position[:-1] + (w2,)
            js.append(j)
    return js