diff --git a/abb_egm_client/egm_client.py b/abb_egm_client/egm_client.py
index 1dc36ff2f2901e8eaddf0530d041c43d5f5f8e55..19900712f436638f105a54b5603bd991d39574b5 100644
--- a/abb_egm_client/egm_client.py
+++ b/abb_egm_client/egm_client.py
@@ -1,6 +1,6 @@
import socket
import logging
-from typing import Any, Sequence, Tuple
+from typing import Any, Optional, Sequence, Tuple
from abb_egm_client.atomic_counter import AtomicCounter
@@ -109,7 +109,11 @@ class EGMClient:
return msg
- def send_planned_configuration(self, configuration: Sequence[float]) -> None:
+ def send_planned_configuration(
+ self,
+ configuration: Sequence[float],
+ reference_speeds: Optional[Sequence[float]] = None,
+ ) -> None:
"""Send target configuration to robot controller.
configuration
@@ -120,29 +124,34 @@ class EGMClient:
msg.planned.joints.joints.extend(configuration)
+ if reference_speeds:
+ msg.speedRef.joints = reference_speeds
+
self.send_msg(msg)
def send_planned_frame(
- self, x: float, y: float, z: float, rx: float, ry: float, rz: float
+ self, xyz: Sequence[float], orientation: Sequence[float]
) -> None:
"""Send target frame to robot controller.
- x
- y
- z
- Cartesian coordinates in mm(?)
- rx
- ry
- rz
- Euler angles in (?)
+ xyz
+ Cartesian coordinates in mm.
+ orientation
+ Orientation described by a quaternion
"""
msg = self._create_sensor_msg()
+ x, y, z = xyz
+
+ q1, q2, q3, q4 = orientation
+
msg.planned.cartesian.pos.x = x
msg.planned.cartesian.pos.y = y
msg.planned.cartesian.pos.z = z
- msg.planned.cartesian.euler.x = rx
- msg.planned.cartesian.euler.y = ry
- msg.planned.cartesian.euler.z = rz
+
+ msg.planned.cartesian.orient.u0 = q1
+ msg.planned.cartesian.orient.u1 = q2
+ msg.planned.cartesian.orient.u2 = q3
+ msg.planned.cartesian.orient.u3 = q4
self.send_msg(msg)
diff --git a/examples/print_egm_feedback.py b/examples/print_egm_feedback.py
index f8d86b0174962056dcb602787b8381a052bccb33..58e012df4a43bec45c6fbf2b87030533bb007b98 100644
--- a/examples/print_egm_feedback.py
+++ b/examples/print_egm_feedback.py
@@ -6,7 +6,7 @@ try:
except ImportError:
raise ImportWarning("abb_egm not found, have you installed the package?")
-UDP_PORT = 6511
+UDP_PORT = 6510
log = logging.getLogger("egm_client")
diff --git a/examples/send_configuration.py b/examples/send_configuration.py
index f5f66fb0f4e20b8bbe5c9923442a51315e361de3..205104787a59565020a4c191737b1d9fb696b092 100644
--- a/examples/send_configuration.py
+++ b/examples/send_configuration.py
@@ -1,9 +1,10 @@
#!/usr/bin/env python
-import logging
import sys
import math
from typing import Sequence
+import numpy as np
+
try:
from abb_egm_client import EGMClient
except ImportError:
@@ -48,25 +49,19 @@ def send_configuration(
egm_client = EGMClient(port=UDP_PORT)
if not target_configuration:
- target_configuration = [18, 45, 0, 20, 45, 0]
+ target_configuration = [10, -6, 2, 30, 5, 5]
while True:
pb_robot_msg = egm_client.receive_msg()
cur_configuration = pb_robot_msg.feedBack.joints.joints
- signed_deltas = [t - c for t, c in zip(target_configuration, cur_configuration)]
-
- steps = [constrain(d, -step_size, step_size) for d in signed_deltas]
-
- if all(map(converged_predicate, steps)):
- logging.info(f"Joint positions converged at {cur_configuration}")
+ if pb_robot_msg.mciConvergenceMet:
+ print(f"Joint positions converged at {np.degrees(cur_configuration)}")
return
- new_configuration = [cur + step for cur, step in zip(cur_configuration, steps)]
-
- logging.info(f"Sending {new_configuration}")
- egm_client.send_planned_configuration(new_configuration)
+ print(f"Sending {target_configuration}")
+ egm_client.send_planned_configuration(target_configuration)
if __name__ == "__main__":
diff --git a/examples/send_frame.py b/examples/send_frame.py
index 82d32bc7aa2c5914f179f78fe7b6e565092f5f10..1117dd61767024a150fee68de0bd625f54af6a6c 100644
--- a/examples/send_frame.py
+++ b/examples/send_frame.py
@@ -1,4 +1,54 @@
#!/usr/bin/env python
"""Send frame example."""
+import sys
+from typing import Sequence
-raise NotImplementedError("Planned example")
+
+try:
+ from abb_egm_client import EGMClient
+except ImportError:
+ raise ImportWarning("abb_egm not found, have you installed the package?")
+
+UDP_PORT = 6510
+
+
+def send_frame(xyz: Sequence[float], orientation: Sequence[float]) -> None:
+ """Move robot to target configuration
+
+ Parameters
+ ----------
+ xyz
+ Position in cartesian coordinates.
+ orientation
+ Rotation described by a quaternion.
+ """
+
+ egm_client = EGMClient(port=UDP_PORT)
+
+ while True:
+ pb_robot_msg = egm_client.receive_msg()
+
+ # cur_configuration = pb_robot_msg.feedBack.joints.joints
+ cur_frame = pb_robot_msg.feedBack.cartesian
+
+ if pb_robot_msg.mciConvergenceMet:
+ print(f"Cartesian positions converged at {cur_frame}")
+ return
+
+ print(f"Sending position {xyz} and orientation {orientation}")
+ egm_client.send_planned_frame(xyz, orientation)
+
+
+if __name__ == "__main__":
+ print(sys.argv)
+
+ if len(sys.argv) < 2:
+ xyz = [100, 100, 100]
+ orientation = [1, 0, 0, 0]
+ elif len(sys.argv) == 8: # 8 args including path to this file
+ xyz = [float(n) for n in sys.argv[1:4]]
+ orientation = [float(n) for n in sys.argv[4:]]
+ else:
+ raise RuntimeError("Wrong number of arguments, need six joint values")
+
+ send_frame(xyz, orientation)