本文整理汇总了Python中PyPR2.moveBodyWithSpeed方法的典型用法代码示例。如果您正苦于以下问题：Python PyPR2.moveBodyWithSpeed方法的具体用法？Python PyPR2.moveBodyWithSpeed怎么用？Python PyPR2.moveBodyWithSpeed使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类PyPR2的用法示例。
在下文中一共展示了PyPR2.moveBodyWithSpeed方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: run_navigation_trajectory
# 需要导入模块: import PyPR2 [as 别名]
# 或者: from PyPR2 import moveBodyWithSpeed [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