def moveToTargetPose(targetPose, motionProxy, isLeftSupport):
''' Move to the desired target with the current foot. '''
targetTf = almath.transformFromPose2D(targetPose)
# Compute foot position with the offset in NAOSpace.
if (isLeftSupport):
footTargetTf = targetTf * almath.transformFromPose2D(rFootOffset)
footTargetPose = almath.pose2DFromTransform(footTargetTf)
name = ["RLeg"]
else:
footTargetTf = targetTf * almath.transformFromPose2D(lFootOffset)
footTargetPose = almath.pose2DFromTransform(footTargetTf)
name = ["LLeg"]
# Clip the footstep to avoid collisions and too wide steps.
almath_foot_clip.clipFootStep(footTargetPose, isLeftSupport)
step = [[footTargetPose.x, footTargetPose.y, footTargetPose.theta]]
speed = [stepSpeed]
# Send the footstep to NAO.
motionProxy.setFootStepsWithSpeed(name, step, speed, False)
# Change current foot.
isLeftSupport = not isLeftSupport
def moveToTargetPose(targetPose, motionProxy, isLeftSupport):
''' Move to the desired target with the current foot. '''
name = ""
targetTf = almath.transformFromPose2D(targetPose)
# Compute foot position with the offset in NAOSpace.
if isLeftSupport:
footTargetTf = targetTf * almath.transformFromPose2D(rFootOffset)
footTargetPose = almath.pose2DFromTransform(footTargetTf)
name = ["RLeg"]
else:
footTargetTf = targetTf * almath.transformFromPose2D(lFootOffset)
footTargetPose = almath.pose2DFromTransform(footTargetTf)
name = ["LLeg"]
# Clip the footstep to avoid collisions and too wide steps.
almath_foot_clip.clipFootStep(footTargetPose, isLeftSupport)
step = [[footTargetPose.x, footTargetPose.y, footTargetPose.theta]]
speed = [stepSpeed]
# Send the footstep to NAO.
motionProxy.setFootStepsWithSpeed(name, step, speed, False)
# Change current foot.
isLeftSupport = not isLeftSupport
def convert_laser_position(self, sensor_device, laser_x, laser_y):
t_world2robot = almath.transformFromPose2D(almath.Pose2D(self.robot_pose))
t_device2point = almath.transformFromPosition3D(almath.Position3D(laser_x, laser_y, 0))
t_robot2device = almath.transformFromPosition6D(self.device_position_list[sensor_device])
t_world2point = t_world2robot * t_robot2device * t_device2point
p2d_world2point = almath.pose2DFromTransform(t_world2point)
return (p2d_world2point.x, p2d_world2point.y)
def convert_laser_position(self, sensor_device, laser_x, laser_y):
# ワールド座標系におけるロボット位置の変換行列
t_world2robot = almath.transformFromPose2D(
almath.Pose2D(self.robot_pose))
# デバイス→レーザー点群
t_device2point = almath.transformFromPosition3D(
almath.Position3D(laser_x, laser_y, 0))
# ロボット→デバイス
t_robot2device = almath.transformFromPosition6D(
self.device_position_list[sensor_device])
# ワールド座標系におけるレーザー点群
t_world2point = t_world2robot * t_robot2device * t_device2point
p2d_world2point = almath.pose2DFromTransform(t_world2point)
return (p2d_world2point.x, p2d_world2point.y)
def convert_laser_position(self, laser_values):
try:
# ロボットの位置
t_world2robot = almath.transformFromPose2D(almath.Pose2D(self.robot_pose))
for sensor_device in range(1):
for point_num in range(15):
laser = laser_values[sensor_device][point_num]
# センサデバイスから点群までの距離
t_device2point = almath.transformFromPosition3D(almath.Position3D(laser[0], laser[1], 0))
# センサデバイスの位置・姿勢
t_robot2device = almath.transformFromPosition6D(self.device_position_list[sensor_device])
# ワールド座標系におけるレーザー点群の位置
t_world2point = t_world2robot * t_robot2device * t_device2point
p2d_world2point = almath.pose2DFromTransform(t_world2point)
laser_values[sensor_device][point_num] = (p2d_world2point.x, p2d_world2point.y)
except Exception as errorMsg:
failure(errorMsg)
return laser_values