diff --git a/README.md b/README.md
index 2c2fbb4c26ed313cccad78ad4066f63f03aaf38c..db975f1154766cee9c6fb99ada728c540c35623f 100644
--- a/README.md
+++ b/README.md
@@ -1,36 +1,38 @@
# ABB EGM examples
-## Setup
+Tested on RobotWare version:
+
+- 6.08.01.
-### 1. Install Anaconda
+## Setup
-[Installation](https://docs.anaconda.com/anaconda/install/index.html) guide.
+1. Clone this repo.
+2. Install `protoc` from protobuf. There are [prebuilt binaries available from
+ GitHub](https://github.com/protocolbuffers/protobuf/releases/latest) if `protoc` is not available in your package manager.
+ 3.Find `egm.proto` in either
+ `%LOCALAPPDATA%\ABB\RobotWare\RobotWare_6.XXXXX\utility\Template\EGM\egm.proto`
+ or on the robot.
-### 2. Create an anaconda environment
+3. Run protoc to generate protobuf classes for python. Substitute `$SRC_DIR` for
+ the location of `egm.proto`
```bash
-conda env create -f environment.yml
-conda activate abb-egm-examples
+protoc --python_out=abb_egm_pyclient $SRC_DIR/egm.proto
```
-### 3. Install package
+4. Install this package in your environment of choice.
```bash
-cd /path/to/this/directory
-conda activate abm-egm-examples
+cd abb_egm_pyclient
pip install -e .
```
## Usage
-Run the `EGM_test.mod` module on the robot controller. Remember to change
-UCDevice argument in EGMSetupUC invocation.
+Run the EGM on your robot controller. [Here are some instructions if needed](https://gitlab.control.lth.se/tetov/abb_egm_instructions).
-And on your computer:
+And on your computer (and your python environment):
```bash
-conda activate abm-egm-examples
-
-python examples/print_egm_feedback.py
-python examples/send_configuration.py
+python -m abb_egm_pyclient.run --help
```
diff --git a/abb_egm_pyclient/egm_client.py b/abb_egm_pyclient/egm_client.py
index 2d987c8f69e718f85e0a88ff8b39be1757aba893..ec3bd59db5f8dfecfb89c736fb1fadca6694fc20 100644
--- a/abb_egm_pyclient/egm_client.py
+++ b/abb_egm_pyclient/egm_client.py
@@ -121,7 +121,16 @@ class EGMClient:
msg = self._create_sensor_msg()
conf_as_list = np.asarray(configuration).tolist()
- msg.planned.joints.joints.extend(conf_as_list)
+
+ joints = conf_as_list[:6]
+ external_joints = conf_as_list[6:]
+
+ msg.planned.joints.joints.extend(joints)
+
+ if len(external_joints) > 0:
+ msg.planned.externalJoints.joints.extend(external_joints)
+
+
self.send_msg(msg)
diff --git a/abb_egm_pyclient/run/__main__.py b/abb_egm_pyclient/run/__main__.py
index dc2a6afd3f0e6b65c4c940e173f481f83e024455..e3889dee8447242adf607852070e3380d4741fd5 100644
--- a/abb_egm_pyclient/run/__main__.py
+++ b/abb_egm_pyclient/run/__main__.py
@@ -1,55 +1,56 @@
import argparse
-from abb_egm_pyclient.run import print_egm_feedback
-from abb_egm_pyclient.run import send_configuration
-from abb_egm_pyclient.run import send_pose
+from abb_egm_pyclient.run.print_egm_feedback import print_egm_feedback
+from abb_egm_pyclient.run.send_configuration import send_configuration
+from abb_egm_pyclient.run.send_pose import send_pose
if __name__ == "__main__":
parser = argparse.ArgumentParser(description="Run one of the example EGM scripts.")
parser.add_argument("port", type=int, help="UDP port")
- subparsers = parser.add_subparsers(help="sub-command help")
-
- func_sel_mapping = {
+ subparsers = parser.add_subparsers(help="sub-command help", dest="subparser_name")
+
+ func_sel_mapping = {
"print": print_egm_feedback,
"joint": send_configuration,
"pose": send_pose,
}
- parser_print = subparsers.add_parsers(
+ parser_print = subparsers.add_parser(
"print",
help="print messages recieved from the EGM interface on the controller",
- dest="func_selection",
)
- parser_joint = subparsers.add_parsers(
+ parser_joint = subparsers.add_parser(
"joint",
help="print messages recieved from the EGM interface on the controller",
- dest="func_selection",
)
- parser_joint.add_argument("joint_values", nargs="*", type=float, metavar=("j1", "j2", "j3", "j4", "j5", "j6", "j7"))
+ parser_joint.add_argument(
+ "joint_values",
+ nargs="*",
+ type=float,
+ )
- parser_world = subparsers.add_parsers(
+ parser_pose = subparsers.add_parser(
"pose",
help="print messages recieved from the EGM interface on the controller",
- dest="func_selection",
)
- parser_joint.add_argument("pose_values", nargs="6", type=float, metavar=("x", "y", "z", "rx", "ry", "rz"))
+ parser_pose.add_argument(
+ "pose_values", nargs=6, type=float, metavar=("x", "y", "z", "rx", "ry", "rz")
+ )
args = parser.parse_args()
-
- if args.func_selection == "print":
+
+ print(args.__dict__)
+
+ if args.subparser_name == "print":
print_egm_feedback(args.port)
- if args.func_selection == "joint":
+ if args.subparser_name == "joint":
if len(args.joint_values) not in (6, 7):
- raise argparse.ArgumentError()
+ raise RuntimeError("Incorrect number of joint values.")
send_configuration(args.port, args.joint_values)
-
- if args.func_selection == "pose":
+
+ if args.subparser_name == "pose":
send_configuration(args.port, *args.pose_values)
-
- func = func_sel_mapping[args.func_selection]
-
-
-
+ func = func_sel_mapping[args.func_selection]
diff --git a/abb_egm_pyclient/run/send_configuration.py b/abb_egm_pyclient/run/send_configuration.py
index 2677666e324bcc02d60cd8a2ef6dc2963d17f1cd..26e03e6cc70be738307fa7db58235dd1dd5317da 100644
--- a/abb_egm_pyclient/run/send_configuration.py
+++ b/abb_egm_pyclient/run/send_configuration.py
@@ -29,6 +29,9 @@ def send_configuration(port: int, target_conf: Sequence[float], joint_vel=1.0) -
pb_robot_msg = egm_client.receive_msg()
start_conf: Sequence[float] = pb_robot_msg.feedBack.joints.joints
+ if len(start_conf) == len(target_conf) - 1:
+ start_conf.append(pb_robot_msg.feedBack.externalJoints.joints[0])
+
start_conf_arr = np.array(start_conf)
target_conf_arr = np.array(target_conf)
@@ -43,7 +46,7 @@ def send_configuration(port: int, target_conf: Sequence[float], joint_vel=1.0) -
cur_configuration: Sequence[float] = pb_robot_msg.feedBack.joints.joints
print(f"Current configuration {cur_configuration}")
-
+
new_conf = start_conf_arr
conf = []