Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Migration to latest rw #32

Merged
merged 4 commits into from
Feb 21, 2019
Merged

Migration to latest rw #32

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
3e852f7
Update to manage latest EGM protocol version
jontje Feb 15, 2019
602f4f7
Updated sample time estimation
jontje Feb 20, 2019
e135a49
Update proto/egm_wrapper.proto
gavanderhoorn Feb 21, 2019
0f46345
Update src/egm_common.cpp
gavanderhoorn Feb 21, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 5 additions & 0 deletions include/abb_libegm/egm_common.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -119,6 +119,11 @@ struct Constants
* \brief Conversion value from milliseconds to seconds.
*/
static const double MS_TO_S;

/**
* \brief Conversion value from seconds to microseconds.
*/
static const double S_TO_US;
gavanderhoorn marked this conversation as resolved.
Show resolved Hide resolved
};
};

Expand Down
10 changes: 10 additions & 0 deletions include/abb_libegm/egm_common_auxiliary.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -321,6 +321,16 @@ bool parse(wrapper::Header* p_target, const EgmHeader& source);
*/
bool parse(wrapper::Status* p_target, const EgmRobot& source);

/**
* \brief Parse an abb::egm::EgmClock object.
*
* \param p_target for containing the parsed data.
* \param source containing data to parse.
*
* \return bool indicating if the parsing was successful or not.
*/
bool parse(wrapper::Clock* p_target, const EgmClock& source);

