Commit 5c2f9c3c authored by Anders Blomdell's avatar Anders Blomdell
Browse files

Standalone version copied out of labcomm2egmri

parents
*~
*.o
*.pb.h
*.pb.cc
timingtest
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)
#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
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
#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;
}
}
#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
#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(