Standalone version copied out of labcomm2egmri

.gitignore

+*~
+*.o
+*.pb.h
+*.pb.cc
+timingtest

Makefile

+BOOST_FLAGS = -lboost_system -lboost_thread -lboost_chrono
+BINARIES=timingtest
+
+all: $(BINARIES)
+
+egmri.pb.cc egmri.pb.h : egmri.proto
+	protoc --cpp_out=. egmri.proto
+
+timingtest:  timingtest.cpp defs.h \
+	     egmri.pb.h egmri.pb.o \
+	     robot_message.h robot_message.o \
+	     sensor_message.h sensor_message.o 
+	gcc -O3 -std=c++11 -o $@ $< $(filter %.o,$^) \
+		$(BOOST_FLAGS) -lprotobuf -lstdc++ -lm -lpthread
+
+%.o:	%.cpp %.h
+	gcc -O3 -std=c++11 -c -o $@ $<
+
+clean:
+	rm -f *~
+	rm -f *.o
+	rm -f *.pb.h
+	rm -f *.pb.cc
+	rm -f $(BINARIES)

defs.h

+#ifndef __DEFS_H__
+#define __DEFS_H__
+
+
+namespace DEFAULT
+{
+  // signal address space
+  namespace YUMI
+  {
+    namespace PORT
+    {
+      const int EGMRI_LEFT = 6511;
+      const int EGMRI_RIGHT = 6512;
+    }
+  }
+}
+
+
+#endif

egmri.proto

