async def step_robot_eef_cb(req: srpc.r.StepRobotEef.Request,
ui: WsClient) -> None:
scene = 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 robot.check_robot_before_move(robot_inst)
tp = await robot.get_end_effector_pose(robot_inst,
req.args.end_effector_id,
req.args.arm_id)
if req.args.mode == req.args.mode.ROBOT:
tp = tr.make_pose_rel(robot_inst.pose, tp)
elif req.args.mode == req.args.mode.RELATIVE:
assert req.args.pose
tp = tr.make_pose_rel(req.args.pose, tp)
elif req.args.mode == req.args.mode.USER:
assert req.args.pose
raise Arcor2Exception("Not supported yet.")
if req.args.what == req.args.what.POSITION:
if req.args.axis == req.args.axis.X:
tp.position.x += req.args.step
elif req.args.axis == req.args.axis.Y:
tp.position.y += req.args.step
elif req.args.axis == req.args.axis.Z:
tp.position.z += req.args.step
elif req.args.what == req.args.what.ORIENTATION:
if req.args.axis == req.args.axis.X:
tp.orientation *= common.Orientation.from_rotation_vector(
x=req.args.step)
elif req.args.axis == req.args.axis.Y:
tp.orientation *= common.Orientation.from_rotation_vector(
y=req.args.step)
elif req.args.axis == req.args.axis.Z:
tp.orientation *= common.Orientation.from_rotation_vector(
z=req.args.step)
if req.args.mode == req.args.mode.ROBOT:
tp = tr.make_pose_abs(robot_inst.pose, tp)
elif req.args.mode == req.args.mode.RELATIVE:
assert req.args.pose
tp = tr.make_pose_abs(req.args.pose, tp)
await robot.check_reachability(scene, robot_inst, req.args.end_effector_id,
req.args.arm_id, tp, req.args.safe)
await ensure_locked(req.args.robot_id, ui)
if req.dry_run:
return
asyncio.ensure_future(
robot.move_to_pose(robot_inst, req.args.end_effector_id,
req.args.arm_id, tp, req.args.speed, req.args.safe))
def move_to_pose(
self,
end_effector_id: str,
target_pose: Pose,
speed: float,
safe: bool = True,
linear: bool = True,
arm_id: Optional[str] = None,
) -> None:
"""Move given robot's end effector to the selected pose.
:param end_effector_id:
:param target_pose:
:param speed:
:param safe:
:return:
"""
if end_effector_id not in self.get_end_effectors_ids(arm_id):
raise Arcor2Exception("Unknown end effector.")
assert arm_id
speed = min(max(0.0, speed), 1.0)
with self._move_lock:
time.sleep(1.0 - speed)
self._poses[arm_id][end_effector_id] = tr.make_pose_rel(self.pose, target_pose)
def test_make_pose_rel_and_abs_again() -> None:
parent = Pose(Position(), Orientation(0, 0, 1, 0))
child_to_be = Pose(Position(1, 0, 0))
child = make_pose_rel(parent, child_to_be)
assert child == Pose(Position(-1, 0, 0), Orientation(0, 0, -1, 0))
assert make_pose_abs(parent, child) == child_to_be
def move(self,
pose: Pose,
move_type: MoveType,
velocity: float = 50.0,
acceleration: float = 50.0) -> None:
"""Moves the robot's end-effector to a specific pose.
:param pose: Target pose.
:move_type: Move type.
:param velocity: Speed of move (percent).
:param acceleration: Acceleration of move (percent).
:return:
"""
if not (0.0 <= velocity <= 100.0):
raise DobotException("Invalid velocity.")
if not (0.0 <= acceleration <= 100.0):
raise DobotException("Invalid acceleration.")
with self._move_lock:
rp = tr.make_pose_rel(self.pose, pose)
# prevent Dobot from moving when given an unreachable goal
try:
jv = self._inverse_kinematics(rp)
except Arcor2NotImplemented: # TODO remove this once M1 has IK
pass
if self.simulator:
self._joint_values = jv
time.sleep((100.0 - velocity) * 0.05)
return
self._handle_pose_in(rp)
try:
self._dobot.clear_alarms()
unrotated = self.UNROTATE_EEF * rp.orientation
rotation = math.degrees(
quaternion.as_rotation_vector(
unrotated.as_quaternion())[2])
self._dobot.speed(velocity, acceleration)
self._dobot.wait_for_cmd(
self._dobot.move_to(
rp.position.x * 1000.0,
rp.position.y * 1000.0,
rp.position.z * 1000.0,
rotation,
MOVE_TYPE_MAPPING[move_type],
))
except DobotApiException as e:
raise DobotException("Move failed.") from e
self.alarms_to_exception()
def inverse_kinematics(self, pose: Pose) -> List[Joint]:
"""Computes inverse kinematics.
Works with absolute pose.
:param end_effector_id: IK target pose end-effector
:param pose: IK target pose
:param start_joints: IK start joints (not supported)
:param avoid_collisions: Return non-collision IK result if true (not supported)
:return: Inverse kinematics
"""
return self._inverse_kinematics(tr.make_pose_rel(self.pose, pose))
def test_make_pose_rel_and_abs_again_random() -> None:
# hierarchy of poses
p1 = random_pose()
p2 = random_pose()
p3 = random_pose()
# global pose
c = random_pose()
# make it relative
c1 = make_pose_rel(p1, c) # c1 = c relative to p1
c2 = make_pose_rel(p2, c1) # c2 = c relative to p2
c3 = make_pose_rel(p3, c2) # c3 = c relative to p3
# make it absolute again
cc2 = make_pose_abs(p3, c3)
assert cc2 == c2
cc1 = make_pose_abs(p2, cc2)
assert cc1 == c1
cc = make_pose_abs(p1, cc1)
assert cc == c
def inverse_kinematics(
self,
end_effector_id: str,
pose: Pose,
start_joints: Optional[List[Joint]] = None,
avoid_collisions: bool = True,
arm_id: Optional[str] = None,
) -> List[Joint]:
"""Computes inverse kinematics.
:param end_effector_id: IK target pose end-effector
:param pose: IK target pose
:param start_joints: IK start joints
:param avoid_collisions: Return non-collision IK result if true
:return: Inverse kinematics
"""
return self._arm_by_name(arm_id).ik(tr.make_pose_rel(self.pose, pose))
def move_to_pose(
self, end_effector_id: str, target_pose: Pose, speed: float, safe: bool = True, arm_id: Optional[str] = None
) -> None:
"""Move given robot's end effector to the selected pose.
:param end_effector_id:
:param target_pose:
:param speed:
:param safe:
:return:
"""
assert 0.0 <= speed <= 1.0
self.set_v(int(speed * self.settings.max_tcp_speed * METERS_TO_MM))
arm = self._arm_by_name(arm_id)
with self._move_lock:
arm.goto_pose(tr.make_pose_rel(self.pose, target_pose), linear=False)
def move(self,
pose: Pose,
move_type: MoveType,
velocity: float = 50.,
acceleration: float = 50.) -> None:
"""
Moves the robot's end-effector to a specific pose.
:param pose: Target pose.
:move_type: Move type.
:param velocity: Speed of move (percent).
:param acceleration: Acceleration of move (percent).
:return:
"""
assert .0 <= velocity <= 100.
assert .0 <= acceleration <= 100.
self._dobot.clear_alarms()
with self._move_lock:
rp = tr.make_pose_rel(self.pose, pose)
if self.settings.simulator:
time.sleep((100.0 - velocity) * 0.05)
self._ee_pose = rp
return
rotation = quaternion.as_euler_angles(
rp.orientation.as_quaternion())[2]
self._dobot.speed(velocity, acceleration)
self._dobot.wait_for_cmd(
self._dobot.move_to(rp.position.x * 1000.0,
rp.position.y * 1000.0,
rp.position.z * 1000.0, rotation,
MOVE_TYPE_MAPPING[move_type]))
self.alarms_to_exception()
def test_make_pose_rel_and_abs_again_2() -> None:
parent = Pose(Position(-1, 1, -1), Orientation(0, -0.707, 0.707, 0))
child_to_be = Pose(Position(10, -10, 3))
child = make_pose_rel(parent, child_to_be)
assert make_pose_abs(parent, child) == child_to_be
def test_make_pose_rel() -> None:
parent = Pose(Position(1, 2, 3), Orientation(0, 0, 1, 0))
child_to_be = copy.deepcopy(parent)
assert make_pose_rel(parent, child_to_be) == Pose()
