async def move_to_pose_cb(req: rpc.robot.MoveToPoseRequest,
ui: WsClient) -> None:
await check_feature(req.args.robot_id, Robot.move_to_pose.__name__)
await robot.check_robot_before_move(req.args.robot_id)
if (req.args.position is None) != (req.args.orientation is None):
target_pose = await robot.get_end_effector_pose(
req.args.robot_id, req.args.end_effector_id)
if req.args.position:
target_pose.position = req.args.position
elif req.args.orientation:
target_pose.orientation = req.args.orientation
elif req.args.position is not None and req.args.orientation is not None:
target_pose = common.Pose(req.args.position, req.args.orientation)
else:
raise Arcor2Exception("Position or orientation should be given.")
# TODO check if the target pose is reachable (dry_run)
asyncio.ensure_future(
robot.move_to_pose(req.args.robot_id, req.args.end_effector_id,
target_pose, req.args.speed))
async def set_eef_perpendicular_to_world_cb(req: srpc.r.SetEefPerpendicularToWorld.Request, ui: WsClient) -> None:
glob.LOCK.scene_or_exception()
ensure_scene_started()
robot_inst = await osa.get_robot_instance(req.args.robot_id)
await check_feature(robot_inst, Robot.move_to_pose.__name__)
await check_feature(robot_inst, Robot.inverse_kinematics.__name__)
await robot.check_robot_before_move(robot_inst)
await ensure_locked(req.args.robot_id, ui)
if req.dry_run: # attempt to find suitable joints can take some time so it is not done for dry_run
return
tp, current_joints = await robot.get_pose_and_joints(robot_inst, req.args.end_effector_id)
target_joints: Optional[List[common.Joint]] = None
target_joints_diff: float = 0.0
# select best (closest joint configuration) reachable pose
tasks = [
robot.ik(robot_inst, req.args.end_effector_id, pose, current_joints, req.args.safe)
for pose in [
common.Pose(
tp.position,
common.Orientation.from_rotation_vector(y=math.pi) * common.Orientation.from_rotation_vector(z=z_rot),
)
for z_rot in np.linspace(-math.pi, math.pi, 360)
]
]
for res in await asyncio.gather(*tasks, return_exceptions=True):
if not isinstance(res, list):
continue
if not target_joints:
target_joints = res
for f, b in zip(current_joints, target_joints):
assert f.name == b.name
target_joints_diff += (f.value - b.value) ** 2
else:
diff = 0.0
for f, b in zip(current_joints, res):
assert f.name == b.name
diff += (f.value - b.value) ** 2
if diff < target_joints_diff:
target_joints = res
target_joints_diff = diff
if not target_joints:
raise Arcor2Exception("Could not find reachable pose.")
asyncio.ensure_future(robot.move_to_joints(robot_inst, target_joints, req.args.speed, req.args.safe))
async def move_to_pose_cb(req: srpc.r.MoveToPose.Request,
ui: WsClient) -> None:
glob.LOCK.scene_or_exception()
user_name = glob.USERS.user_name(ui)
async with ctx_write_lock(req.args.robot_id, user_name, auto_unlock=False):
ensure_scene_started()
robot_inst = await osa.get_robot_instance(req.args.robot_id)
await robot.check_eef_arm(robot_inst, req.args.arm_id,
req.args.end_effector_id)
await check_feature(robot_inst, Robot.move_to_pose.__name__)
await robot.check_robot_before_move(robot_inst)
if (req.args.position is None) != (req.args.orientation is None):
target_pose = await robot.get_end_effector_pose(
robot_inst, req.args.end_effector_id, req.args.arm_id)
if req.args.position:
target_pose.position = req.args.position
elif req.args.orientation:
target_pose.orientation = req.args.orientation
elif req.args.position is not None and req.args.orientation is not None:
target_pose = common.Pose(req.args.position, req.args.orientation)
else:
raise Arcor2Exception("Position or orientation should be given.")
# TODO check if the target pose is reachable (dry_run)
asyncio.ensure_future(
robot.move_to_pose(
robot_inst,
req.args.end_effector_id,
req.args.arm_id,
target_pose,
req.args.speed,
req.args.safe,
user_name,
))
def put_focus() -> RespT:
"""Calculates position of object.
---
put:
tags:
- Utils
description: Calculates position of object.
requestBody:
content:
application/json:
schema:
$ref: MeshFocusAction
responses:
200:
description: Ok
content:
application/json:
schema:
$ref: Pose
"""
return jsonify(common.Pose().to_dict())
async def set_eef_perpendicular_to_world_cb(req: srpc.r.SetEefPerpendicularToWorld.Request, ui: WsClient) -> None:
glob.LOCK.scene_or_exception()
ensure_scene_started()
robot_inst = get_robot_instance(req.args.robot_id)
await check_feature(robot_inst, Robot.move_to_pose.__name__)
await check_feature(robot_inst, Robot.inverse_kinematics.__name__)
await check_feature(robot_inst, Robot.forward_kinematics.__name__)
await robot.check_robot_before_move(robot_inst)
await ensure_write_locked(req.args.robot_id, glob.USERS.user_name(ui))
if req.dry_run: # attempt to find suitable joints can take some time so it is not done for dry_run
return
tp, current_joints = await robot.get_pose_and_joints(robot_inst, req.args.end_effector_id, req.args.arm_id)
target_joints_diff: float = 0.0
winning_idx: int = -1
poses: list[common.Pose] = [
common.Pose(
tp.position,
common.Orientation.from_rotation_vector(y=math.pi) * common.Orientation.from_rotation_vector(z=z_rot),
)
for z_rot in np.linspace(-math.pi, math.pi, 360)
]
# select best (closest joint configuration) reachable pose
tasks = [
robot.ik(robot_inst, req.args.end_effector_id, req.args.arm_id, pose, current_joints, req.args.safe)
for pose in poses
]
# order of results from gather corresponds to order of tasks
for idx, res in enumerate(await asyncio.gather(*tasks, return_exceptions=True)):
if not isinstance(res, list):
continue
diff = 0.0
for f, b in zip(current_joints, res):
assert (
f.name == b.name
), f"current joints: {[j.name for j in current_joints]}, joints from ik {[j.name for j in res]}."
diff += (f.value - b.value) ** 2
if winning_idx < 0:
target_joints_diff = diff
winning_idx = idx
elif diff < target_joints_diff:
winning_idx = idx
target_joints_diff = diff
if winning_idx < 0:
raise Arcor2Exception("Could not find reachable pose.")
asyncio.ensure_future(
robot.move_to_pose(
robot_inst,
req.args.end_effector_id,
req.args.arm_id,
poses[winning_idx],
req.args.speed,
req.args.safe,
req.args.linear,
)
)
def pose(self, orientation_id: str) -> cmn.Pose:
ap, ori = self.bare_ap_and_orientation(orientation_id)
return cmn.Pose(ap.position, ori.orientation)
