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) )