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