diff --git a/include/abb_libegm/egm_common.h b/include/abb_libegm/egm_common.h index 5014170..21ae83b 100644 --- a/include/abb_libegm/egm_common.h +++ b/include/abb_libegm/egm_common.h @@ -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; }; }; diff --git a/include/abb_libegm/egm_common_auxiliary.h b/include/abb_libegm/egm_common_auxiliary.h index 26b3a56..3051e7e 100644 --- a/include/abb_libegm/egm_common_auxiliary.h +++ b/include/abb_libegm/egm_common_auxiliary.h @@ -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. * diff --git a/proto/egm.proto b/proto/egm.proto index 123f9d6..7fab8dc 100644 --- a/proto/egm.proto +++ b/proto/egm.proto @@ -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 @@ -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 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 { required double x = 1; required double y = 2; @@ -31,7 +31,7 @@ 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; @@ -39,14 +39,20 @@ message EgmQuaternion // Quaternion orientation interpreted relative to the se 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; @@ -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; } message EgmSpeedRef // Speed reference values for robot (joint or cartesian) and additional axis (array of 6 values) @@ -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 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 @@ -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; @@ -146,13 +158,14 @@ message EgmRobot optional EgmMotorState motorState = 4; optional EgmMCIState mciState = 5; - optional bool mciConvergenceMet = 6; + optional bool mciConvergenceMet = 6; 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; @@ -160,7 +173,7 @@ message EgmSensor 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; diff --git a/proto/egm_wrapper.proto b/proto/egm_wrapper.proto index 86d04a4..96dace1 100644 --- a/proto/egm_wrapper.proto +++ b/proto/egm_wrapper.proto @@ -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. @@ -167,6 +174,7 @@ 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. @@ -174,6 +182,7 @@ message Planned { optional Robot robot = 1; optional External external = 2; + optional Clock time = 3; } //====================================================================================================================== @@ -196,4 +205,4 @@ message Output { optional Robot robot = 1; // Outputs to the robot. optional External external = 2; // Outputs to external axes. -} \ No newline at end of file +} diff --git a/src/egm_base_interface.cpp b/src/egm_base_interface.cpp index 6022603..f35245b 100644 --- a/src/egm_base_interface.cpp +++ b/src/egm_base_interface.cpp @@ -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); + } + + 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; } diff --git a/src/egm_common.cpp b/src/egm_common.cpp index ae1d69d..13eec24 100644 --- a/src/egm_common.cpp +++ b/src/egm_common.cpp @@ -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 \ No newline at end of file +} // end namespace abb diff --git a/src/egm_common_auxiliary.cpp b/src/egm_common_auxiliary.cpp index 3d58422..fcf3347 100644 --- a/src/egm_common_auxiliary.cpp +++ b/src/egm_common_auxiliary.cpp @@ -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, @@ -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; @@ -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; @@ -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()); + } } } @@ -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; } diff --git a/src/egm_interpolator.cpp b/src/egm_interpolator.cpp index a7175a0..932fbf8 100644 --- a/src/egm_interpolator.cpp +++ b/src/egm_interpolator.cpp @@ -143,7 +143,7 @@ void EGMInterpolator::SplinePolynomial::update(const SplineConditions& condition // E.g. for stopping use K = 0.0. // //--------------------- - // Conditons: + // Conditions: //--------------------- // 0 < t <= T // @@ -171,7 +171,7 @@ void EGMInterpolator::SplinePolynomial::update(const SplineConditions& condition // S(t) = A + B*t // //--------------------- - // Conditons: + // Conditions: //--------------------- // 0 <= t <= T // @@ -194,7 +194,7 @@ void EGMInterpolator::SplinePolynomial::update(const SplineConditions& condition // S(t) = A + B*t + C*t^2 // //--------------------- - // Conditons: + // Conditions: //--------------------- // 0 <= t <= T // @@ -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 // @@ -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 //