+syntax = "proto2";
+
+//======================================================================================================
+//
+// Definition of ABB Externally Guided Motion Research Interface (EGMRI) V1.0.
+//
+// Messages of type EgmriRobot are sent out from the robot controller.
+// Messages of type EgmriSensor are sent to the robot controller.
+//
+//======================================================================================================
+package abb.egmri;
+
+
+
+
+//======================================================================================================
+// Auxiliary components
+//======================================================================================================
+
+//---------------------------------------------------
+// Header component
+//---------------------------------------------------
+
+message EgmriHeader
+{ 
+    optional uint32 seqno = 1; // Sequence number (to be able to find lost messages)
+    optional uint32 tm    = 2; // Timestamp in ms
+
+    enum MessageType {
+        MSGTYPE_UNDEFINED  = 0;
+        MSGTYPE_COMMAND    = 1; // For future use
+        MSGTYPE_DATA       = 2; // Sent by robot controller
+        MSGTYPE_CORRECTION = 3; // Sent by sensor
+    }
+
+    optional MessageType mtype = 3 [default = MSGTYPE_UNDEFINED];
+}
+
+
+//---------------------------------------------------
+// Basic Cartesian space components
+//---------------------------------------------------
+
+// Cartesian position
+message EgmriCartesian
+{
+    required float x = 1;
+    required float y = 2;
+    required float z = 3;
+}
+
+// Quaternion orientation
+message EgmriQuaternion
+{
+    required float u0 = 1;
+    required float u1 = 2;
+    required float u2 = 3;
+    required float u3 = 4;
+}
+
+// Pose (i.e. Cartesian position and Quaternion orientation)
+message EgmriPose
+{ 
+    optional EgmriCartesian  position    = 1; // Units: mm
+    optional EgmriQuaternion orientation = 2;
+} 
+
+
+//---------------------------------------------------
+// Basic joint space components
+//---------------------------------------------------
+
+// Array of 6 joint values
+message EgmriJoints
+{
+    repeated float joints = 1;
+}
+
+// Joint space component
+message EgmriJointSpace
+{
+    optional EgmriJoints joints         = 1; // Array of 6 joint values for TCP robot
+    optional EgmriJoints externalJoints = 2; // Array of 6 joint values for additional axis
+}
+
+
+//---------------------------------------------------
+// Composite components
+//---------------------------------------------------
+
+// Robot feedback values
+message EgmriFeedback
+{
+    // Cartesian space feedback data for TCP pose, i.e. actual measurements for robot's TCP
+    optional EgmriPose cartesianPose = 1;
+
+    // Joint space feedback data for position, speed and torque, i.e. actual measurements for robot (joints) and additional axis (externalJoints)
+    optional EgmriJointSpace jointPosition = 2; // Units: rad
+    optional EgmriJointSpace jointSpeed    = 3; // Units: rad/s
+    optional EgmriJointSpace jointTorque   = 4; // Units: Nm
+}
+
+// Robot planned values
+message EgmriPlanned
+{
+    // Cartesian space planned data for TCP pose
+    optional EgmriPose       cartesianPose = 1;
+
+    // Joint space planned data for position and speed, for robot (joints) and additional axis (externalJoints)
+    optional EgmriJointSpace jointPosition = 2; // Units: rad
+    optional EgmriJointSpace jointSpeed    = 3; // Units: rad/s
+}
+
+// Servocontroller PID parameters for a P-PI cascade controller, for robot (joints) and additional axis (externalJoints)
+message EgmriPIDParameters
+{
+    optional EgmriJointSpace kp = 1; // Position control propotional gain
+    optional EgmriJointSpace kv = 2; // Speed control propotional gain
+    optional EgmriJointSpace ki = 3; // Speed control integration gain
+}
+
+
+//---------------------------------------------------
+// Additonal components
+//---------------------------------------------------
+
+// Motor state
+message EgmriMotorState
+{
+    enum MotorStateType {
+        MOTORS_UNDEFINED = 0;
+        MOTORS_ON        = 1;
+        MOTORS_OFF       = 2;
+    }
+
+    required MotorStateType state = 1;
+}
+
+// MCI (Motion Correction Interface, internal component of EGMRI) state
+message EgmriMCIState
+{
+    enum MCIStateType {
+        MCI_UNDEFINED = 0;
+        MCI_ERROR     = 1;
+        MCI_STOPPED   = 2;
+        MCI_RUNNING   = 3;
+    }
+
+    required MCIStateType state = 1 [default = MCI_UNDEFINED];
+}
+
+// RAPID execution state
+message EgmriRapidCtrlExecState
+{
+    enum RapidCtrlExecStateType {
+        RAPID_UNDEFINED = 0;
+        RAPID_STOPPED   = 1;
+        RAPID_RUNNING   = 2;
+    };
+
+    required RapidCtrlExecStateType state = 1  [default = RAPID_UNDEFINED];
+}
+
+// Test signals, for future use 
+message EgmriTestSignals
+{
+    repeated float signals = 1;
+}
+
+
+
+
+//======================================================================================================
+// Primary components
+//======================================================================================================
+
+// Robot controller outbound message, sent to sensor
+message EgmriRobot
+{
+    optional EgmriHeader             header            = 1;
+    optional EgmriFeedback           feedback          = 2;
+    optional EgmriPlanned            planned           = 3;
+    optional EgmriPIDParameters      pidParameters     = 4;
+
+    optional EgmriMotorState         motorState        = 5;
+    optional EgmriMCIState           mciState          = 6;
+    optional bool                    mciConvergenceMet = 7;
+    optional EgmriTestSignals        testSignals       = 8;
+    optional EgmriRapidCtrlExecState rapidExecState    = 9; 
+}
+
+// Robot controller inbound message, sent from sensor
+message EgmriSensor
+{
+    optional EgmriHeader        header             = 1;
+    optional EgmriJointSpace    positionReferences = 2; // Units: deg
+    optional EgmriJointSpace    speedReferences    = 3; // Units: deg/s
+    optional EgmriJointSpace    torqueFeedforward  = 4; // Units: Nm
+    optional EgmriPIDParameters pidParameters      = 5;
+}
\ No newline at end of file

robot_message.cpp

