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 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 approach_home(self):
x_coord, y_coord, theta = self.services.PositionManager.get_home_pos()
coord_pod = [x_coord, y_coord, theta]
p2D_world2robot = almath.Pose2D(
self.services.ALMotion.getRobotPosition(True))
p2D_world2target = almath.Pose2D(coord_pod)
t_world2robot = almath.transformFromPose2D(p2D_world2robot)
t_world2target = almath.transformFromPose2D(p2D_world2target)
t_robot2target = t_world2robot.inverse() * t_world2target
p6D_robot2target = almath.position6DFromTransform(t_robot2target)
coord_pod = list(p6D_robot2target.toVector())
coord_home = [
coord_pod[0] + DISTANCE_FIRST_APPROACH * math.cos(coord_pod[-1]),
coord_pod[1] + DISTANCE_FIRST_APPROACH * math.sin(coord_pod[-1]),
coord_pod[-1] + math.pi
]
is_success = False
if self.services.PositionManager.turn_toward_position(
[DISTANCE_FIRST_APPROACH, 0]):
x, y, theta = coord_home
r, r_theta = self.get_polar_coord([x, y, theta])
if self.services.ALMotion.moveTo(r, 0, 0, MOVE_CONFIG_HIGH_SPEED):
is_success = True
yield self.services.PositionManager.turn_toward_position(
[0, 0], _async=True)
yield stk.coroutines.Return(is_success)
def find_home(self, research_360_activated=False):
"returns (coords) of any code it finds, or (None)."
distance_pod = None
mode_search = False
if not self.services.PositionManager:
yield stk.coroutines.Return(None)
if self.pod_pos_in_world and not research_360_activated:
pod_future = self.services.PositionManager.turn_toward_position(
[-1.0, 0], _async=True)
yield pod_future
distance_pod = self.services.PositionManager.get_dist_from_robot(
-0.70, 0)
pod_future = self.services.ALTracker._lookAtWithEffector(
[distance_pod, 0, 0], 2, 2, 0.1, 0, _async=True)
yield pod_future
else:
mode_search = True
pod_future = None
if mode_search:
pod_future = self.search_home()
yield pod_future
self.logger.info(repr(pod_future.value()))
else:
yield self.services.ALRecharge.setUseTrackerSearcher(False,
_async=True)
pod_future = qi. async (self.services.ALRecharge.lookForStation)
self.future_sleep = stk.coroutines.sleep(5)
yield self.future_sleep
if pod_future.isRunning():
self.services.ALRecharge.stopAll()
yield stk.coroutines.Return(None)
if pod_future:
coord_pod = pod_future.value()
if coord_pod and coord_pod[1] and len(coord_pod[1]) == 3:
p2D_world2robot = almath.Pose2D(
self.services.ALMotion.getRobotPosition(True))
coord_pod = coord_pod[1]
self.services.PositionManager.init_position_with_coord(
coord_pod)
self.pod_pos_in_world = [0.0, 0.0]
p2D_world2target = almath.Pose2D(coord_pod)
t_world2robot = almath.transformFromPose2D(p2D_world2robot)
t_world2target = almath.transformFromPose2D(p2D_world2target)
t_robot2target = t_world2robot.inverse() * t_world2target
p6D_robot2target = almath.position6DFromTransform(
t_robot2target)
coord_pod = list(p6D_robot2target.toVector())
coord_home = [
coord_pod[0] + DISTANCE_FROM_POD * math.cos(coord_pod[-1]),
coord_pod[1] + DISTANCE_FROM_POD * math.sin(coord_pod[-1]),
coord_pod[-1]
]
yield stk.coroutines.Return(coord_home)
yield stk.coroutines.Return(None)
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 get_home_position(self, params):
p6Ds = self.detect_with_try_params_list(params)
p6Ds_list = {}
params_list = []
if not isinstance(params, list):
params_list.append(dict(DEFAULT_PARAMS))
else:
params_list = params
id_list = []
for tmp_params in params_list:
id_list += tmp_params["ids"]
for ids in id_list:
try:
p6D_world2target = almath.Position6D(p6Ds[ids]["world2target"])
t_world2target = almath.transformFromPosition6D(
p6D_world2target)
self.logger.info("@@@ TARGET IN WORLD %s" % p6D_world2target)
p2D_world2robot = almath.Pose2D(
self.ALMotion.getRobotPosition(True))
t_world2robot = almath.transformFromPose2D(p2D_world2robot)
self.logger.info("@@@ ROBOT POSITION %s" % p2D_world2robot)
t_robot2target = t_world2robot.inverse() * t_world2target
p6D_robot2target = almath.position6DFromTransform(
t_robot2target)
self.logger.info("@@@ TARGET IN ROBOT %s" % p6D_robot2target)
p6Ds_list[ids] = list(p6D_robot2target.toVector())
except Exception as e:
pass
return p6Ds_list
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
def find_home(self, research_360_activated=False):
"returns (coords) of any code it finds, or (None)."
distance_big_aruco = None
distance_small_aruco = None
id_aruco = 128
mode_search = False
if not self.services.PositionManager:
self.logger.info("no PositionManager")
yield stk.coroutines.Return(None)
if self.aruco_pos_in_world and not research_360_activated:
distance_big_aruco = self.services.PositionManager.get_dist_from_robot(
0, 0)
distance_small_aruco = self.services.PositionManager.get_dist_from_robot(
0.56, 0)
if distance_big_aruco > 0.60:
aruco_future = self.services.PositionManager.turn_toward_position(\
[0, 0],
_async=True)
yield aruco_future
aruco_future = self.services.ALTracker._lookAtWithEffector(
[distance_big_aruco, 0, 0], 2, 2, 0.1, 0, _async=True)
yield aruco_future
id_aruco = 128
elif distance_small_aruco > 0.40:
aruco_future = self.services.PositionManager.turn_toward_position(\
[1.0, 0],
_async=True)
yield aruco_future
aruco_future = self.services.ALTracker._lookAtWithEffector(
[distance_small_aruco, 0, 0], 2, 2, 0.1, 0, _async=True)
yield aruco_future
id_aruco = 448
else:
yield stk.coroutines.Return(None)
else:
self.logger.info("Lost mode")
mode_search = True
try:
aruco_future = None
if mode_search:
aruco_future = yield self.search_home()
else:
print "DBG done moving, now scanning"
aruco_future = yield self.services.DXAruco.get_home_position_in_world(
DEFAULT_PARAMS[id_aruco], _async=True)
if aruco_future:
self.logger.info(aruco_future)
if id_aruco in aruco_future:
self.logger.info("id_aruco")
self.logger.info(id_aruco)
else:
self.logger.info("id_aruco")
[id_aruco] = [tmp for tmp in [128, 448] if tmp != id_aruco]
self.logger.info(id_aruco)
if id_aruco in aruco_future:
self.logger.info(aruco_future)
coord_aruco = aruco_future[id_aruco]
coord_home_pos = None
if len(coord_aruco) == 6:
coord_aruco = [
coord_aruco[0], coord_aruco[1], coord_aruco[-1]
]
coord_home_pos = coord_aruco
p2D_world2robot = almath.Pose2D(
self.services.ALMotion.getRobotPosition(True))
p2D_world2target = almath.Pose2D(coord_aruco)
t_world2robot = almath.transformFromPose2D(p2D_world2robot)
t_world2target = almath.transformFromPose2D(
p2D_world2target)
t_robot2target = t_world2robot.inverse() * t_world2target
p6D_robot2target = almath.position6DFromTransform(
t_robot2target)
coord_aruco = list(p6D_robot2target.toVector())
x, y, theta = [
coord_aruco[0], coord_aruco[1], coord_aruco[-1]
]
coord_home = None
if id_aruco == 128:
theta += math.radians(-135)
coord_home = [x, y, theta]
coord_home_pos = [
coord_home_pos[0], coord_home_pos[1],
coord_home_pos[-1] + math.radians(-135)
]
else:
theta += math.radians(135)
coord_home = [
x - DISTANCE_FROM_SMALL_ARUCO * math.cos(theta),
y - DISTANCE_FROM_SMALL_ARUCO * math.sin(theta),
theta
]
coord_home_pos = [
coord_home_pos[0] - DISTANCE_FROM_SMALL_ARUCO *
math.cos(coord_home_pos[-1] + math.radians(135)),
coord_home_pos[1] - DISTANCE_FROM_SMALL_ARUCO *
math.sin(coord_home_pos[-1] + math.radians(135)),
coord_home_pos[-1] + math.radians(135)
]
if coord_home_pos != None:
self.services.PositionManager.init_position_with_coord(
coord_home_pos)
self.aruco_pos_in_world = [0.0, 0.0]
self.logger.info("coord " + repr(coord_home))
yield stk.coroutines.Return([coord_home, id_aruco])
return
except Exception as error_msg:
self.logger.warning("No aruco with the id " + repr(id_aruco) +
" found.")
# Now continue at the next head angle
yield stk.coroutines.Return(None)
