Implemented support for force exposure.

examples/RobotListen.cpp

@@ -48,12 +48,16 @@ int main(int argc, char *argv[])
             while(!stop){
                 Vector6d jointConfig = robot->getCurrentState()->getJointConfig();
                 Vector6d operationalConfig = robot->getCurrentState()->getOperationalConfig();
+                Vector6d operationalVelocity = robot->getCurrentState()->getOperationalVelocity();
+                Vector6d operationalForce = robot->getCurrentState()->getOperationalForce();
                 double currentTime = robot->getCurrentState()->getTimestamp();
 
                 if(currentTime > previousTime)
                 {
                     std::cout << "Joint config: " << TransformUtils::radToDeg(jointConfig).transpose() << std::endl;
-                    std::cout << "Operational config: " << operationalConfig.transpose() << "\n" << std::endl;
+                    std::cout << "Operational config: " << operationalConfig.transpose() << std::endl;
+                    std::cout << "Operational velocity: " << operationalVelocity.transpose() << std::endl;
+                    std::cout << "Operational force: " << operationalForce.transpose() << "\n" << std::endl;
                     previousTime = robot->getCurrentState()->getTimestamp();
                 }
             }

source/pyromocc/examples/connect_and_listen.py

@@ -1,16 +1,26 @@
-from pyromocc import Robot
+import time
 import numpy as np
-from time import sleep
+import argparse
+
+from pyromocc import Robot
+
 np.set_printoptions(precision=3)
 
-robot = Robot(ip="192.168.153.131", port=30003, manipulator="UR10")
-robot.connect()
+parser = argparse.ArgumentParser(description='UR robot listener example')
+parser.add_argument('--ip', type=str, default="192.168.199.129", help='Robot IP address')
+parser.add_argument('--port', type=int, default=30003, help='Robot port number')
+parser.add_argument('--manipulator', type=str, default="UR5", help='Manipulator type')
+parser.add_argument('--sw_version', type=str, default="3.15", help='Controller software version')
 
-sleep(1.0)
-print(robot.joint_config)
+args = parser.parse_args()
 
-robot.movej([0.0, -np.pi/2, -np.pi/2, -np.pi/2, 0, 0], 50, 100)
+robot = Robot(ip=args.ip, port=args.port, manipulator=args.manipulator, sw_version=args.sw_version)
+robot.connect()
 
-sleep(1.0)
-print(robot.joint_config)
-print(robot.pose)
+t0 = time.time()
+while time.time()-t0 < 5:
+    print(f"Joint config: {robot.joint_config}")
+    print(f"Joint velocity: {robot.joint_velocity}")
+    print(f"Operational config {robot.operational_config}")
+    print(f"Operational velocity {robot.operational_velocity}")
+    print(f"Operational force {robot.operational_force} \n")

source/pyromocc/pyromocc/robot.py

@@ -116,6 +116,10 @@ def operational_config(self):
     def operational_velocity(self):
         return self.get_state().get_operational_config()
 
+    @property
+    def operational_force(self):
+        return self.get_state().get_operational_force()
+
     @property
     def pose(self):
         pose = np.copy(self.get_state().get_pose())

source/pyromocc/source/pyromocc.cpp

@@ -64,6 +64,7 @@ PYBIND11_MODULE(pyromocc, m) {
     robotState.def("get_joint_velocity", &RobotState::getJointVelocity);
     robotState.def("get_operational_config", &RobotState::getOperationalConfig);
     robotState.def("get_operational_velocity", &RobotState::getOperationalVelocity);
+    robotState.def("get_operational_force", &RobotState::getOperationalForce);
     robotState.def("joint_to_pose", [](RobotState& self, Eigen::Ref<const Eigen::RowVectorXd> joint_config){
         return self.jointConfigToOperationalConfig(joint_config).matrix();
     });

source/romocc/communication/MessageDecoder.h

@@ -13,6 +13,7 @@ struct ROMOCC_EXPORT ConfigState
     Eigen::RowVectorXd jointVelocity;
     Eigen::RowVectorXd operationalConfiguration;
     Eigen::RowVectorXd operationalVelocity;
+    Eigen::RowVectorXd operationalForce;
     double timestamp = 0;
 };
 

source/romocc/manipulators/ur/UrMessageDecoder.cpp

@@ -18,6 +18,7 @@ ConfigState UrMessageDecoder::analyzeTCPSegment(unsigned char* packet)
         double* jointVelocity = romocc::arrayToLittleEndian(raw_state->actual_velocities_);
         double* operationalConfiguration = romocc::arrayToLittleEndian(raw_state->actual_tool_coordinates_);
         double* operationalVelocity = romocc::arrayToLittleEndian(raw_state->actual_tool_speed_);
+        double* operationalForce = romocc::arrayToLittleEndian(raw_state->generalised_tool_forces_);
 
         TransformUtils::scaleTranslation(operationalConfiguration, 1000);
         TransformUtils::scaleTranslation(operationalVelocity, 1000);
@@ -27,6 +28,7 @@ ConfigState UrMessageDecoder::analyzeTCPSegment(unsigned char* packet)
         state.jointVelocity = RowVector6d(jointVelocity);
         state.operationalConfiguration = RowVector6d(operationalConfiguration);
         state.operationalVelocity = RowVector6d(operationalVelocity);
+        state.operationalForce = RowVector6d(operationalForce);
     }
     return state;
 }

source/romocc/robotics/RobotState.cpp

@@ -53,6 +53,7 @@ void RobotState::setState(romocc::ConfigState configState) {
     mJointVelocity = configState.jointVelocity;
     mOperationalConfiguration = configState.operationalConfiguration;
     mOperationalVelocity = configState.operationalVelocity;
+    mOperationalForce = configState.operationalForce;
     m_bMee = TransformUtils::Affine::toAffine3DFromVector6D(mOperationalConfiguration);
 
     // m_bMee = transform_to_joint(configState.jointConfiguration);
@@ -153,6 +154,14 @@ Vector6d RobotState::getOperationalVelocity() {
     return ret;
 }
 
+Vector6d RobotState::getOperationalForce() {
+    Vector6d ret;
+    mValueLock.lock();
+    ret = mOperationalForce;
+    mValueLock.unlock();
+    return ret;
+}
+
 Vector6d RobotState::operationalConfigToJointConfig(Transform3d transform) {
     auto target_pose = TransformUtils::kdl::fromAffine(transform);
     auto q_init = TransformUtils::kdl::fromVector6D(this->getJointConfig());

source/romocc/robotics/RobotState.h

@@ -41,6 +41,7 @@ class ROMOCC_EXPORT RobotState : public Object
         Vector6d getJointVelocity();
         Vector6d getOperationalConfig();
         Vector6d getOperationalVelocity();
+        Vector6d getOperationalForce();
         Vector6d operationalConfigToJointConfig(Transform3d transform);
         Transform3d jointConfigToOperationalConfig(Vector6d jointConfig);
 
@@ -58,6 +59,7 @@ class ROMOCC_EXPORT RobotState : public Object
         Vector6d mJointVelocity;
         Vector6d mOperationalConfiguration;
         Vector6d mOperationalVelocity;
+        Vector6d mOperationalForce;
         Transform3d m_bMee;
 
         std::mutex mValueLock;