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_joints_cb(req: srpc.r.MoveToJoints.Request,
ui: WsClient) -> None:
ensure_scene_started()
await check_feature(req.args.robot_id, Robot.move_to_joints.__name__)
await robot.check_robot_before_move(req.args.robot_id)
asyncio.ensure_future(
robot.move_to_joints(req.args.robot_id, req.args.joints,
req.args.speed, req.args.safe))
async def move_to_joints_cb(req: srpc.r.MoveToJoints.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 = get_robot_instance(req.args.robot_id)
await check_feature(robot_inst, Robot.move_to_joints.__name__)
await robot.check_robot_before_move(robot_inst)
asyncio.ensure_future(
robot.move_to_joints(robot_inst, req.args.joints, req.args.speed, req.args.safe, user_name, req.args.arm_id)
)