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)
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)
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
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)
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)
def body_position():
return(PyPR2.getRobotPose()['position'])