+#include "robot_message.h"
+
+#include <iostream>
+#include <math.h>
+#include "egmri.pb.h"
+
+using abb::egmri::EgmriRobot;
+using abb::egmri::EgmriHeader;
+using abb::egmri::EgmriHeader_MessageType_MSGTYPE_DATA;
+using abb::egmri::EgmriPlanned;
+using abb::egmri::EgmriJointSpace;
+using abb::egmri::EgmriJoints;
+using abb::egmri::EgmriFeedback;
+using abb::egmri::EgmriPIDParameters;
+
+egmri_robot_message parse_robot_message(const char *data, size_t length) {
+  EgmriRobot robot;
+  robot.ParseFromArray(data, length);
+
+  // cerr << "has_header: " << robot.has_header() << endl;
+  // cerr << "has_feedback: " << robot.has_feedback() << endl;
+  // cerr << "has_planned: " << robot.has_planned() << endl;
+  // cerr << "has_pidparameters: " << robot.has_pidparameters() << endl;
+  // cerr << "has_motorstate: " << robot.has_motorstate() << endl;
+  // cerr << "has_mcistate: " << robot.has_mcistate() << endl;
+  // cerr << "has_testsignals: " << robot.has_testsignals() << endl;
+  // cerr << "has_rapidexecstate: " << robot.has_rapidexecstate() << endl;
+  // cerr << "has_mciconvergencemet: " << robot.has_mciconvergencemet() << endl;
+
+  // Header
+  assert(robot.has_header());  
+  EgmriHeader header = robot.header();
+  assert(header.has_seqno());
+  assert(header.has_tm());
+  assert(header.has_mtype());
+  assert(header.mtype() == EgmriHeader_MessageType_MSGTYPE_DATA);
+
+  // Planned (reference)
+  assert(robot.has_planned());
+  EgmriPlanned planned = robot.planned();
+  assert(planned.has_cartesianpose());
+  assert(planned.has_jointposition());
+  assert(planned.has_jointspeed());
+  EgmriJointSpace ref_positionspace = planned.jointposition();
+  assert(ref_positionspace.has_joints());
+  assert(ref_positionspace.has_externaljoints());
+  EgmriJoints ref_position = ref_positionspace.joints();
+  EgmriJoints ref_position_external = ref_positionspace.externaljoints();
+  assert(ref_position.joints_size() == 6);
+  assert(ref_position_external.joints_size() == 1);
+  EgmriJointSpace ref_speedspace = planned.jointspeed();
+  assert(ref_speedspace.has_joints());
+  assert(ref_speedspace.has_externaljoints());
+  EgmriJoints ref_speed = ref_speedspace.joints();
+  EgmriJoints ref_speed_external = ref_speedspace.externaljoints();
+  assert(ref_speed.joints_size() == 6);
+  assert(ref_speed_external.joints_size() == 1);
+
+  // Feedback (actual)
+  assert(robot.has_feedback());
+  EgmriFeedback feedback = robot.feedback();
+  // assert(feedback.has_cartesianpose())
+  assert(feedback.has_jointposition());
+  assert(feedback.has_jointspeed());
+  assert(feedback.has_jointtorque());
+  EgmriJointSpace act_positionspace = feedback.jointposition();
+  assert(act_positionspace.has_joints());
+  assert(act_positionspace.has_externaljoints());
+  EgmriJoints act_position = act_positionspace.joints();
+  EgmriJoints act_position_external = act_positionspace.externaljoints();
+  assert(act_position.joints_size() == 6);
+  assert(act_position_external.joints_size() == 1);
+  EgmriJointSpace act_speedspace = feedback.jointspeed();
+  assert(act_speedspace.has_joints());
+  assert(act_speedspace.has_externaljoints());
+  EgmriJoints act_speed = act_speedspace.joints();
+  EgmriJoints act_speed_external = act_speedspace.externaljoints();
+  assert(act_speed.joints_size() == 6);
+  assert(act_speed_external.joints_size() == 1);
+  EgmriJointSpace act_torquespace = feedback.jointtorque();
+  assert(act_torquespace.has_joints());
+  assert(act_torquespace.has_externaljoints());
+  EgmriJoints act_torque = act_torquespace.joints();
+  EgmriJoints act_torque_external = act_torquespace.externaljoints();
+  assert(act_torque.joints_size() == 6);
+  assert(act_torque_external.joints_size() == 1);
+
+  // PID parameters
+  assert(robot.has_pidparameters());
+  EgmriPIDParameters pidparameters = robot.pidparameters();
+  assert(pidparameters.has_kp());
+  assert(pidparameters.has_kv());
+  assert(pidparameters.has_ki());
+  EgmriJointSpace kpspace = pidparameters.kp();
+  assert(kpspace.has_joints());
+  assert(kpspace.has_externaljoints());
+  EgmriJoints kp = kpspace.joints();
+  EgmriJoints kp_external = kpspace.externaljoints();
+  assert(kp.joints_size() == 6);
+  assert(kp_external.joints_size() == 1);
+  EgmriJointSpace kvspace = pidparameters.kv();
+  assert(kvspace.has_joints());
+  assert(kvspace.has_externaljoints());
+  EgmriJoints kv = kvspace.joints();
+  EgmriJoints kv_external = kvspace.externaljoints();
+  assert(kv.joints_size() == 6);
+  assert(kv_external.joints_size() == 1);
+  EgmriJointSpace kispace = pidparameters.ki();
+  assert(kispace.has_joints());
+  assert(kispace.has_externaljoints());
+  EgmriJoints ki = kispace.joints();
+  EgmriJoints ki_external = kispace.externaljoints();
+  assert(ki.joints_size() == 6);
+  assert(ki_external.joints_size() == 1);
+
+  struct egmri_robot_message robot_message;
+  robot_message.seqno = header.seqno();
+  robot_message.tm = header.tm();
+  for (int i = 0 ; i < 3 ; i++) {
+    robot_message.joint[i].pid.kp = kp.joints(i);
+    robot_message.joint[i].pid.kv = kv.joints(i);
+    robot_message.joint[i].pid.ki = ki.joints(i);
+    robot_message.joint[i].ref.position = ref_position.joints(i);
+    robot_message.joint[i].ref.speed = ref_speed.joints(i);
+    robot_message.joint[i].act.position = act_position.joints(i);
+    robot_message.joint[i].act.speed = act_speed.joints(i);
+    robot_message.joint[i].act.torque = act_torque.joints(i);
+  }
+  robot_message.joint[3].pid.kp = kp_external.joints(0);
+  robot_message.joint[3].pid.kv = kv_external.joints(0);
+  robot_message.joint[3].pid.ki = ki_external.joints(0);
+  robot_message.joint[3].ref.position = ref_position_external.joints(0);
+  robot_message.joint[3].ref.speed = ref_speed_external.joints(0);
+  robot_message.joint[3].act.position = act_position_external.joints(0);
+  robot_message.joint[3].act.speed = act_speed_external.joints(0);
+  robot_message.joint[3].act.torque = act_torque_external.joints(0);
+  for (int i = 4 ; i < 7 ; i++) {
+    robot_message.joint[i].pid.kp = kp.joints(i - 1);
+    robot_message.joint[i].pid.kv = kv.joints(i - 1);
+    robot_message.joint[i].pid.ki = ki.joints(i - 1);
+    robot_message.joint[i].ref.position = ref_position.joints(i - 1);
+    robot_message.joint[i].ref.speed = ref_speed.joints(i - 1);
+    robot_message.joint[i].act.position = act_position.joints(i - 1);
+    robot_message.joint[i].act.speed = act_speed.joints(i - 1);
+    robot_message.joint[i].act.torque = act_torque.joints(i - 1);
+  }
+  return robot_message;
+}
+
+void print_robot_message(egmri_robot_message data)
+{
+  std::cerr << "seqno: " << data.seqno << " tm: " << data.tm << std::endl;
+  for (int i = 0 ; i < 7 ; i++) {
+    std::cerr << " " << i
+              << "("   << data.joint[i].ref.position * RAD_TO_DEG
+              << ", " << data.joint[i].act.position * RAD_TO_DEG
+              << "),(" << data.joint[i].ref.speed * RAD_TO_DEG
+              << "," << data.joint[i].act.speed * RAD_TO_DEG
+              << "),(-," << data.joint[i].act.torque
+              << ")" << std::endl;
+  }
+}
+

