Python PyPR2.getRobotPose方法代码示例

# 需要导入模块: import PyPR2 [as 别名]
# 或者: from PyPR2 import getRobotPose [as 别名]
def body_angle(in_degrees=True ):
    if in_degrees:
        gain = 180.0/math.pi
    else:
        gain = 1.0     

    orient = geo.Orientation_3D(PyPR2.getRobotPose()['orientation'], representation = 'quaternion')
    tau = orient.angle()
    u   = orient.axis()
    return (gen.sign(u[2])*gain*tau)

# 需要导入模块: import PyPR2 [as 别名]
# 或者: from PyPR2 import getRobotPose [as 别名]
def pos_rarm_grip():
    '''
    Returns the global position of the right arm gripper finger tip
    '''
    pr = vecmat.as_vector(PyPR2.getRelativeTF('base_footprint' , 'r_gripper_r_finger_tip_link')['position'])
    pl = vecmat.as_vector(PyPR2.getRelativeTF('base_footprint' , 'r_gripper_l_finger_tip_link')['position'])
    p  = (pl + pr)/2

    orient = geo.Orientation_3D(PyPR2.getRobotPose()['orientation'], representation = 'quaternion')
    pg     = numpy.dot(orient.matrix(), p) # p global is Rb Multiply by p

    p0 = PyPR2.getRobotPose()['position']

    x = pg[0] + p0[0]
    y = pg[1] + p0[1]
    z = pg[2] + p0[2]

    pos = numpy.array([x,y,z])
    return(pos)    

# 需要导入模块: import PyPR2 [as 别名]
# 或者: from PyPR2 import getRobotPose [as 别名]
def run_navigation_trajectory(duration, pos_traj, ori_traj, phi_dot = 1.0, k = 1.0):
    t_s   = time.time()
    t     = 0.0
    pos_traj.set_phi(0.0)
    while t < duration:
        t0 = t
        # Get the desired pose:
        pos_traj.set_phi(phi_dot*t)
        ori_traj.set_phi(phi_dot*t)
        xd   = pos_traj.current_position[0]
        yd   = pos_traj.current_position[1]
        thd  = ori_traj.current_position[0]
        # Get the desired pose speed:
        vxd  = pos_traj.current_velocity[0]*phi_dot
        vyd   = pos_traj.current_velocity[1]*phi_dot
        vthd = ori_traj.current_velocity[0]*phi_dot
        # Get the actual pose:
        PyPR2.getRobotPose()
        rp  = PyPR2.getRobotPose()
        p0  = rp['position']
        xa  = p0[0]
        ya  = p0[1]
        tha = body_angle(in_degrees = False)
        # calculate error:
        ex  = xa - xd
        ey  = ya - yd
        eth = tha - thd
        # find the control speed to be sent:
        vx  = vxd  - k*(xa - xd)        
        vy  = vyd  - k*(ya - yd)        
        vth = vthd - k*(tha - thd)        
        
        PyPR2.moveBodyWithSpeed(vx, vy, vth)

        t      = time.time() - t_s
        dt     = t - t0

# 需要导入模块: import PyPR2 [as 别名]
# 或者: from PyPR2 import getRobotPose [as 别名]
def take_robot_to(X, Y, time_to_reach = 5.0):
    global body_reached, body_failed
    body_reached = False
    body_failed  = False
    rp = PyPR2.getRobotPose()
    P0 = rp['position']
    dX = X - P0[0]
    dY = Y - P0[1]
    tau = body_angle(in_degrees = False)
    S   = math.sin(tau)
    C   = math.cos(tau)
    dx =  C*dX + S*dY
    dy = -S*dX + C*dY

    PyPR2.moveBodyTo(dx, dy, 0.0, time_to_reach)

# 需要导入模块: import PyPR2 [as 别名]
# 或者: from PyPR2 import getRobotPose [as 别名]
def move_robot_to(x, y , tau, time_to_reach = 5.0, in_degrees = True):
    global body_reached, body_failed
    if in_degrees:
        gain = math.pi/180.0
    else:
        gain = 1.0     
    body_reached = False
    body_failed  = False
    rp = PyPR2.getRobotPose()
    p0 = rp['position']
    dx = x - p0[0]
    dy = y - p0[1]

    '''
    o  = quat(rp['orientation'])   # convert to cgkit quaternion
    R  = numpy.linalg.inv(o.toMat3())
    '''
    o  = geo.Orientation_3D(rp['orientation'], representation = 'quaternion')
    R  = numpy.linalg.inv(o.matrix())

    dp = numpy.dot(R.T, vecmat.as_vector([dx, dy, 0.0]))
    tau0 = body_angle(in_degrees = in_degrees)
    PyPR2.moveBodyTo( dp[0], dp[1], gain*(tau - tau0), time_to_reach)

# 需要导入模块: import PyPR2 [as 别名]
# 或者: from PyPR2 import getRobotPose [as 别名]
def body_position():
    return(PyPR2.getRobotPose()['position'])