/**
* \brief Parse two abb::egm::EgmJoints objects.
*
Expand Down
57 changes: 35 additions & 22 deletions proto/egm.proto
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Definition of ABB sensor interface V1.0
// Definition of ABB sensor interface V1.1
//
// messages of type EgmRobot are sent out from the robot controller
// messages of type EgmSensor are sent to the robot controller
Expand All @@ -8,20 +8,20 @@ package abb.egm;
message EgmHeader
{
optional uint32 seqno = 1; // sequence number (to be able to find lost messages)
optional uint32 tm = 2; // time stamp in ms
optional uint32 tm = 2; // controller send time stamp in ms
gavanderhoorn marked this conversation as resolved.
Show resolved Hide resolved

enum MessageType {
MSGTYPE_UNDEFINED = 0;
MSGTYPE_COMMAND = 1; // for future use
MSGTYPE_DATA = 2; // sent by robot controller
MSGTYPE_CORRECTION = 3; // sent by sensor
MSGTYPE_PATH_CORRECTION = 4; // sent by sensor
MSGTYPE_CORRECTION = 3; // sent by sensor for position guidance
MSGTYPE_PATH_CORRECTION = 4; // sent by sensor for path correction
}

optional MessageType mtype = 3 [default = MSGTYPE_UNDEFINED];
}

message EgmCartesian // Cartesian position in mm, interpreted relative to the sensor frame defined by EGMActPose
message EgmCartesian // Cartesian position in mm
gavanderhoorn marked this conversation as resolved.
Show resolved Hide resolved
{
required double x = 1;
required double y = 2;
Expand All @@ -31,22 +31,28 @@ message EgmCartesian // Cartesian position in mm, interpreted relative to the
// If you have pose input, i.e. not joint input, you can choose to send orientation data as quaternion or as Euler angles.
// If both are sent, Euler angles have higher priority.

message EgmQuaternion // Quaternion orientation interpreted relative to the sensor frame defined by EGMActPose
message EgmQuaternion // Quaternion orientation
{
required double u0 = 1;
required double u1 = 2;
required double u2 = 3;
required double u3 = 4;
}

message EgmEuler // Euler angle orientation in degrees, interpreted relative to the sensor frame defined by EGMActPose
message EgmEuler // Euler angle orientation in degrees
{
required double x = 1;
required double y = 2;
required double z = 3;
}

message EgmPose // Pose (i.e. cartesian position and Quaternion orientation) interpreted relative to the sensor frame defined by EGMActPose
message EgmClock // Time in seconds and microseconds since 1 Jan 1970
{
required uint64 sec = 1;
required uint64 usec = 2;
}

message EgmPose // Pose (i.e. cartesian position and Quaternion orientation) relative to the correction frame defined by EGMActPose
{
optional EgmCartesian pos = 1;
optional EgmQuaternion orient = 2;
Expand All @@ -63,16 +69,17 @@ message EgmJoints // Array of 6 joint values for TCP robot in degrees
repeated double joints = 1;
}

message EgmExternalJoints // Array of 6 joint values for additional axis in degrees
message EgmExternalJoints // Array of 6 joint values for additional axis, in degrees for rotating axis, in mm for linear axis
{
repeated double joints = 1;
}

message EgmPlanned // Planned position for robot (joints or cartesian) and additional axis (array of 6 values)
{
optional EgmJoints joints = 1;
optional EgmPose cartesian = 2;
optional EgmJoints externalJoints = 3;
{ // Is used for position streaming (source: controller) and position guidance (source: sensor)
optional EgmJoints joints = 1;
optional EgmPose cartesian = 2;
optional EgmJoints externalJoints = 3;
optional EgmClock time = 4;
gavanderhoorn marked this conversation as resolved.
Show resolved Hide resolved
}

message EgmSpeedRef // Speed reference values for robot (joint or cartesian) and additional axis (array of 6 values)
Expand All @@ -85,16 +92,17 @@ message EgmSpeedRef // Speed reference values for robot (joint or cartesian)

message EgmPathCorr // Cartesian path correction and measurment age
{
required EgmCartesian pos = 1; // Sensor measurement (x, y, z)
required EgmCartesian pos = 1; // Sensor measurement (x, y, z) relative the sensor tool coordinate system
gavanderhoorn marked this conversation as resolved.
Show resolved Hide resolved
required uint32 age = 2; // Sensor measurement age in ms
}


message EgmFeedBack // Feed back position, i.e. actual measured position for robot (joints or cartesian) and additional axis (array of 6 values)
{
optional EgmJoints joints = 1;
optional EgmPose cartesian = 2;
optional EgmJoints externalJoints = 3;
optional EgmJoints joints = 1;
optional EgmPose cartesian = 2;
optional EgmJoints externalJoints = 3;
optional EgmClock time = 4;
}

message EgmMotorState // Motor state
Expand Down Expand Up @@ -136,8 +144,12 @@ message EgmTestSignals // Test signals
repeated double signals = 1;
}

message EgmMeasuredForce // Array of 6 force values for a robot
{
repeated double force = 1;
}

// Robot controller outbound message
// Robot controller outbound message, sent from the controller to the sensor during position guidance and position streaming
message EgmRobot
{
optional EgmHeader header = 1;
Expand All @@ -146,21 +158,22 @@ message EgmRobot

optional EgmMotorState motorState = 4;
optional EgmMCIState mciState = 5;
optional bool mciConvergenceMet = 6;
optional bool mciConvergenceMet = 6;
gavanderhoorn marked this conversation as resolved.
Show resolved Hide resolved
optional EgmTestSignals testSignals = 7;
optional EgmRapidCtrlExecState rapidExecState = 8;
optional EgmRapidCtrlExecState rapidExecState = 8;
optional EgmMeasuredForce measuredForce = 9;
}


// Robot controller inbound message, sent from sensor
// Robot controller inbound message, sent from sensor to the controller during position guidance
message EgmSensor
{
optional EgmHeader header = 1;
optional EgmPlanned planned = 2;
optional EgmSpeedRef speedRef = 3;
}

// Robot controller inbound message, sent from sensor
// Robot controller inbound message, sent from sensor during path correction
message EgmSensorPathCorr
{
optional EgmHeader header = 1;
Expand Down
11 changes: 10 additions & 1 deletion proto/egm_wrapper.proto
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -61,6 +61,13 @@ message Status
optional RAPIDExecutionState rapid_execution_state = 4 [default = RAPID_UNDEFINED];
}

// Note: Time in seconds and microseconds since the epoch (1 Jan 1970)
message Clock
{
optional uint64 sec = 1;
optional uint64 usec = 2;
}

//===========================================================
//
// Joint space related messages.
Expand Down Expand Up @@ -167,13 +174,15 @@ message Feedback
{
optional Robot robot = 1;
optional External external = 2;
optional Clock time = 3;
}

// Note: The velocity sub fields are estimated from the actual EGM messages.
message Planned
{
optional Robot robot = 1;
optional External external = 2;
optional Clock time = 3;
}

//======================================================================================================================
Expand All @@ -196,4 +205,4 @@ message Output
{
optional Robot robot = 1; // Outputs to the robot.
optional External external = 2; // Outputs to external axes.
}
}
21 changes: 15 additions & 6 deletions src/egm_base_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -125,13 +125,22 @@ double EGMBaseInterface::InputContainer::estimateSampleTime()
{
double estimate = 0.0;

if (current_.has_header() && previous_.has_header())
{
double temp = (double) (current_.header().time_stamp() - previous_.header().time_stamp());
estimate = temp*Constants::Conversion::MS_TO_S;
if (current_.has_feedback() && previous_.has_feedback() &&
current_.feedback().has_time() && previous_.feedback().has_time() &&
current_.feedback().time().has_sec() && previous_.feedback().time().has_sec() &&
current_.feedback().time().has_usec() && previous_.feedback().time().has_usec())
{
google::protobuf::uint64 diff_s = (current_.feedback().time().sec() - previous_.feedback().time().sec());
google::protobuf::uint64 diff_us = (current_.feedback().time().usec() - previous_.feedback().time().usec());
if (diff_s > 0)
{
diff_us += diff_s*((google::protobuf::uint64) Constants::Conversion::S_TO_US);
}
gavanderhoorn marked this conversation as resolved.
Show resolved Hide resolved

estimate = std::floor(((double) diff_us) * Constants::Conversion::MS_TO_S) * Constants::Conversion::MS_TO_S;
}
if (estimate <= 0.0)

if (estimate < Constants::RobotController::LOWEST_SAMPLE_TIME)
{
estimate = Constants::RobotController::LOWEST_SAMPLE_TIME;
}
Expand Down
3 changes: 2 additions & 1 deletion src/egm_common.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -61,6 +61,7 @@ const double Constants::Conversion::RAD_TO_DEG = 180.0 / M_PI;
const double Constants::Conversion::DEG_TO_RAD = M_PI / 180.0;
const double Constants::Conversion::MM_TO_M = 0.001;
const double Constants::Conversion::MS_TO_S = 0.001;
const double Constants::Conversion::S_TO_US = 1e6;

} // end namespace egm
} // end namespace abb
} // end namespace abb
48 changes: 38 additions & 10 deletions src/egm_common_auxiliary.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -691,6 +691,23 @@ bool parse(wrapper::Status* p_target, const EgmRobot& source)
return success;
}

bool parse(wrapper::Clock* p_target, const EgmClock& source)
{
bool success = true;

if (p_target && source.has_sec() && source.has_usec())
{
p_target->set_sec(source.sec());
p_target->set_usec(source.usec());
}
else
{
success = false;
}

return success;
}

bool parse(wrapper::Joints* p_target_robot,
wrapper::Joints* p_target_external,
const EgmJoints& source_robot,
Expand All @@ -712,12 +729,12 @@ bool parse(wrapper::Joints* p_target_robot,
{
for (int i = 0; i < source_robot.joints_size(); ++i)
{
p_target_robot->add_values(source_robot.joints(i) * Constants::Conversion::RAD_TO_DEG);
p_target_robot->add_values(source_robot.joints(i));
}

for (int i = 0; i < source_external.joints_size(); ++i)
{
p_target_external->add_values(source_external.joints(i) * Constants::Conversion::RAD_TO_DEG);
p_target_external->add_values(source_external.joints(i));
}

success = true;
Expand All @@ -731,17 +748,17 @@ bool parse(wrapper::Joints* p_target_robot,
if (source_robot.joints_size() == Constants::RobotController::DEFAULT_NUMBER_OF_ROBOT_JOINTS &&
source_external.joints_size() >= 1)
{
p_target_robot->add_values(source_robot.joints(0) * Constants::Conversion::RAD_TO_DEG);
p_target_robot->add_values(source_robot.joints(1) * Constants::Conversion::RAD_TO_DEG);
p_target_robot->add_values(source_external.joints(0) * Constants::Conversion::RAD_TO_DEG);
p_target_robot->add_values(source_robot.joints(2) * Constants::Conversion::RAD_TO_DEG);
p_target_robot->add_values(source_robot.joints(3) * Constants::Conversion::RAD_TO_DEG);
p_target_robot->add_values(source_robot.joints(4) * Constants::Conversion::RAD_TO_DEG);
p_target_robot->add_values(source_robot.joints(5) * Constants::Conversion::RAD_TO_DEG);
p_target_robot->add_values(source_robot.joints(0));
p_target_robot->add_values(source_robot.joints(1));
p_target_robot->add_values(source_external.joints(0));
p_target_robot->add_values(source_robot.joints(2));
p_target_robot->add_values(source_robot.joints(3));
p_target_robot->add_values(source_robot.joints(4));
p_target_robot->add_values(source_robot.joints(5));

for (int i = 1; i < source_external.joints_size(); ++i)
{
p_target_external->add_values(source_external.joints(i) * Constants::Conversion::RAD_TO_DEG);
p_target_external->add_values(source_external.joints(i));
}

success = true;
Expand Down Expand Up @@ -821,6 +838,11 @@ bool parse(wrapper::Feedback* p_target, const EgmFeedBack& source, const RobotAx
if (success)
{
success = parse(p_target->mutable_robot()->mutable_cartesian()->mutable_pose(), source.cartesian());

if (success)
{
success = parse(p_target->mutable_time(), source.time());
}
}
}

Expand All @@ -840,8 +862,14 @@ bool parse(wrapper::Planned* p_target, const EgmPlanned& source, const RobotAxes
if (success)
{
success = parse(p_target->mutable_robot()->mutable_cartesian()->mutable_pose(), source.cartesian());

if (success)
{
success = parse(p_target->mutable_time(), source.time());
}
}
}

return success;
}

Expand Down
10 changes: 5 additions & 5 deletions src/egm_interpolator.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -143,7 +143,7 @@ void EGMInterpolator::SplinePolynomial::update(const SplineConditions& condition
// E.g. for stopping use K = 0.0.
//
//---------------------
// Conditons:
// Conditions:
//---------------------
// 0 < t <= T
//
Expand Down Expand Up @@ -171,7 +171,7 @@ void EGMInterpolator::SplinePolynomial::update(const SplineConditions& condition
// S(t) = A + B*t
//
//---------------------
// Conditons:
// Conditions:
//---------------------
// 0 <= t <= T
//
Expand All @@ -194,7 +194,7 @@ void EGMInterpolator::SplinePolynomial::update(const SplineConditions& condition
// S(t) = A + B*t + C*t^2
//
//---------------------
// Conditons:
// Conditions:
//---------------------
// 0 <= t <= T
//
Expand All @@ -219,7 +219,7 @@ void EGMInterpolator::SplinePolynomial::update(const SplineConditions& condition
// S(t) = A + B*t + C*t^2 + D*t^3
//
//---------------------
// Conditons:
// Conditions:
//---------------------
// 0 <= t <= T
//
Expand Down Expand Up @@ -249,7 +249,7 @@ void EGMInterpolator::SplinePolynomial::update(const SplineConditions& condition
// S(t) = A + B*t + C*t^2 + D*t^3 + E*t^4 + F*t^5
//
//---------------------
// Conditons:
// Conditions:
//---------------------
// 0 <= t <= T
//
Expand Down