robot_message.h

+#ifndef __ROBOT_MESSAGE_H__
+#define __ROBOT_MESSAGE_H__
+
+#include "egmri.pb.h"
+
+#define RAD_TO_DEG (180.0 / M_PI)
+
+struct egmri_robot_message {
+  uint32_t seqno;
+  uint32_t tm;
+  struct {
+    struct {
+      float kp;
+      float kv;
+      float ki;
+    } pid;
+    struct {
+      float position;
+      float speed;
+    } ref;
+    struct {
+      float position;
+      float speed;
+      float torque;
+    } act;
+  } joint[7];
+};
+
+egmri_robot_message parse_robot_message(const char *data, size_t length);
+void print_robot_message(egmri_robot_message data);
+
+#endif

sensor_message.cpp

+#include "sensor_message.h"
+
+#include <iostream>
+#include <math.h>
+#include "egmri.pb.h"
+
+using abb::egmri::EgmriHeader;
+using abb::egmri::EgmriHeader_MessageType_MSGTYPE_CORRECTION;
+using abb::egmri::EgmriJointSpace;
+using abb::egmri::EgmriJoints;
+using abb::egmri::EgmriPIDParameters;
+using abb::egmri::EgmriSensor;
+
+#define DEG_TO_RAD (M_PI / 180.0)
+#define RAD_TO_DEG (180.0 / M_PI)
+
+egmri_sensor_message parse_sensor_message(const char *data, size_t length) {
+  EgmriSensor sensor;
+  sensor.ParseFromArray(data, length);
+  return to_sensor_message(sensor);
+}
+
+egmri_sensor_message to_sensor_message(EgmriSensor sensor) {
+  // Header
+  assert(sensor.has_header());  
+  EgmriHeader header = sensor.header();
+  assert(header.has_seqno());
+  assert(header.has_tm());
+  assert(header.has_mtype());
+  assert(header.mtype() == EgmriHeader_MessageType_MSGTYPE_CORRECTION);
+
+  // Position 
+  assert(sensor.has_positionreferences());
+  EgmriJointSpace ref_positionspace = sensor.positionreferences();
+  assert(ref_positionspace.has_joints());
+  assert(ref_positionspace.has_externaljoints());
+  EgmriJoints ref_position = ref_positionspace.joints();
+  EgmriJoints ref_position_external = ref_positionspace.externaljoints();
+  assert(ref_position.joints_size() == 6);
+  assert(ref_position_external.joints_size() == 1);
+
+  // Speed
+  assert(sensor.has_speedreferences());
+  EgmriJointSpace ref_speedspace = sensor.speedreferences();
+  assert(ref_speedspace.has_joints());
+  assert(ref_speedspace.has_externaljoints());
+  EgmriJoints ref_speed = ref_speedspace.joints();
+  EgmriJoints ref_speed_external = ref_speedspace.externaljoints();
+  assert(ref_speed.joints_size() == 6);
+  assert(ref_speed_external.joints_size() == 1);
+
+  // Torque
+  assert(sensor.has_torquefeedforward());
+  EgmriJointSpace ref_torquespace = sensor.torquefeedforward();
+  assert(ref_torquespace.has_joints());
+  assert(ref_torquespace.has_externaljoints());
+  EgmriJoints ref_torque = ref_torquespace.joints();
+  EgmriJoints ref_torque_external = ref_torquespace.externaljoints();
+  assert(ref_torque.joints_size() == 6);
+  assert(ref_torque_external.joints_size() == 1);
+
+  // PID parameters
+  assert(sensor.has_pidparameters());
+  EgmriPIDParameters pidparameters = sensor.pidparameters();
+  assert(pidparameters.has_kp());
+  assert(pidparameters.has_kv());
+  assert(pidparameters.has_ki());
+  EgmriJointSpace kpspace = pidparameters.kp();
+  assert(kpspace.has_joints());
+  assert(kpspace.has_externaljoints());
+  EgmriJoints kp = kpspace.joints();
+  EgmriJoints kp_external = kpspace.externaljoints();
+  assert(kp.joints_size() == 6);
+  assert(kp_external.joints_size() == 1);
+  EgmriJointSpace kvspace = pidparameters.kv();
+  assert(kvspace.has_joints());
+  assert(kvspace.has_externaljoints());
+  EgmriJoints kv = kvspace.joints();
+  EgmriJoints kv_external = kvspace.externaljoints();
+  assert(kv.joints_size() == 6);
+  assert(kv_external.joints_size() == 1);
+  EgmriJointSpace kispace = pidparameters.ki();
+  assert(kispace.has_joints());
+  assert(kispace.has_externaljoints());
+  EgmriJoints ki = kispace.joints();
+  EgmriJoints ki_external = kispace.externaljoints();
+  assert(ki.joints_size() == 6);
+  assert(ki_external.joints_size() == 1);
+  
+  struct egmri_sensor_message sensor_message;
+  sensor_message.seqno = header.seqno();
+  sensor_message.tm = header.tm();
+  for (int i = 0 ; i < 3 ; i++) {
+    sensor_message.joint[i].pid.kp = kp.joints(i);
+    sensor_message.joint[i].pid.kv = kv.joints(i);
+    sensor_message.joint[i].pid.ki = ki.joints(i);
+    sensor_message.joint[i].ref.position = ref_position.joints(i) * DEG_TO_RAD;
+    sensor_message.joint[i].ref.speed = ref_speed.joints(i) * DEG_TO_RAD;
+    sensor_message.joint[i].ref.torque = ref_torque.joints(i);
+  }
+  sensor_message.joint[3].pid.kp = kp_external.joints(0);
+  sensor_message.joint[3].pid.kv = kv_external.joints(0);
+  sensor_message.joint[3].pid.ki = ki_external.joints(0);
+  sensor_message.joint[3].ref.position = ref_position_external.joints(0) * DEG_TO_RAD;
+  sensor_message.joint[3].ref.speed = ref_speed_external.joints(0) * DEG_TO_RAD;
+  sensor_message.joint[3].ref.torque = ref_torque_external.joints(0);
+  for (int i = 4 ; i < 7 ; i++) {
+    sensor_message.joint[i].pid.kp = kp.joints(i - 1);
+    sensor_message.joint[i].pid.kv = kv.joints(i - 1);
+    sensor_message.joint[i].pid.ki = ki.joints(i - 1);
+    sensor_message.joint[i].ref.position = ref_position.joints(i - 1) * DEG_TO_RAD;
+    sensor_message.joint[i].ref.speed = ref_speed.joints(i - 1) * DEG_TO_RAD;
+    sensor_message.joint[i].ref.torque = ref_torque.joints(i - 1);
+  }
+  return sensor_message;
+}
+
+EgmriSensor encode_sensor_message(egmri_sensor_message sensor_message)
+{
+  EgmriSensor sensor;
+  sensor.mutable_header()->set_seqno(sensor_message.seqno);
+  sensor.mutable_header()->set_tm(sensor_message.tm);
+  sensor.mutable_header()->set_mtype(EgmriHeader_MessageType_MSGTYPE_CORRECTION);
+  for (int i = 0 ; i < 3 ; i++) {
+    sensor.mutable_pidparameters()->mutable_kp()->mutable_joints()->add_joints(
+      sensor_message.joint[i].pid.kp);
+    sensor.mutable_pidparameters()->mutable_kv()->mutable_joints()->add_joints(
+      sensor_message.joint[i].pid.kv);
+    sensor.mutable_pidparameters()->mutable_ki()->mutable_joints()->add_joints(
+      sensor_message.joint[i].pid.ki);
+    sensor.mutable_positionreferences()->mutable_joints()->add_joints(
+      sensor_message.joint[i].ref.position * RAD_TO_DEG);
+    sensor.mutable_speedreferences()->mutable_joints()->add_joints(
+      sensor_message.joint[i].ref.speed * RAD_TO_DEG);
+    sensor.mutable_torquefeedforward()->mutable_joints()->add_joints(
+      sensor_message.joint[i].ref.torque);
+  }
+  sensor.mutable_pidparameters()->mutable_kp()->mutable_externaljoints()->add_joints(
+    sensor_message.joint[3].pid.kp);
+  sensor.mutable_pidparameters()->mutable_kv()->mutable_externaljoints()->add_joints(
+    sensor_message.joint[3].pid.kv);
+  sensor.mutable_pidparameters()->mutable_ki()->mutable_externaljoints()->add_joints(
+      sensor_message.joint[3].pid.ki);
+  sensor.mutable_positionreferences()->mutable_externaljoints()->add_joints(
+    sensor_message.joint[3].ref.position * RAD_TO_DEG);
+  sensor.mutable_speedreferences()->mutable_externaljoints()->add_joints(
+    sensor_message.joint[3].ref.speed * RAD_TO_DEG);
+  sensor.mutable_torquefeedforward()->mutable_externaljoints()->add_joints(
+    sensor_message.joint[3].ref.torque);
+  for (int i = 4 ; i < 7 ; i++) {
+    sensor.mutable_pidparameters()->mutable_kp()->mutable_joints()->add_joints(
+      sensor_message.joint[i].pid.kp);
+    sensor.mutable_pidparameters()->mutable_kv()->mutable_joints()->add_joints(
+      sensor_message.joint[i].pid.kv);
+    sensor.mutable_pidparameters()->mutable_ki()->mutable_joints()->add_joints(
+      sensor_message.joint[i].pid.ki);
+    sensor.mutable_positionreferences()->mutable_joints()->add_joints(
+      sensor_message.joint[i].ref.position * RAD_TO_DEG);
+    sensor.mutable_speedreferences()->mutable_joints()->add_joints(
+      sensor_message.joint[i].ref.speed