def _inverse_kinematics(self, pose: Pose) -> list[Joint]:
"""Computes inverse kinematics.
Works with robot-relative pose.
Inspired by DobotKinematics.py from open-dobot project and DobotInverseKinematics.py from BenchBot.
:param pose: IK target pose (relative to robot)
:return: Inverse kinematics
"""
self._check_orientation(pose)
# TODO this is probably not working properly (use similar solution as in _check_orientation?)
_, _, yaw = quaternion.as_euler_angles(
pose.orientation.as_quaternion())
x = pose.position.x
y = pose.position.y
z = pose.position.z + self.end_effector_length
# pre-compute distances
# radial position of end effector in the x-y plane
r = math.sqrt(math.pow(x, 2) + math.pow(y, 2))
rho_sq = pow(r - self.link_4_length, 2) + pow(z, 2)
rho = math.sqrt(
rho_sq) # distance b/w the ends of the links joined at the elbow
l2_sq = self.link_2_length**2
l3_sq = self.link_3_length**2
# law of cosines
try:
alpha = math.acos(
(l2_sq + rho_sq - l3_sq) / (2.0 * self.link_2_length * rho))
gamma = math.acos((l2_sq + l3_sq - rho_sq) /
(2.0 * self.link_2_length * self.link_3_length))
except ValueError:
raise DobotException("Failed to compute IK.")
beta = math.atan2(z, r - self.link_4_length)
# joint angles
baseAngle = math.atan2(y, x)
rearAngle = math.pi / 2 - beta - alpha
frontAngle = math.pi / 2 - gamma
joints = [
Joint(Joints.J1, baseAngle),
Joint(Joints.J2, rearAngle),
Joint(Joints.J3, frontAngle),
Joint(Joints.J4, -rearAngle - frontAngle),
Joint(Joints.J5, yaw - baseAngle),
]
self.validate_joints(joints)
return joints
def __init__(self, pose: Pose, port: str = "/dev/dobot", simulator: bool = False) -> None:
super(DobotMagician, self).__init__(pose, port, simulator)
if self.simulator:
self._joint_values = [
Joint(Joints.J1, -0.038),
Joint(Joints.J2, 0.341),
Joint(Joints.J3, 0.3632),
Joint(Joints.J4, -0.704),
Joint(Joints.J5, -0.568),
]
def move_to_calibration_pose(self) -> None:
joint_values = [ # TODO define as pose
Joint(Joints.J1, -0.0115),
Joint(Joints.J2, 0.638),
Joint(Joints.J3, -0.5486),
Joint(Joints.J4, -0.0898),
Joint(Joints.J5, 1.41726),
]
self.move(self.forward_kinematics("", joint_values), MoveType.LINEAR,
50)
def main() -> None:
# robots
aubo = Aubo(uid("rbt"), "Whatever", Pose(), RobotSettings("http://127.0.0.1:13000", "aubo"))
simatic = Aubo(uid("rbt"), "Whatever", Pose(), RobotSettings("http://127.0.0.1:13001", "simatic"))
# objects with pose, with 'System' and 'Configurations' controllers
barcode = Barcode(uid("srv"), "Whatever", Pose(), settings=Settings("http://127.0.0.1:14000", "simulator"))
search = Search(uid("srv"), "Whatever", Pose(), settings=Settings("http://127.0.0.1:12000", "simulator"))
ict = Ict(uid("srv"), "Whatever", Pose(), settings=Settings("http://127.0.0.1:19000", "simulator"))
# objects without pose, without 'System' and 'Configurations' controllers
interaction = Interaction(uid("srv"), "Whatever", SimpleSettings("http://127.0.0.1:17000"))
statistic = Statistic(uid("srv"), "Whatever", SimpleSettings("http://127.0.0.1:16000"))
# Add @action decorator to all actions of each object using patch_object_actions.
# It has to be called exactly once for each type!
# Certain functions of Execution unit (as e.g. pausing script execution) would not work without this step.
# Note: in a generated script, this is done within the Resources context manager.
# Side effect: a lot of JSON data will be printed out to the console when running the script manually.
patch_object_actions(Aubo)
patch_object_actions(Barcode)
patch_object_actions(Search)
patch_object_actions(Ict)
patch_object_actions(Interaction)
patch_object_actions(Statistic)
scene_service.delete_all_collisions()
scene_service.upsert_collision(Box("box_id", 0.1, 0.1, 0.1), Pose())
scene_service.start() # this is normally done by auto-generated Resources class
aubo.move("suction", Pose(), MoveTypeEnum.SIMPLE, safe=False)
simatic.set_joints(ProjectRobotJoints("", "", [Joint("x", 0), Joint("y", 0)]), MoveTypeEnum.SIMPLE)
barcode.scan()
search.set_search_parameters(SearchEngineParameters(search_log_level=SearchLogLevel(LogLevel.DEBUG)))
search.grab_image()
interaction.add_notification("Test", NotificationLevelEnum.INFO)
try:
statistic.get_groups()
except rest.RestHttpException as e:
# service returned error code
print(f"The error code is {e.error_code}.")
except rest.RestException as e:
# request totally failed (timeout, connection error, etc)
print(str(e))
if ict.ready():
test = ict.test("OK")
print(test)
scene_service.stop() # this is normally done by auto-generated Resources class
def robot_joints(self, include_gripper: bool = False) -> list[Joint]:
if self.simulator:
return self._joint_values
joints = self._dobot.get_pose().joints.in_radians()
j3 = self._dobot.get_pose().joints.j3 / 1000
return [
Joint(Joints.J1, joints.j1),
Joint(Joints.J2, joints.j2),
Joint(Joints.J3, j3),
Joint(Joints.J4, joints.j4),
]
def __init__(self,
pose: Pose,
port: str = "/dev/dobot",
simulator: bool = False) -> None:
super(DobotM1, self).__init__(pose, port, simulator)
if self.simulator:
self._joint_values = [
Joint(Joints.J1, 0),
Joint(Joints.J2, 0),
Joint(Joints.J3, 0),
Joint(Joints.J4, 0)
]
def robot_joints(self) -> List[Joint]:
if self.simulator:
return self._joint_values
joints = self._dobot.get_pose().joints.in_radians()
return [
Joint(Joints.J1, joints.j1),
Joint(Joints.J2, joints.j2),
Joint(Joints.J3, joints.j3 - joints.j2),
Joint(Joints.J4, -joints.j3),
Joint(Joints.J5, joints.j4),
]
def put_fk() -> RespT:
"""Get the current state.
---
put:
description: Get the current state.
tags:
- Robot
requestBody:
content:
application/json:
schema:
type: array
items:
$ref: Joint
responses:
200:
description: Ok
content:
application/json:
schema:
$ref: Pose
403:
description: Not started
"""
assert _dobot is not None
joints = [Joint.from_dict(j) for j in request.json]
return jsonify(_dobot.forward_kinematics(joints))
def put_fk() -> RespT:
"""Get the current state.
---
put:
description: Get the current state.
tags:
- Robot
requestBody:
content:
application/json:
schema:
type: array
items:
$ref: Joint
responses:
200:
description: Ok
content:
application/json:
schema:
$ref: Pose
403:
description: Not started
"""
assert _dobot is not None
if not isinstance(request.json, list):
raise FlaskException("Body should be a JSON array containing joints.",
error_code=400)
joints = [Joint.from_dict(j) for j in request.json]
return jsonify(_dobot.forward_kinematics(joints))
def test_global_aps_cls() -> None:
proj = Project("test", "scene_id")
pos = Position(1, 2, 3)
ap1 = ActionPoint("ap1", pos)
ap1_o1 = NamedOrientation("o1", Orientation(0.707, 0, 0, 0.707))
ap1.orientations.append(ap1_o1)
ap1_j1 = ProjectRobotJoints("j1", "robot", [Joint("whatever", 1234)])
ap1.robot_joints.append(ap1_j1)
proj.action_points.append(ap1)
os.environ["ARCOR2_PROJECT_PATH"] = "/tmp"
import arcor2.resources # noqa
my_name = "my_module"
my_spec = importlib.util.spec_from_loader(my_name, loader=None)
my_module = importlib.util.module_from_spec(my_spec)
cproj = CachedProject(proj)
src = global_action_points_class(cproj)
exec(src, my_module.__dict__)
sys.modules["my_module"] = my_module
aps = my_module.ActionPoints(SimResources(cproj)) # type: ignore
assert aps.ap1.position == pos
assert aps.ap1.position is not pos
assert aps.ap1.poses.o1 == Pose(ap1.position, ap1_o1.orientation)
assert aps.ap1.poses.o1.orientation is not ap1_o1.orientation
assert aps.ap1.joints.j1 == ap1_j1
assert aps.ap1.joints.j1 is not ap1_j1
def _response_to_joints(self, res: RawResponse) -> List[Joint]:
tokens = res.message.split()
if len(tokens) != self.JOINTS:
raise YumiException("Invalid format for states! Got: \n{0}".format(res.message))
values = [math.radians(float(token)) for token in tokens]
assert self.name
return [Joint(f"yumi_joint_{i + 1}_{self.name[0]}", j) for i, j in enumerate(values)]
def robot_joints(self) -> List[Joint]:
if self.settings.simulator:
return [
Joint(Joints.J1, 0),
Joint(Joints.J2, 0),
Joint(Joints.J3, 0),
Joint(Joints.J4, 0),
Joint(Joints.J5, 0)
]
joints = self._dobot.get_pose().joints.in_radians()
return [
Joint(Joints.J1, joints.j1),
Joint(Joints.J2, joints.j2),
Joint(Joints.J3, joints.j3 - joints.j2),
Joint(Joints.J4, -joints.j3),
Joint(Joints.J5, joints.j4)
]
def test_get_value() -> None:
scene = Scene("s1", "s1")
obj = SceneObject("test_name", TestObject.__name__)
prj = ProjectRobotJoints("name", obj.id, [Joint("name", 0.333)])
scene.objects.append(obj)
project = Project("p1", "s1")
ap1 = ActionPoint("ap1", Position(1, 0, 0))
ap1.robot_joints.append(prj)
project.action_points.append(ap1)
invalid_param_name = "invalid_param"
act = Action(
"ac1",
f"{obj.id}/{TestObject.action.__name__}",
parameters=[
ActionParameter(param_name, JointsPlugin.type_name(),
json.dumps(prj.id)),
ActionParameter(invalid_param_name, JointsPlugin.type_name(),
json.dumps("non_sense")),
],
)
ap1.actions.append(act)
cscene = CachedScene(scene)
cproject = CachedProject(project)
with pytest.raises(Arcor2Exception):
JointsPlugin.parameter_value(type_defs, cscene, cproject, act.id,
"non_sense")
with pytest.raises(Arcor2Exception):
JointsPlugin.parameter_value(type_defs, cscene, cproject, "non_sense",
param_name)
with pytest.raises(ParameterPluginException):
JointsPlugin.parameter_value(type_defs, cscene, cproject, act.id,
invalid_param_name)
value = JointsPlugin.parameter_value(type_defs, cscene, cproject, act.id,
param_name)
exe_value = JointsPlugin.parameter_execution_value(type_defs, cscene,
cproject, act.id,
param_name)
assert value == value
assert value == exe_value
class DummyMultiArmRobot(MultiArmRobot):
"""Example class representing multiarm robot."""
_ABSTRACT = False
class Arms(StrEnum):
left: str = "left"
right: str = "right"
GRIPPERS: dict[str, set[str]] = {
Arms.left: {"l_gripper_1", "l_gripper_2"},
Arms.right: {"r_gripper_1", "r_gripper_2"},
}
SUCTIONS: dict[str, set[str]] = {
Arms.left: {"l_suction_1", "l_suction_2"},
Arms.right: {"r_suction_1", "r_suction_2"},
}
EEF: dict[str, set[str]] = {Arms.left: {"l_eef_1", "l_eef_2"}, Arms.right: {"r_eef_1", "r_eef_2"}}
JOINTS: dict[str, list[Joint]] = {Arms.left: [Joint("l_joint_1", 1.0)], Arms.right: [Joint("r_joint_1", -1.0)]}
def __init__(self, obj_id: str, name: str, pose: Pose, settings: Optional[Settings] = None) -> None:
super().__init__(obj_id, name, pose, settings)
self._hand_teaching: dict[str, bool] = {self.Arms.left: False, self.Arms.right: False}
self._poses: dict[str, dict[str, Pose]] = {}
for arm, eefs in self.EEF.items():
self._poses[arm] = {}
for eef in eefs:
self._poses[arm][eef] = Pose()
def get_arm_ids(self) -> set[str]:
return self.Arms.set()
@staticmethod
def _get_from_dict(d: dict[str, set[str]], arm_id: Optional[str] = None) -> set[str]:
if arm_id is None:
raise Arcor2Exception("Arm has to be specified.")
try:
return d[arm_id]
except KeyError:
raise Arcor2Exception("Unknown arm.")
def get_end_effectors_ids(self, arm_id: Optional[str] = None) -> set[str]:
return self._get_from_dict(self.EEF, arm_id)
def get_end_effector_pose(self, end_effector: str, arm_id: Optional[str] = None) -> Pose:
if end_effector not in self.get_end_effectors_ids(arm_id):
raise Arcor2Exception("Unknown end effector.")
assert arm_id
return tr.make_pose_abs(self.pose, self._poses[arm_id][end_effector])
def robot_joints(self, include_gripper: bool = False, arm_id: Optional[str] = None) -> list[Joint]:
"""With no arm specified, returns all robot joints. Otherwise, returns
joints for the given arm.
:param arm_id:
:return:
"""
if arm_id is None:
return [item for sublist in self.JOINTS.values() for item in sublist]
try:
return self.JOINTS[arm_id]
except KeyError:
raise Arcor2Exception("Unknown arm.")
def grippers(self, arm_id: Optional[str] = None) -> set[str]:
return self._get_from_dict(self.GRIPPERS, arm_id)
def suctions(self, arm_id: Optional[str] = None) -> set[str]:
return self._get_from_dict(self.SUCTIONS, arm_id)
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 move_to_joints(
self, target_joints: list[Joint], speed: float, safe: bool = True, arm_id: Optional[str] = None
) -> None:
"""Sets target joint values.
:param target_joints:
:param speed:
:param safe:
:return:
"""
if arm_id is None:
if len(target_joints) != len(self.robot_joints()):
raise Arcor2Exception("Joints for both arms have to be specified.")
elif arm_id not in self.get_arm_ids():
raise Arcor2Exception("Unknown arm.")
speed = min(max(0.0, speed), 1.0)
with self._move_lock:
time.sleep(1.0 - speed)
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
"""
if end_effector_id not in self.get_end_effectors_ids(arm_id):
raise Arcor2Exception("Unknown end effector.")
assert arm_id is not None
try:
return self.JOINTS[arm_id]
except KeyError:
raise Arcor2Exception("Unknown arm.")
def forward_kinematics(self, end_effector_id: str, joints: list[Joint], arm_id: Optional[str] = None) -> Pose:
"""Computes forward kinematics.
:param end_effector_id: Target end effector name
:param joints: Input joint values
:return: Pose of the given end effector
"""
if end_effector_id not in self.get_end_effectors_ids(arm_id):
raise Arcor2Exception("Unknown end effector.")
return Pose()
def get_hand_teaching_mode(self, arm_id: Optional[str] = None) -> bool:
"""
This is expected to be implemented if the robot supports set_hand_teaching_mode
:return:
"""
if arm_id is None:
raise Arcor2Exception("Arm has to be specified.")
try:
return self._hand_teaching[arm_id]
except KeyError:
raise Arcor2Exception("Unknown arm.")
def set_hand_teaching_mode(self, enabled: bool, arm_id: Optional[str] = None) -> None:
if arm_id is None:
raise Arcor2Exception("Arm has to be specified.")
try:
state = self._hand_teaching[arm_id]
except KeyError:
raise Arcor2Exception("Unknown arm.")
if state == enabled:
raise Arcor2Exception("That's the current state.")
self._hand_teaching[arm_id] = enabled
def test_value_to_json() -> None:
prj = ProjectRobotJoints("name", "robot_id", [Joint("name", 0.333)])
assert JointsPlugin.value_to_json(prj) == prj.to_json()
def test_plugin_from_instance() -> None:
assert plugin_from_instance(ProjectRobotJoints("name", "robot_id", [Joint("name", 0.333)])) is JointsPlugin
