From de513aac0457fac45594f798fa82923722fcb259 Mon Sep 17 00:00:00 2001 From: Chunting Date: Fri, 10 Nov 2017 17:59:27 -0500 Subject: [PATCH 001/102] KukaToFlatbuffer.hpp create some new toflatbuffer functions for KUKA --- example/fusionTrackExample.cpp | 3 +- include/grl/kuka/KukaToFlatbuffer.hpp | 102 ++++++++++++++++++++++++++ 2 files changed, 104 insertions(+), 1 deletion(-) create mode 100644 include/grl/kuka/KukaToFlatbuffer.hpp diff --git a/example/fusionTrackExample.cpp b/example/fusionTrackExample.cpp index 2c36c0e..2c8e209 100644 --- a/example/fusionTrackExample.cpp +++ b/example/fusionTrackExample.cpp @@ -167,7 +167,7 @@ int main(int argc, char **argv) // with update calls, so yield processor time with the // shortest possible sleep. If you call as fast as is possible // they will write to their .log file, causing all sorts of - // slowdowns and writing huge files to disk very fast. + // slowdowns and other problems. std::this_thread::yield(); } @@ -191,6 +191,7 @@ int main(int argc, char **argv) std::string binary_file_path = binary_file_prefix + std::to_string(buffer_num) + binary_file_suffix; std::string json_file_path = json_file_prefix + std::to_string(buffer_num) + json_file_suffix; std::cout << " fbb.GetSize(): " << fbb.GetSize() << std::endl; + std::cout << "FinishAndVerifyBuffer: " << success << std::endl; success = success && grl::SaveFlatBufferFile( fbb.GetBufferPointer(), diff --git a/include/grl/kuka/KukaToFlatbuffer.hpp b/include/grl/kuka/KukaToFlatbuffer.hpp new file mode 100644 index 0000000..da5d311 --- /dev/null +++ b/include/grl/kuka/KukaToFlatbuffer.hpp @@ -0,0 +1,102 @@ +#ifndef GRL_KUKA_TO_FLATBUFFER +#define GRL_KUKA_TO_FLATBUFFER + +#include "grl/flatbuffer/JointState_generated.h" +#include "grl/flatbuffer/ArmControlState_generated.h" +#include "grl/flatbuffer/KUKAiiwa_generated.h" +namespace grl { namespace robot { namespace arm { + +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const double x = 0.0, + const double y = 0.0, + const double z = 0.0) +{ + return grl::flatbuffer::CreateEulerTranslationParams(fbb, x, y, z); +} + +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const double r1 = 0.0, + const double r2 = 0.0, + const double r3 = 0.0, + grl::flatbuffer::EulerOrder &eulerOrder) +{ + return grl::flatbuffer::CreateEulerRotationParams(fbb, r1, r2, r3, eulerOrder); +} + +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const grl::flatbuffer::Vector3d &position, + const grl::flatbuffer::EulerRotation &rotation) +{ + return grl::flatbuffer::CreateEulerPoseParams(fbb, position, rotation); +} + +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const grl::flatbuffer::EulerPose &stiffness, + const grl::flatbuffer::EulerPose &damping, + const double nullspaceStiffness, + const double nullspaceDamping, + const grl::flatbuffer::EulerPose &maxPathDeviation, + const grl::flatbuffer::EulerPose &maxCartesianVelocity, + const grl::flatbuffer::EulerPose &maxControlForce, + const bool maxControlForceExceededStop) +{ + return grl::flatbuffer::CreateCartesianImpedenceControlMode( + fbb, + stiffness, + damping, + nullspaceStiffness, + nullspaceDamping, + maxPathDeviation, + maxCartesianVelocity, + maxControlForce, + maxControlForceExceededStop); +} + +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + std::vector &joint_stiffness, + std::vector &joint_damping) +{ + auto jointStiffnessBuffer = fbb.CreateVector(joint_stiffness.data(),joint_stiffness.size()); + auto jointDampingBuffer = fbb.CreateVector(joint_damping.data(),joint_damping.size()); + return grl::flatbuffer::CreateJointImpedenceControlMode(fbb, jointStiffnessBuffer, jointDampingBuffer); +} + +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + grl::flatbuffer::EOverlayType overlayType, + const int32_t sendPeriodMillisec, + const int32_t setReceiveMultiplier, + const bool updatePortOnRemote, + const int16_t portOnRemote, + const bool updatePortOnController, + const int16_t portOnController) +{ + + return grl::flatbuffer::CreateFRI( + fbb, + EOverlayType, + sendPeriodMillisec, + setReceiveMultiplier, + updatePortOnRemote, + portOnRemote, + updatePortOnController, + portOnController + ); +} + +// flatbuffers::Offset toFlatBuffer( ) +// { + +// } + +} // End of arm namespace +} // End of robot namespace +} // End of grl namespace + + +#endif // GRL_ATRACSYS_FUSION_TRACK_TO_FLATBUFFER From 18305eb5e4550cb4fb8d7721dc95964db679ff17 Mon Sep 17 00:00:00 2001 From: Chunting Date: Sat, 11 Nov 2017 15:59:24 -0500 Subject: [PATCH 002/102] KukaToFlatbuffer.hpp added some toFlatBuffer functions. --- include/grl/kuka/KukaToFlatbuffer.hpp | 146 +++++++++++++++++++++++++- 1 file changed, 144 insertions(+), 2 deletions(-) diff --git a/include/grl/kuka/KukaToFlatbuffer.hpp b/include/grl/kuka/KukaToFlatbuffer.hpp index da5d311..88c9f05 100644 --- a/include/grl/kuka/KukaToFlatbuffer.hpp +++ b/include/grl/kuka/KukaToFlatbuffer.hpp @@ -68,7 +68,7 @@ flatbuffers::Offset toFlatBuff flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder &fbb, - grl::flatbuffer::EOverlayType overlayType, + const ::OverlayType &overlayType, const int32_t sendPeriodMillisec, const int32_t setReceiveMultiplier, const bool updatePortOnRemote, @@ -76,7 +76,6 @@ flatbuffers::Offset toFlatBuffer( const bool updatePortOnController, const int16_t portOnController) { - return grl::flatbuffer::CreateFRI( fbb, EOverlayType, @@ -89,6 +88,149 @@ flatbuffers::Offset toFlatBuffer( ); } +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const std::vector &jointAccelerationRel, + const std::vector &jointVelocityRel, + const bool updateMinimumTrajectoryExecutionTime, + const double minimumTrajectoryExecutionTime) +{ + auto jointAccelerationRelBuffer = fbb.CreateVector(jointAccelerationRel.data(),jointAccelerationRel.size()); + auto jointVelocityRelBuffer = fbb.CreateVector(jointVelocityRel.data(),jointVelocityRel.size()); + return grl::flatbuffer::CreateSmartServo( + fbb, + jointAccelerationRelBuffer, + jointAccelerationRelBuffer, + updateMinimumTrajectoryExecutionTime, + minimumTrajectoryExecutionTime + ); +} + +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const std::string &dataType, + const std::string &defaultValue, + const std::string &displayName, + const std::string &id, + const std::string &min, + const std::string &max, + const std::string &unit, + const std::string &value, + bool shouldRemove = false, + bool shouldUpdate = false) +{ + auto jointAccelerationRelBuffer = fbb.CreateVector(jointAccelerationRel.data(),jointAccelerationRel.size()); + auto jointVelocityRelBuffer = fbb.CreateVector(jointVelocityRel.data(),jointVelocityRel.size()); + return grl::flatbuffer::CreateProcessData( + fbb, + fbb.CreateString(datatype), + fbb.CreateString(defaultValue), + fbb.CreateString(displayName), + fbb.CreateString(id), + fbb.CreateString(min), + fbb.CreateString(max), + fbb.CreateString(unit), + fbb.CreateString(value), + fbb.CreateString(datatype), + shouldRemove, + shouldUpdate + ); +} + + +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder fbb, + const std::string &name, + const std::string &parent, + const grl::flatbuffer::Pose &pose, + const grl::flatbuffer::Inertia &inertia) + { + return grl::flatbuffer::CreateLinkObject( + fbb, + fbb.CreateString(name), + fbb.CreateString(parent), + pose, + inertia + ); + } + + + + +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const std::string &name, + const grl::flatbuffer::KUKAiiwaInterface &commandInterface, + const grl::flatbuffer::KUKAiiwaInterface &monitorInterface, + // const grl::flatbuffer::ClientCommandMode clientCommandMode, + const ::ClientCommandMode &clientCommandMode, + const ::OverlayType &overlayType, + const ::ControlMode &controlMode, + flatbuffers::Offset &setCartImpedance, + flatbuffers::Offset &setJointImpedance, + flatbuffers::Offset &smartServoConfig, + flatbuffers::Offset &FRIConfig, + flatbuffers::Offset>> &tools, + flatbuffers::Offset>> &processData, + const std::string ¤tMotionCenter, + const bool requestMonitorProcessData) +{ + auto jointAccelerationRelBuffer = fbb.CreateVector(jointAccelerationRel.data(),jointAccelerationRel.size()); + auto jointVelocityRelBuffer = fbb.CreateVector(jointVelocityRel.data(),jointVelocityRel.size()); + return grl::flatbuffer::CreateKUKAiiwaArmConfiguration( + fbb, + fbb.CreateString(name), + commandInterface, + monitorInterface, + clientCommandMode, + overlayType, + controlMode, + setCartImpedance, + setJointImpedance, + smartServoConfig, + FRIConfig, + tools, + processData, + fbb.CreateString(currentMotionCenter), + requestMonitorProcessData); +} + +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const std::vector &position, + const std::vector &velocity, + const std::vector &acceleration, + const std::vector &torque) +{ + return grl::flatbuffer::CreateJointState( + fbb, + position ? fbb.CreateVector(position) : 0, + velocity ? fbb.CreateVector(velocity) : 0, + acceleration ? fbb.CreateVector(acceleration) : 0, + torque ? fbb.CreateVector(torque) : 0); +} +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder fbb, + flatbuffers::Offset &measuredState, + const grl::flatbuffer::Pose &cartesianFlangePose, + flatbuffers::Offset &jointStateReal, + flatbuffers::Offset &jointStateInterpolated, + flatbuffers::Offset &externalState, + const ::OperationMode &operationMode, + const grl::flatbuffer::Wrench &CartesianWrench) +{ + return grl::flatbuffer::CreateJointState( + fbb, + measuredState, + cartesianFlangePose, + jointStateReal, + jointStateInterpolated, + externalState, + operationMode, + CartesianWrench); +} + + // flatbuffers::Offset toFlatBuffer( ) // { From f84c435ae2c80031377214cd603f0f2f7a7f5917 Mon Sep 17 00:00:00 2001 From: Chunting Date: Sun, 12 Nov 2017 02:00:30 -0500 Subject: [PATCH 003/102] Added some helper functions. --- include/grl/kuka/KukaToFlatbuffer.hpp | 44 +++++++++++++++++++++------ 1 file changed, 34 insertions(+), 10 deletions(-) diff --git a/include/grl/kuka/KukaToFlatbuffer.hpp b/include/grl/kuka/KukaToFlatbuffer.hpp index 88c9f05..5128321 100644 --- a/include/grl/kuka/KukaToFlatbuffer.hpp +++ b/include/grl/kuka/KukaToFlatbuffer.hpp @@ -4,27 +4,50 @@ #include "grl/flatbuffer/JointState_generated.h" #include "grl/flatbuffer/ArmControlState_generated.h" #include "grl/flatbuffer/KUKAiiwa_generated.h" +#include "grl/flatbuffer/LinkObject_generated.h" namespace grl { namespace robot { namespace arm { flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder &fbb, - const double x = 0.0, - const double y = 0.0, - const double z = 0.0) + const double x, + const double y, + const double z) { return grl::flatbuffer::CreateEulerTranslationParams(fbb, x, y, z); } flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder &fbb, - const double r1 = 0.0, - const double r2 = 0.0, - const double r3 = 0.0, + const double r1, + const double r2, + const double r3, grl::flatbuffer::EulerOrder &eulerOrder) { return grl::flatbuffer::CreateEulerRotationParams(fbb, r1, r2, r3, eulerOrder); } +// Helper function is also defined in FusionTrackToFlatbuffer.hpp +grl::flatbuffer::Vector3d toFlatBuffer(const Eigen::Vector3d &pt) +{ + return grl::flatbuffer::Vector3d(pt.x(), pt.y(), pt.z()); +} + +// How to distinguish different eulerOrder? +grl::flatbuffer::EulerRotation toFlatBuffer( + const Eigen::Vector3d &pt, + grl::flatbuffer::EulerOrder eulerOrder ) +{ + return grl::flatbuffer::EulerRotation(pt.x(), pt.y(), pt.z(), eulerOrder); +} + +grl::flatbuffer::EulerPose toFlatBuffer( + const grl::flatbuffer::Vector3d &positon, + const grl::flatbuffer::EulerRotation &eulerRotation) +{ + return grl::flatbuffer::EulerPose(positon, eulerRotation); +} +// With ::ftk3DPoint, we can get the structure Vector3D, so do we need overwrite this function with input parameter ::ftk3DPoint? +// table EulerPose and EulerPoseParams are both defined in Euler.fbs. flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder &fbb, const grl::flatbuffer::Vector3d &position, @@ -69,8 +92,9 @@ flatbuffers::Offset toFlatBuff flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder &fbb, const ::OverlayType &overlayType, - const int32_t sendPeriodMillisec, - const int32_t setReceiveMultiplier, + const ::ConnectionInfo &connectionInfo, + // const int32_t sendPeriodMillisec, + // const int32_t setReceiveMultiplier, const bool updatePortOnRemote, const int16_t portOnRemote, const bool updatePortOnController, @@ -79,8 +103,8 @@ flatbuffers::Offset toFlatBuffer( return grl::flatbuffer::CreateFRI( fbb, EOverlayType, - sendPeriodMillisec, - setReceiveMultiplier, + connectionInfo.sendPeriod, + connectionInfo.receiveMultiplier, updatePortOnRemote, portOnRemote, updatePortOnController, From 4a267f77bc041d4b53e044b986a3a326915a811d Mon Sep 17 00:00:00 2001 From: Chunting Date: Mon, 13 Nov 2017 01:33:03 -0500 Subject: [PATCH 004/102] kukaToFlatbuffer.hpp added some toFlatBuffer functions. --- include/grl/kuka/KukaToFlatbuffer.hpp | 120 +++++++++++++++++++++++--- 1 file changed, 106 insertions(+), 14 deletions(-) diff --git a/include/grl/kuka/KukaToFlatbuffer.hpp b/include/grl/kuka/KukaToFlatbuffer.hpp index 5128321..a77c5ff 100644 --- a/include/grl/kuka/KukaToFlatbuffer.hpp +++ b/include/grl/kuka/KukaToFlatbuffer.hpp @@ -56,6 +56,18 @@ flatbuffers::Offset toFlatBuffer( return grl::flatbuffer::CreateEulerPoseParams(fbb, position, rotation); } +// grl::flatbuffer::Pose toFlatBuffer(Eigen::Affine3d tf) +// { +// Eigen::Vector3d pos = tf.translation(); +// Eigen::Quaterniond eigenQuat(tf.rotation()); +// return grl::flatbuffer::Pose(toFlatBuffer(pos), toFlatBuffer(eigenQuat)); +// } + +// grl::flatbuffer::Pose toFlatBuffer(Eigen::Affine3f tf) +// { +// return toFlatBuffer(tf.cast()); +// } + flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder &fbb, const grl::flatbuffer::EulerPose &stiffness, @@ -112,6 +124,7 @@ flatbuffers::Offset toFlatBuffer( ); } +/// KUKAiiwa.fbs flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder &fbb, const std::vector &jointAccelerationRel, @@ -130,6 +143,7 @@ flatbuffers::Offset toFlatBuffer( ); } +/// KUKAiiwa.fbs flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder &fbb, const std::string &dataType, @@ -143,8 +157,6 @@ flatbuffers::Offset toFlatBuffer( bool shouldRemove = false, bool shouldUpdate = false) { - auto jointAccelerationRelBuffer = fbb.CreateVector(jointAccelerationRel.data(),jointAccelerationRel.size()); - auto jointVelocityRelBuffer = fbb.CreateVector(jointVelocityRel.data(),jointVelocityRel.size()); return grl::flatbuffer::CreateProcessData( fbb, fbb.CreateString(datatype), @@ -161,12 +173,12 @@ flatbuffers::Offset toFlatBuffer( ); } - +/// LinkObject.fbs flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder fbb, const std::string &name, const std::string &parent, - const grl::flatbuffer::Pose &pose, + const grl::flatbuffer::Pose &pose, // Using the helper function toFlatBuffer to get this parameter? const grl::flatbuffer::Inertia &inertia) { return grl::flatbuffer::CreateLinkObject( @@ -180,16 +192,16 @@ flatbuffers::Offset toFlatBuffer( - +/// KUKAiiwa.fbs flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder &fbb, const std::string &name, - const grl::flatbuffer::KUKAiiwaInterface &commandInterface, + const grl::flatbuffer::KUKAiiwaInterface &commandInterface, // enum defined in KUKAiiwa.fbs const grl::flatbuffer::KUKAiiwaInterface &monitorInterface, // const grl::flatbuffer::ClientCommandMode clientCommandMode, - const ::ClientCommandMode &clientCommandMode, - const ::OverlayType &overlayType, - const ::ControlMode &controlMode, + const ::ClientCommandMode &clientCommandMode, // enum + const ::OverlayType &overlayType, // enum + const ::ControlMode &controlMode, // enum flatbuffers::Offset &setCartImpedance, flatbuffers::Offset &setJointImpedance, flatbuffers::Offset &smartServoConfig, @@ -199,8 +211,6 @@ flatbuffers::Offset toFlatBuffer( const std::string ¤tMotionCenter, const bool requestMonitorProcessData) { - auto jointAccelerationRelBuffer = fbb.CreateVector(jointAccelerationRel.data(),jointAccelerationRel.size()); - auto jointVelocityRelBuffer = fbb.CreateVector(jointVelocityRel.data(),jointVelocityRel.size()); return grl::flatbuffer::CreateKUKAiiwaArmConfiguration( fbb, fbb.CreateString(name), @@ -219,6 +229,7 @@ flatbuffers::Offset toFlatBuffer( requestMonitorProcessData); } +/// JointState.fbs flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder &fbb, const std::vector &position, @@ -233,6 +244,8 @@ flatbuffers::Offset toFlatBuffer( acceleration ? fbb.CreateVector(acceleration) : 0, torque ? fbb.CreateVector(torque) : 0); } + +/// KUKAiiwa.fbs flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder fbb, flatbuffers::Offset &measuredState, @@ -254,12 +267,91 @@ flatbuffers::Offset toFlatBuffer( CartesianWrench); } +/// KUKAiiwa.fbs +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const std::string &name, + const td::string &destination, + const td::string &source + const double timestamp, + const bool setArmControlState, + flatbuffers::Offset &armControlState, + const bool setArmConfiguration, + flatbuffers::Offset &armConfiguration, + const bool hasMonitorState , + flatbuffers::Offset &monitorState, + const bool hasMonitorConfig, + flatbuffers::Offset &monitorConfig) +{ + return grl::flatbuffer::CreateKUKAiiwaState( + fbb, + fbb.CreateString(name), + fbb.CreateString(destination), + fbb.CreateString(source), + timestamp, + setArmControlState, + armControlState, + setArmConfiguration, + armConfiguration, + hasMonitorState, + monitorState, + hasMonitorConfig, + monitorConfig); +} + +/// ArmControlState.fbs +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + std::vector> &traj + ) +{ + flatbuffers::Offset>> traj_vector = fbb.CreateVector(traj.data(), traj.size()); + return grl::flatbuffer::CreateMoveArmTrajectory( + fbb, + traj_vector); +} -// flatbuffers::Offset toFlatBuffer( ) -// { +/// ArmControlState.fbs +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + flatbuffers::Offset &goal + ) +{ + return grl::flatbuffer::CreateMoveArmJointServo( + fbb, + goal); +} + +/// ArmControlState.fbs +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const std::string *parent, + const grl::flatbuffer::Pose &goal) +{ + return grl::flatbuffer::CreateMoveArmJointServo( + fbb, + fbb.CreateString(parent), + goal); +} -// } +/// ArmControlState.fbs +// flatbuffers::Offset toFlatBuffer( +// flatbuffers::FlatBufferBuilder &fbb, +// const std::string &name, +// int64_t sequenceNumber, +// double timeStamp) +// { +// // The parameter of ArmState is undetermined. +// grl::flatbuffer::ArmState state_type = grl::flatbuffer::ArmStat::StartArm; +// return grl::flatbuffer::CreateArmControlState( +// fbb, +// fbb.CreateString(name), +// sequenceNumber, +// timeStamp, +// _state_type, +// _state_type.Union()); +// } } // End of arm namespace } // End of robot namespace } // End of grl namespace From f7ae1607a23f5918f196e6c5ca3933f2f5d7f983 Mon Sep 17 00:00:00 2001 From: Chunting Date: Tue, 14 Nov 2017 13:58:26 -0500 Subject: [PATCH 005/102] FusionTrackToFlatbuffer.hpp, converted the enum to flatbuffer enum objects --- include/grl/flatbuffer/FusionTrack.fbs | 8 +-- .../grl/sensor/FusionTrackToFlatbuffer.hpp | 57 +++++++++++++++++-- 2 files changed, 57 insertions(+), 8 deletions(-) diff --git a/include/grl/flatbuffer/FusionTrack.fbs b/include/grl/flatbuffer/FusionTrack.fbs index 9aa5bdd..0363e20 100644 --- a/include/grl/flatbuffer/FusionTrack.fbs +++ b/include/grl/flatbuffer/FusionTrack.fbs @@ -75,13 +75,13 @@ table FusionTrackFrame { height:uint; imageStrideInBytes:int; imageHeaderVersion:uint; - imageHeaderStatus:int; + imageHeaderStatus:ftkQueryStatus; imageLeftPixels:string; imageLeftPixelsVersion:uint; - imageLeftStatus:int; + imageLeftStatus:ftkQueryStatus; imageRightPixels:string; imageRightPixelsVersion:uint; - imageRightStatus:int; + imageRightStatus:ftkQueryStatus; regionsOfInterestLeft:[ftkRegionOfInterest]; regionsOfInterestLeftVersion:uint; regionsOfInterestLeftStatus:int; @@ -94,7 +94,7 @@ table FusionTrackFrame { markers:[ftkMarker]; markersVersion:uint; markersStatus:int; - deviceType:int; + deviceType:ftkDeviceType; ftkError:long; } diff --git a/include/grl/sensor/FusionTrackToFlatbuffer.hpp b/include/grl/sensor/FusionTrackToFlatbuffer.hpp index 896606c..1c68ee0 100644 --- a/include/grl/sensor/FusionTrackToFlatbuffer.hpp +++ b/include/grl/sensor/FusionTrackToFlatbuffer.hpp @@ -68,6 +68,48 @@ grl::flatbuffer::Pose toFlatBuffer(Eigen::Affine3f tf) return toFlatBuffer(tf.cast()); } + +/// the second parameter is merely a tag to uniquely identify this function so it compiles +/// its value is not utilized or modified. +grl::flatbuffer::ftkQueryStatus toFlatBuffer(const ::ftkQueryStatus queryStatus) { + + switch(queryStatus) { + + case QS_OK: + return grl::flatbuffer::ftkQueryStatus::QS_OK; + case QS_ERR_OVERFLOW: + return grl::flatbuffer::ftkQueryStatus::QS_ERR_OVERFLOW; + case QS_ERR_INVALID_RESERVED_SIZE: + return grl::flatbuffer::ftkQueryStatus::QS_ERR_INVALID_RESERVED_SIZE; + case QS_REPROCESS: + return grl::flatbuffer::ftkQueryStatus::QS_REPROCESS; + default: + break; + } + return grl::flatbuffer::ftkQueryStatus::QS_WAR_SKIPPED; +} + +/// the second parameter is merely a tag to identify this function so it compiles +/// its value is not utilized or modified. +grl::flatbuffer::ftkDeviceType toFlatBuffer(const ::ftkDeviceType deviceType, grl::flatbuffer::ftkDeviceType tag) { +// grl::flatbuffer::ftkDeviceType toFlatBuffer(const ::ftkDeviceType deviceType) { + switch(deviceType) { + case DEV_SIMULATOR: /*!< Internal use only */ + return grl::flatbuffer::ftkDeviceType::DEV_SIMULATOR; + + case DEV_INFINITRACK: /*!< Device is an infiniTrack */ + return grl::flatbuffer::ftkDeviceType::DEV_INFINITRACK; + + case DEV_FUSIONTRACK_500: /*!< Device is a fusionTrack 500 */ + return grl::flatbuffer::ftkDeviceType::DEV_FUSIONTRACK_500; + + case DEV_FUSIONTRACK_250: /*!< Device is a fusionTrack 250 */ + return grl::flatbuffer::ftkDeviceType::DEV_FUSIONTRACK_250; + default: /**< Unknown device type. */ + break; + }; + return grl::flatbuffer::ftkDeviceType::DEV_UNKNOWN_DEVICE; +} flatbuffers::Offset toFlatBuffer(flatbuffers::FlatBufferBuilder &fbb, const ::ftkGeometry &geometry, @@ -315,13 +357,17 @@ toFlatBuffer(flatbuffers::FlatBufferBuilder &fbb, uint32_t height = frame.imageHeader.height; int32_t imageStrideInBytes = frame.imageHeader.imageStrideInBytes; uint32_t imageHeaderVersion = frame.FrameQueryP->imageHeaderVersionSize.Version; - int32_t imageHeaderStatus = frame.FrameQueryP->imageHeaderStat; + + grl::flatbuffer::ftkQueryStatus imageHeaderStatus = toFlatBuffer(frame.FrameQueryP->imageHeaderStat); flatbuffers::Offset imageLeftPixels = frame.CameraImageLeftP ? fbb.CreateString(reinterpret_cast(frame.CameraImageLeftP->begin()), sizeof(frame.CameraImageLeftP)) : 0; uint32_t imageLeftPixelsVersion = frame.FrameQueryP->imageLeftVersionSize.Version; - int32_t imageLeftStatus = frame.FrameQueryP->imageLeftStat; + // int32_t imageLeftStatus = frame.FrameQueryP->imageLeftStat; + + grl::flatbuffer::ftkQueryStatus imageLeftStatus = toFlatBuffer(frame.FrameQueryP->imageLeftStat); flatbuffers::Offset imageRightPixels = frame.CameraImageRightP ? fbb.CreateString(reinterpret_cast(frame.CameraImageRightP->begin()), sizeof(frame.CameraImageRightP)) : 0; uint32_t imageRightPixelsVersion = frame.FrameQueryP->imageRightVersionSize.Version; - int32_t imageRightStatus = frame.FrameQueryP->imageRightStat; + // int32_t imageRightStatus = frame.FrameQueryP->imageRightStat; + grl::flatbuffer::ftkQueryStatus imageRightStatus = toFlatBuffer(frame.FrameQueryP->imageRightStat); flatbuffers::Offset>> regionsOfInterestLeft = (writeRegionOfInterest && frame.ImageRegionOfInterestBoxesLeft.size()) ? toFlatBuffer(fbb, frame.ImageRegionOfInterestBoxesLeft): 0; @@ -331,6 +377,7 @@ toFlatBuffer(flatbuffers::FlatBufferBuilder &fbb, (writeRegionOfInterest && frame.ImageRegionOfInterestBoxesRight.size()) ? toFlatBuffer(fbb, frame.ImageRegionOfInterestBoxesRight): 0; uint32_t regionsOfInterestRightVersion = frame.FrameQueryP->rawDataRightVersionSize.Version; int32_t regionsOfInterestRightStatus = frame.FrameQueryP->rawDataRightStat; + flatbuffers::Offset>> threeDFiducials = (writeFiducials && frame.Fiducials.size()) ? toFlatBuffer(fbb, frame.Fiducials, fiducialIndexToMarkerIDs) : 0; uint32_t threeDFiducialsVersion = frame.FrameQueryP->threeDFiducialsVersionSize.Version; @@ -339,7 +386,9 @@ toFlatBuffer(flatbuffers::FlatBufferBuilder &fbb, (writeMarkers && frame.Markers.size()) ? toFlatBuffer(fbb, frame.Markers, markerIndexToMarkerNames) : 0; uint32_t markersVersion = frame.FrameQueryP->markersVersionSize.Version; int32_t markersStatus = frame.FrameQueryP->markersStat; - int32_t deviceType = frame.DeviceType; + // int32_t deviceType = frame.DeviceType; + auto deviceType = toFlatBuffer(frame.DeviceType, grl::flatbuffer::ftkDeviceType::DEV_UNKNOWN_DEVICE); + // auto deviceType = grl::flatbuffer::ftkDeviceType::DEV_SIMULATOR; int64_t ftkError = frame.Error; return grl::flatbuffer::CreateFusionTrackFrame( From 9325862e4dcd796dd89661b925fb66a5555beaad Mon Sep 17 00:00:00 2001 From: Chunting Date: Tue, 14 Nov 2017 23:03:48 -0500 Subject: [PATCH 006/102] kukaToFlatbuffer.hpp is created to get the flatbuffer objects (toFlatBuffer functions). --- include/grl/kuka/KukaToFlatbuffer.hpp | 133 +++++++++++++++++++++++++- 1 file changed, 131 insertions(+), 2 deletions(-) diff --git a/include/grl/kuka/KukaToFlatbuffer.hpp b/include/grl/kuka/KukaToFlatbuffer.hpp index a77c5ff..1bda950 100644 --- a/include/grl/kuka/KukaToFlatbuffer.hpp +++ b/include/grl/kuka/KukaToFlatbuffer.hpp @@ -7,6 +7,135 @@ #include "grl/flatbuffer/LinkObject_generated.h" namespace grl { namespace robot { namespace arm { +/// faltbuffer enum objects +/// 1. Which element in the enum should be selected as the defaut return value? +/// 2. Is the argument like enum, int, double etc. passed by value or by reference? + +grl::flatbuffer::ESessionState toFlatBuffer(const ::FRISessionState sessionState) { + + switch(sessionState) { + case FRISessionState_MONITORING_WAIT: + return grl::flatbuffer::ESessionState::MONITORING_WAIT; + case FRISessionState_MONITORING_READY: + return grl::flatbuffer::ESessionState::MONITORING_READY; + case FRISessionState_COMMANDING_WAIT: + return grl::flatbuffer::ESessionState::COMMANDING_WAIT; + case FRISessionState_COMMANDING_ACTIVE: + return grl::flatbuffer::ESessionState::COMMANDING_ACTIVE; + default: + break; + } + return grl::flatbuffer::ESessionState::IDLE; +} + +grl::flatbuffer::EConnectionQuality toFlatBuffer(const ::FRIConnectionQuality connectionQuality) { + + switch(connectionQuality) { + case FRIConnectionQuality_POOR: + return grl::flatbuffer::EConnectionQuality::POOR; + case FRIConnectionQuality_FAIR: + return grl::flatbuffer::EConnectionQuality:FAIR; + case FRIConnectionQuality_GOOD: + return grl::flatbuffer::EConnectionQuality::GOOD; + + default: + break; + } + return grl::flatbuffer::EConnectionQuality::EXCELLENT; +} + +grl::flatbuffer::ESafetyState toFlatBuffer(const ::SafetyState safetyState) { + + switch(safetyState) { + case SafetyState_SAFETY_STOP_LEVEL_0: + return grl::flatbuffer::ESafetyState::SAFETY_STOP_LEVEL_1; + case SafetyState_SAFETY_STOP_LEVEL_1: + return grl::flatbuffer::ESafetyState:SAFETY_STOP_LEVEL_2; + case SafetyState_SAFETY_STOP_LEVEL_2: + return grl::flatbuffer::ESafetyState::SAFETY_STOP_LEVEL_3; + + default: + break; + } + return grl::flatbuffer::ESafetyState::SAFETY_STOP_LEVEL_0; +} + +grl::flatbuffer::EOperationMode toFlatBuffer(const ::OperationMode operationMode) { + + switch(operationMode) { + case OperationMode_TEST_MODE_1: + return grl::flatbuffer::EOperationMode::OperationMode_TEST_MODE_1; + case OperationMode_TEST_MODE_2: + return grl::flatbuffer::EOperationMode:OperationMode_TEST_MODE_2; + + default: + break; + } + return grl::flatbuffer::EOperationMode::AUTOMATIC_MODE; +} + +grl::flatbuffer::EDriveState toFlatBuffer(const ::DriveState driveState) { + + switch(driveState) { + case DriveState_OFF: + return grl::flatbuffer::EDriveState::OFF; + case DriveState_TRANSITIONING: + return grl::flatbuffer::EDriveState:TRANSITIONING; + + default: // DriveState_ACTIVE + break; + } + return grl::flatbuffer::EDriveState::ACTIVE; +} + +grl::flatbuffer::EControlMode toFlatBuffer(const ::ControlMode controlMode) { + + switch(controlMode) { + case ControlMode_POSITION_CONTROLMODE: + return grl::flatbuffer::EControlMode::POSITION_CONTROL_MODE; + case ControlMode_CARTESIAN_IMPEDANCE_CONTROLMODE: + return grl::flatbuffer::EControlMode:CART_IMP_CONTROL_MODE; + case ControlMode_JOINT_IMPEDANCE_CONTROLMODE: + return grl::flatbuffer::EControlMode:JOINT_IMP_CONTROL_MODE; + case ControlMode_CARTESIAN_IMPEDANCE_CONTROLMODE: + + default: // ControlMode_NO_CONTROLMODE + break; + } + return grl::flatbuffer::EControlMode::NO_CONTROL; +} + +grl::flatbuffer::EClientCommandMode toFlatBuffer(const ::ClientCommandMode clientcommandMode) { + + switch(clientcommandMode) { + case ClientCommandMode_POSITION: + return grl::flatbuffer::EClientCommandMode::POSITION; + case ClientCommandMode_WRENCH: + return grl::flatbuffer::EClientCommandMode::WRENCH; + case ClientCommandMode_TORQUE: + return grl::flatbuffer::TORQUE; + + default: // ClientCommandMode_NO_COMMAND_MODE + break; + } + return grl::flatbuffer::EClientCommandMode::NO_COMMAND_MODE; +} + +grl::flatbuffer::EOverlayType toFlatBuffer(const ::OverlayType overlayType) { + + switch(overlayType) { + + case OverlayType_JOINT: + return grl::flatbuffer::EOverlayType::JOINT; + case OverlayType_CARTESIAN: + return grl::flatbuffer::CARTESIAN; + + default: // OverlayType_NO_OVERLAY + break; + } + return grl::flatbuffer::EOverlayType::OverlayType_NO_OVERLAY; +} + flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder &fbb, const double x, @@ -167,7 +296,6 @@ flatbuffers::Offset toFlatBuffer( fbb.CreateString(max), fbb.CreateString(unit), fbb.CreateString(value), - fbb.CreateString(datatype), shouldRemove, shouldUpdate ); @@ -230,6 +358,7 @@ flatbuffers::Offset toFlatBuffer( } /// JointState.fbs +/// JointValues in FRIMessages.pb.h, the same thing? flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder &fbb, const std::vector &position, @@ -278,7 +407,7 @@ flatbuffers::Offset toFlatBuffer( flatbuffers::Offset &armControlState, const bool setArmConfiguration, flatbuffers::Offset &armConfiguration, - const bool hasMonitorState , + const bool hasMonitorState, // MessageMonitorData monitorData in FRIMessage.pb.h flatbuffers::Offset &monitorState, const bool hasMonitorConfig, flatbuffers::Offset &monitorConfig) From 623b4f3294dce45ab0a3543c487aa03362be404a Mon Sep 17 00:00:00 2001 From: Chunting Date: Wed, 15 Nov 2017 17:07:29 -0500 Subject: [PATCH 007/102] Intermediate codes which can't be compiled --- include/grl/flatbuffer/KUKAiiwa.fbs | 28 ++++++- include/grl/kuka/Kuka.hpp | 2 +- include/grl/kuka/KukaJAVAdriver.hpp | 4 +- include/grl/kuka/KukaToFlatbuffer.hpp | 115 +++++++++++++++++++------- test/CMakeLists.txt | 4 + test/kukaiiwaTest.cpp | 29 +++++++ 6 files changed, 148 insertions(+), 34 deletions(-) create mode 100644 test/kukaiiwaTest.cpp diff --git a/include/grl/flatbuffer/KUKAiiwa.fbs b/include/grl/flatbuffer/KUKAiiwa.fbs index 2b4fd5d..37298e5 100644 --- a/include/grl/flatbuffer/KUKAiiwa.fbs +++ b/include/grl/flatbuffer/KUKAiiwa.fbs @@ -241,12 +241,30 @@ table KUKAiiwaMonitorConfiguration { // Get state data for the arm (things that change often) table KUKAiiwaMonitorState { + // this is the measured current state of the arm joints + // as seen in monitorState and _FRIMonitoringMessage + // Several values from KukaState should be copied into here + // - KukaState::position + // - KukaState::torque measuredState:JointState; - cartesianFlangePose:Pose; // cartesian pose of the flange relative to the base of the arm - jointStateReal:JointState; // this is the real current state of the arm joints - /// FRI can adjust the java commanded position. "Interpolated" is the original Java commanded position. + // cartesian pose of the flange relative to the base of the arm + cartesianFlangePose:Pose; + + // This is the real estimated state of the arm joints + // after combining data from the arm with sensors + // TODO: Not supported for now + jointStateReal:JointState; + // TODO: verify this description is correct + // FRI can adjust the java commanded position. + // "Interpolated" is the original Java commanded position. + // "ipo" is an interpolated position. Kuka does this strange + // thing where you send a command in Java then use FRI to + // modify or "interpolate", drawing some other path different + // from what the users originally specified. It seems strange, + // confusing, useless. + // KukaState::ipoJointPosition goes in here jointStateInterpolated:JointState; /// The state of the arm as calculated by kuka after @@ -254,10 +272,14 @@ table KUKAiiwaMonitorState { /// and any attachments configured to be present. /// /// Most likely only contains torque. + /// KukaState::ExternalTorque goes here externalState:JointState; + /// KUKA::FRI::EOperationMode operationMode:EOperationMode; + sessionState:ESessionState; + // This is a received cartesian wrench state CartesianWrench:Wrench; } diff --git a/include/grl/kuka/Kuka.hpp b/include/grl/kuka/Kuka.hpp index ff837c4..0f445f0 100644 --- a/include/grl/kuka/Kuka.hpp +++ b/include/grl/kuka/Kuka.hpp @@ -52,7 +52,7 @@ struct KukaState { cartesian_state externalForce; joint_state commandedPosition; cartesian_state commandedCartesianWrenchFeedForward; - cartesian_state wrenchJava; + cartesian_state wrenchJava; // Seven elements joint_state commandedTorque; joint_state ipoJointPosition; diff --git a/include/grl/kuka/KukaJAVAdriver.hpp b/include/grl/kuka/KukaJAVAdriver.hpp index 38d8349..44ce4bb 100644 --- a/include/grl/kuka/KukaJAVAdriver.hpp +++ b/include/grl/kuka/KukaJAVAdriver.hpp @@ -637,7 +637,7 @@ namespace grl { namespace robot { namespace arm { fd_set mask, temp_mask, dummy_mask; Params params_; - KukaState armState_; + KukaState armState_; // structure defined in Kuka.hpp // armControlMode is the current GRL_Driver.java configuration to which the arm is currently set. // Options are: // ArmState_NONE = 0, @@ -651,7 +651,7 @@ namespace grl { namespace robot { namespace arm { // ArmState_MoveArmCartesianServo = 8 flatbuffer::ArmState armControlMode_; flatbuffer::KUKAiiwaInterface commandInterface_ = flatbuffer::KUKAiiwaInterface::SmartServo;// KUKAiiwaInterface::SmartServo; - flatbuffer::KUKAiiwaInterface monitorInterface_ = flatbuffer::KUKAiiwaInterface::FRI; + flatbuffer::KUKAiiwaInterface monitorInterface_ = flatbuffer::KUKAiiwaInterface::FRI; // flatbuffers::FlatBufferBuilder builder_; // // flatbuffer::JointStateBuilder jointStateServoBuilder_; diff --git a/include/grl/kuka/KukaToFlatbuffer.hpp b/include/grl/kuka/KukaToFlatbuffer.hpp index 1bda950..396f800 100644 --- a/include/grl/kuka/KukaToFlatbuffer.hpp +++ b/include/grl/kuka/KukaToFlatbuffer.hpp @@ -5,6 +5,11 @@ #include "grl/flatbuffer/ArmControlState_generated.h" #include "grl/flatbuffer/KUKAiiwa_generated.h" #include "grl/flatbuffer/LinkObject_generated.h" +#include "grl/flatbuffer/Euler_generated.h" +#include + +#include +#include namespace grl { namespace robot { namespace arm { /// faltbuffer enum objects @@ -34,7 +39,7 @@ grl::flatbuffer::EConnectionQuality toFlatBuffer(const ::FRIConnectionQuality co case FRIConnectionQuality_POOR: return grl::flatbuffer::EConnectionQuality::POOR; case FRIConnectionQuality_FAIR: - return grl::flatbuffer::EConnectionQuality:FAIR; + return grl::flatbuffer::EConnectionQuality::FAIR; case FRIConnectionQuality_GOOD: return grl::flatbuffer::EConnectionQuality::GOOD; @@ -48,25 +53,25 @@ grl::flatbuffer::ESafetyState toFlatBuffer(const ::SafetyState safetyState) { switch(safetyState) { case SafetyState_SAFETY_STOP_LEVEL_0: - return grl::flatbuffer::ESafetyState::SAFETY_STOP_LEVEL_1; + return grl::flatbuffer::ESafetyState::SAFETY_STOP_LEVEL_0; case SafetyState_SAFETY_STOP_LEVEL_1: - return grl::flatbuffer::ESafetyState:SAFETY_STOP_LEVEL_2; + return grl::flatbuffer::ESafetyState::SAFETY_STOP_LEVEL_1; case SafetyState_SAFETY_STOP_LEVEL_2: - return grl::flatbuffer::ESafetyState::SAFETY_STOP_LEVEL_3; + return grl::flatbuffer::ESafetyState::SAFETY_STOP_LEVEL_2; default: break; } - return grl::flatbuffer::ESafetyState::SAFETY_STOP_LEVEL_0; + return grl::flatbuffer::ESafetyState::NORMAL_OPERATION; } grl::flatbuffer::EOperationMode toFlatBuffer(const ::OperationMode operationMode) { switch(operationMode) { case OperationMode_TEST_MODE_1: - return grl::flatbuffer::EOperationMode::OperationMode_TEST_MODE_1; + return grl::flatbuffer::EOperationMode::TEST_MODE_1; case OperationMode_TEST_MODE_2: - return grl::flatbuffer::EOperationMode:OperationMode_TEST_MODE_2; + return grl::flatbuffer::EOperationMode::TEST_MODE_2; default: break; @@ -80,7 +85,7 @@ grl::flatbuffer::EDriveState toFlatBuffer(const ::DriveState driveState) { case DriveState_OFF: return grl::flatbuffer::EDriveState::OFF; case DriveState_TRANSITIONING: - return grl::flatbuffer::EDriveState:TRANSITIONING; + return grl::flatbuffer::EDriveState::TRANSITIONING; default: // DriveState_ACTIVE break; @@ -94,11 +99,9 @@ grl::flatbuffer::EControlMode toFlatBuffer(const ::ControlMode controlMode) { case ControlMode_POSITION_CONTROLMODE: return grl::flatbuffer::EControlMode::POSITION_CONTROL_MODE; case ControlMode_CARTESIAN_IMPEDANCE_CONTROLMODE: - return grl::flatbuffer::EControlMode:CART_IMP_CONTROL_MODE; + return grl::flatbuffer::EControlMode::CART_IMP_CONTROL_MODE; case ControlMode_JOINT_IMPEDANCE_CONTROLMODE: - return grl::flatbuffer::EControlMode:JOINT_IMP_CONTROL_MODE; - case ControlMode_CARTESIAN_IMPEDANCE_CONTROLMODE: - + return grl::flatbuffer::EControlMode::JOINT_IMP_CONTROL_MODE; default: // ControlMode_NO_CONTROLMODE break; } @@ -113,7 +116,7 @@ grl::flatbuffer::EClientCommandMode toFlatBuffer(const ::ClientCommandMode clie case ClientCommandMode_WRENCH: return grl::flatbuffer::EClientCommandMode::WRENCH; case ClientCommandMode_TORQUE: - return grl::flatbuffer::TORQUE; + return grl::flatbuffer::EClientCommandMode::TORQUE; default: // ClientCommandMode_NO_COMMAND_MODE break; @@ -128,12 +131,12 @@ grl::flatbuffer::EOverlayType toFlatBuffer(const ::OverlayType overlayType) { case OverlayType_JOINT: return grl::flatbuffer::EOverlayType::JOINT; case OverlayType_CARTESIAN: - return grl::flatbuffer::CARTESIAN; + return grl::flatbuffer::EOverlayType::CARTESIAN; default: // OverlayType_NO_OVERLAY break; } - return grl::flatbuffer::EOverlayType::OverlayType_NO_OVERLAY; + return grl::flatbuffer::EOverlayType::NO_OVERLAY; } flatbuffers::Offset toFlatBuffer( @@ -144,7 +147,8 @@ flatbuffers::Offset toFlatBuffer( { return grl::flatbuffer::CreateEulerTranslationParams(fbb, x, y, z); } - +/// Euler.fbs struct EulerRotationParams +/// enum EulerOrder also defined in Euler.fbs flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder &fbb, const double r1, @@ -154,35 +158,49 @@ flatbuffers::Offset toFlatBuffer( { return grl::flatbuffer::CreateEulerRotationParams(fbb, r1, r2, r3, eulerOrder); } +/////////////////////////////////////////////////////////////////// // Helper function is also defined in FusionTrackToFlatbuffer.hpp grl::flatbuffer::Vector3d toFlatBuffer(const Eigen::Vector3d &pt) { return grl::flatbuffer::Vector3d(pt.x(), pt.y(), pt.z()); } -// How to distinguish different eulerOrder? +////////////////////////////////////////////////////////////////// +/// Euler.fbs, struct EulerRotation grl::flatbuffer::EulerRotation toFlatBuffer( const Eigen::Vector3d &pt, grl::flatbuffer::EulerOrder eulerOrder ) { return grl::flatbuffer::EulerRotation(pt.x(), pt.y(), pt.z(), eulerOrder); } - +/// Euler.fbs, struct EulerPose grl::flatbuffer::EulerPose toFlatBuffer( const grl::flatbuffer::Vector3d &positon, const grl::flatbuffer::EulerRotation &eulerRotation) { return grl::flatbuffer::EulerPose(positon, eulerRotation); } +/// Overload the above function +/// TODO (@Chunting) Check if ptr has the same physical meaning with pt, if so, discard it. +grl::flatbuffer::EulerPose toFlatBuffer( + const Eigen::Vector3d &pt, + const Eigen::Vector3d &ptr, + grl::flatbuffer::EulerOrder eulerOrder) +{ + auto positon = toFlatBuffer(pt); + auto eulerRotation = toFlatBuffer(ptr,eulerOrder); + return grl::flatbuffer::EulerPose(positon, eulerRotation); +} -// With ::ftk3DPoint, we can get the structure Vector3D, so do we need overwrite this function with input parameter ::ftk3DPoint? -// table EulerPose and EulerPoseParams are both defined in Euler.fbs. +/// tables EulerPose and EulerPoseParams are both defined in Euler.fbs. +/// The same thing with EulerPose, this function can be overloaded with Eigen arguments. +/// Note that the parameters should be passed by pointer, if by reference, it can't be compiled. flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder &fbb, const grl::flatbuffer::Vector3d &position, const grl::flatbuffer::EulerRotation &rotation) { - return grl::flatbuffer::CreateEulerPoseParams(fbb, position, rotation); + return grl::flatbuffer::CreateEulerPoseParams(fbb, &position, &rotation); } // grl::flatbuffer::Pose toFlatBuffer(Eigen::Affine3d tf) @@ -199,8 +217,8 @@ flatbuffers::Offset toFlatBuffer( flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder &fbb, - const grl::flatbuffer::EulerPose &stiffness, - const grl::flatbuffer::EulerPose &damping, + const grl::flatbuffer::EulerPose& stiffness, + const grl::flatbuffer::EulerPose& damping, const double nullspaceStiffness, const double nullspaceDamping, const grl::flatbuffer::EulerPose &maxPathDeviation, @@ -210,13 +228,13 @@ flatbuffers::Offset toFlatBuffer { return grl::flatbuffer::CreateCartesianImpedenceControlMode( fbb, - stiffness, - damping, + std::addressof(stiffness), + std::addressof(damping), nullspaceStiffness, nullspaceDamping, - maxPathDeviation, - maxCartesianVelocity, - maxControlForce, + std::addressof(maxPathDeviation), + std::addressof(maxCartesianVelocity), + std::addressof(maxControlForce), maxControlForceExceededStop); } @@ -374,6 +392,26 @@ flatbuffers::Offset toFlatBuffer( torque ? fbb.CreateVector(torque) : 0); } +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const std::vector& kukaStates, + const std::vector &velocity = 0, + const std::vector &acceleration = 0) +{ + std::vector position; + std::vector torque; + for(auto kukaState : kukaStates){ + position.push_back(kukaState.position); + torque.push_back(kukaState.torque); + } + return grl::flatbuffer::CreateJointState( + fbb, + position ? fbb.CreateVector(position) : 0, + velocity ? fbb.CreateVector(velocity) : 0, + acceleration ? fbb.CreateVector(acceleration) : 0, + torque ? fbb.CreateVector(torque) : 0); +} + /// KUKAiiwa.fbs flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder fbb, @@ -396,6 +434,27 @@ flatbuffers::Offset toFlatBuffer( CartesianWrench); } +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder fbb, + flatbuffers::Offset &measuredState, + const grl::flatbuffer::Pose &cartesianFlangePose, + flatbuffers::Offset &jointStateReal, + flatbuffers::Offset &jointStateInterpolated, + flatbuffers::Offset &externalState, + const ::OperationMode &operationMode, + const grl::flatbuffer::Wrench &CartesianWrench) +{ + return grl::flatbuffer::CreateJointState( + fbb, + measuredState, + cartesianFlangePose, + jointStateReal, + jointStateInterpolated, + externalState, + operationMode, + CartesianWrench); +} + /// KUKAiiwa.fbs flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder &fbb, diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 879a800..d6e9655 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -90,6 +90,10 @@ if(TARGET KukaFRIClient OR FRI_Client_SDK_Cpp_FOUND) basis_add_test(KukaLBRiiwaVrepPluginTest.cpp) basis_target_link_libraries(KukaLBRiiwaVrepPluginTest ${Boost_LIBRARIES} ${Boost_REGEX_LIBRARY} ${Boost_UNIT_TEST_FRAMEWORK_LIBRARY} ${FRI_Client_SDK_Cpp_LIBRARIES} ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} v_repLib KukaFRIClient ${LINUX_ONLY_LIBS} ) + + basis_add_executable(kukaiiwaTest.cpp)# ${GRL_FLATBUFFERS_OUTPUTS}) + basis_target_link_libraries(kukaiiwaTest ${Boost_LIBRARIES} ${Boost_REGEX_LIBRARY} ${FRI_Client_SDK_Cpp_LIBRARIES} ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} KukaFRIClient) + endif() diff --git a/test/kukaiiwaTest.cpp b/test/kukaiiwaTest.cpp new file mode 100644 index 0000000..867e89c --- /dev/null +++ b/test/kukaiiwaTest.cpp @@ -0,0 +1,29 @@ +#define BOOST_TEST_DYN_LINK +#define BOOST_TEST_MODULE grlTest + +// system includes +#include +#include +#include +#include + +//// local includes +//////#include "flatbuffers/util.h" +///#include "grl/sensor/FusionTrack.hpp" +///#include "grl/sensor/FusionTrackToEigen.hpp" +#include "grl/kuka/KukaToFlatbuffer.hpp" +#include +#include "grl/kuka/KukaJAVAdriver.hpp" + +BOOST_AUTO_TEST_SUITE(KukaTest) + +BOOST_AUTO_TEST_CASE(runRepeatedly) +{ + bool debug = true; + if (debug) std::cout << "starting KukaTest" << std::endl; + // grl::robot::arm::KukaJAVAdriver::Parms javaDriverP = grl::robot::arm::KukaJAVAdriver::defaultParams(); + // rl::robot::arm::KukaJAVAdriver javaDriver(javaDriverP); + // javaDriver.construct(); +} + +BOOST_AUTO_TEST_SUITE_END() From d9ce32d060eb6238209b6afa6868f4b4aeb2d0b2 Mon Sep 17 00:00:00 2001 From: Chunting Date: Sat, 18 Nov 2017 23:14:28 -0500 Subject: [PATCH 008/102] toFlatBuffer functions can compile --- include/grl/flatbuffer/KUKAiiwa.fbs | 21 ++++ include/grl/kuka/Kuka.hpp | 4 + include/grl/kuka/KukaToFlatbuffer.hpp | 133 ++++++++++++++------------ test/kukaiiwaTest.cpp | 7 +- 4 files changed, 98 insertions(+), 67 deletions(-) diff --git a/include/grl/flatbuffer/KUKAiiwa.fbs b/include/grl/flatbuffer/KUKAiiwa.fbs index 37298e5..2550b5c 100644 --- a/include/grl/flatbuffer/KUKAiiwa.fbs +++ b/include/grl/flatbuffer/KUKAiiwa.fbs @@ -284,6 +284,25 @@ table KUKAiiwaMonitorState { } +// a table that is used as a record of all communication over FRI +// it is not for user configuration or anything else, it should +// simply reflect a history of what was sent and what was received. +table FRIMessageLog { + + messageIdentifier:int; + sequenceCounter:int; + reflectedSequenceCounter:int; + // hardware timestamps + sec:int; + nanosec:int; // one billionth of a second + measuredJointPosition:[double]; + measuredTorque:[double]; + commandedJointPosition:[double]; + commandedTorque:[double]; + externalTorque:[double]; + +} + table KUKAiiwaState { name:string; // The name of the robot to update (identifier used when applicable, doesn't ever change or set the name) @@ -303,8 +322,10 @@ table KUKAiiwaState { hasMonitorConfig:bool = false; monitorConfig:KUKAiiwaMonitorConfiguration; + FRIMessage:FRIMessageLog; } + // Full message that is sent back and forth // between KUKA iiwa and driver // This is used because they can also be diff --git a/include/grl/kuka/Kuka.hpp b/include/grl/kuka/Kuka.hpp index 0f445f0..9673a99 100644 --- a/include/grl/kuka/Kuka.hpp +++ b/include/grl/kuka/Kuka.hpp @@ -9,12 +9,16 @@ #include #include #include +#include #include "grl/flatbuffer/KUKAiiwa_generated.h" #include "grl/tags.hpp" #include "grl/exception.hpp" +#include +#include + namespace KUKA { namespace LBRState { /// @todo replace all instances of this with the getter now provided diff --git a/include/grl/kuka/KukaToFlatbuffer.hpp b/include/grl/kuka/KukaToFlatbuffer.hpp index 396f800..650fd79 100644 --- a/include/grl/kuka/KukaToFlatbuffer.hpp +++ b/include/grl/kuka/KukaToFlatbuffer.hpp @@ -1,11 +1,14 @@ #ifndef GRL_KUKA_TO_FLATBUFFER #define GRL_KUKA_TO_FLATBUFFER +#include + #include "grl/flatbuffer/JointState_generated.h" #include "grl/flatbuffer/ArmControlState_generated.h" #include "grl/flatbuffer/KUKAiiwa_generated.h" #include "grl/flatbuffer/LinkObject_generated.h" #include "grl/flatbuffer/Euler_generated.h" +#include "grl/kuka/Kuka.hpp" #include #include @@ -238,7 +241,7 @@ flatbuffers::Offset toFlatBuffer maxControlForceExceededStop); } -flatbuffers::Offset toFlatBuffer( +flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder &fbb, std::vector &joint_stiffness, std::vector &joint_damping) @@ -259,9 +262,10 @@ flatbuffers::Offset toFlatBuffer( const bool updatePortOnController, const int16_t portOnController) { + // auto _overlayType = toFlatBuffer(overlayType); return grl::flatbuffer::CreateFRI( fbb, - EOverlayType, + toFlatBuffer(overlayType), connectionInfo.sendPeriod, connectionInfo.receiveMultiplier, updatePortOnRemote, @@ -306,7 +310,7 @@ flatbuffers::Offset toFlatBuffer( { return grl::flatbuffer::CreateProcessData( fbb, - fbb.CreateString(datatype), + fbb.CreateString(dataType), fbb.CreateString(defaultValue), fbb.CreateString(displayName), fbb.CreateString(id), @@ -331,8 +335,8 @@ flatbuffers::Offset toFlatBuffer( fbb, fbb.CreateString(name), fbb.CreateString(parent), - pose, - inertia + std::addressof(pose), + std::addressof(inertia) ); } @@ -362,9 +366,9 @@ flatbuffers::Offset toFlatBuffer( fbb.CreateString(name), commandInterface, monitorInterface, - clientCommandMode, - overlayType, - controlMode, + toFlatBuffer(clientCommandMode), + toFlatBuffer(overlayType), + toFlatBuffer(controlMode), setCartImpedance, setJointImpedance, smartServoConfig, @@ -386,30 +390,32 @@ flatbuffers::Offset toFlatBuffer( { return grl::flatbuffer::CreateJointState( fbb, - position ? fbb.CreateVector(position) : 0, - velocity ? fbb.CreateVector(velocity) : 0, - acceleration ? fbb.CreateVector(acceleration) : 0, - torque ? fbb.CreateVector(torque) : 0); + position.empty() ? fbb.CreateVector(position) : 0, + velocity.empty() ? fbb.CreateVector(velocity) : 0, + acceleration.empty() ? fbb.CreateVector(acceleration) : 0, + torque.empty() ? fbb.CreateVector(torque) : 0); } flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder &fbb, const std::vector& kukaStates, - const std::vector &velocity = 0, - const std::vector &acceleration = 0) + const std::vector &velocity, + const std::vector &acceleration) { std::vector position; std::vector torque; - for(auto kukaState : kukaStates){ - position.push_back(kukaState.position); - torque.push_back(kukaState.torque); + std::size_t sizeofStates = kukaStates.size(); + for(auto &kukaState : kukaStates){ + + boost::copy(kukaState.position, &position[0]); + boost::copy(kukaState.torque, &torque[0]); } return grl::flatbuffer::CreateJointState( fbb, - position ? fbb.CreateVector(position) : 0, - velocity ? fbb.CreateVector(velocity) : 0, - acceleration ? fbb.CreateVector(acceleration) : 0, - torque ? fbb.CreateVector(torque) : 0); + position.empty() ? fbb.CreateVector(position) : 0, + velocity.empty() ? fbb.CreateVector(velocity) : 0, + acceleration.empty() ? fbb.CreateVector(acceleration) : 0, + torque.empty() ? fbb.CreateVector(torque) : 0); } /// KUKAiiwa.fbs @@ -420,56 +426,60 @@ flatbuffers::Offset toFlatBuffer( flatbuffers::Offset &jointStateReal, flatbuffers::Offset &jointStateInterpolated, flatbuffers::Offset &externalState, + const ::FRISessionState &sessionState, const ::OperationMode &operationMode, const grl::flatbuffer::Wrench &CartesianWrench) { - return grl::flatbuffer::CreateJointState( + return grl::flatbuffer::CreateKUKAiiwaMonitorState( fbb, measuredState, - cartesianFlangePose, + std::addressof(cartesianFlangePose), jointStateReal, jointStateInterpolated, externalState, - operationMode, - CartesianWrench); -} -flatbuffers::Offset toFlatBuffer( - flatbuffers::FlatBufferBuilder fbb, - flatbuffers::Offset &measuredState, - const grl::flatbuffer::Pose &cartesianFlangePose, - flatbuffers::Offset &jointStateReal, - flatbuffers::Offset &jointStateInterpolated, - flatbuffers::Offset &externalState, - const ::OperationMode &operationMode, - const grl::flatbuffer::Wrench &CartesianWrench) -{ - return grl::flatbuffer::CreateJointState( - fbb, - measuredState, - cartesianFlangePose, - jointStateReal, - jointStateInterpolated, - externalState, - operationMode, - CartesianWrench); + toFlatBuffer(operationMode), + toFlatBuffer(sessionState), + std::addressof(CartesianWrench) + ); } +// flatbuffers::Offset toFlatBuffer( +// flatbuffers::FlatBufferBuilder fbb, +// flatbuffers::Offset &measuredState, +// const grl::flatbuffer::Pose &cartesianFlangePose, +// flatbuffers::Offset &jointStateReal, +// flatbuffers::Offset &jointStateInterpolated, +// flatbuffers::Offset &externalState, +// const ::OperationMode &operationMode, +// const grl::flatbuffer::Wrench &CartesianWrench) +// { +// return grl::flatbuffer::CreateJointState( +// fbb, +// measuredState, +// cartesianFlangePose, +// jointStateReal, +// jointStateInterpolated, +// externalState, +// toFlatBuffer(operationMode), +// CartesianWrench); +// } + /// KUKAiiwa.fbs flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder &fbb, const std::string &name, - const td::string &destination, - const td::string &source + const std::string &destination, + const std::string &source, const double timestamp, const bool setArmControlState, - flatbuffers::Offset &armControlState, + flatbuffers::Offset &armControlState, const bool setArmConfiguration, - flatbuffers::Offset &armConfiguration, + flatbuffers::Offset &armConfiguration, const bool hasMonitorState, // MessageMonitorData monitorData in FRIMessage.pb.h - flatbuffers::Offset &monitorState, + flatbuffers::Offset &monitorState, const bool hasMonitorConfig, - flatbuffers::Offset &monitorConfig) + flatbuffers::Offset &monitorConfig) { return grl::flatbuffer::CreateKUKAiiwaState( fbb, @@ -511,16 +521,17 @@ flatbuffers::Offset toFlatBuffer( } /// ArmControlState.fbs -flatbuffers::Offset toFlatBuffer( - flatbuffers::FlatBufferBuilder &fbb, - const std::string *parent, - const grl::flatbuffer::Pose &goal) -{ - return grl::flatbuffer::CreateMoveArmJointServo( - fbb, - fbb.CreateString(parent), - goal); -} +// flatbuffers::Offset toFlatBuffer( +// flatbuffers::FlatBufferBuilder &fbb, +// const std::string &parent, +// const grl::flatbuffer::Pose &goal) +// { +// return grl::flatbuffer::CreateMoveArmJointServo( +// fbb, +// fbb.CreateString(parent), +// std::addressof(goal) +// ); +// } /// ArmControlState.fbs diff --git a/test/kukaiiwaTest.cpp b/test/kukaiiwaTest.cpp index 867e89c..228b587 100644 --- a/test/kukaiiwaTest.cpp +++ b/test/kukaiiwaTest.cpp @@ -7,13 +7,8 @@ #include #include -//// local includes -//////#include "flatbuffers/util.h" -///#include "grl/sensor/FusionTrack.hpp" -///#include "grl/sensor/FusionTrackToEigen.hpp" #include "grl/kuka/KukaToFlatbuffer.hpp" -#include -#include "grl/kuka/KukaJAVAdriver.hpp" + BOOST_AUTO_TEST_SUITE(KukaTest) From b8e6f499d2cda9b29a35678431f4e32057259b1b Mon Sep 17 00:00:00 2001 From: Andrew Hundt Date: Tue, 21 Nov 2017 18:04:19 -0500 Subject: [PATCH 009/102] INTERMEDIATE CODE: DOES NOT COMPILE initial integration of TimeEvent into low level kuka driver --- include/grl/flatbuffer/KUKAiiwa.fbs | 10 +- include/grl/kuka/Kuka.hpp | 25 +- include/grl/kuka/KukaDriver.hpp | 7 +- include/grl/kuka/KukaFRIdriver.hpp | 48 +-- include/grl/kuka/KukaJAVAdriver.hpp | 89 +++--- include/grl/kuka/KukaToFlatbuffer.hpp | 70 ++++- include/thirdparty/fbs_tk/fbs_tk.hpp | 425 ++++++++++++++++++++++++++ 7 files changed, 617 insertions(+), 57 deletions(-) create mode 100644 include/thirdparty/fbs_tk/fbs_tk.hpp diff --git a/include/grl/flatbuffer/KUKAiiwa.fbs b/include/grl/flatbuffer/KUKAiiwa.fbs index 2550b5c..0494130 100644 --- a/include/grl/flatbuffer/KUKAiiwa.fbs +++ b/include/grl/flatbuffer/KUKAiiwa.fbs @@ -289,19 +289,27 @@ table KUKAiiwaMonitorState { // simply reflect a history of what was sent and what was received. table FRIMessageLog { + sessionState:ESessionState; + connectionQuality:EConnectionQuality; + controlMode:EControlMode; + // MessageHeader; messageIdentifier:int; sequenceCounter:int; reflectedSequenceCounter:int; // hardware timestamps sec:int; nanosec:int; // one billionth of a second + // MessageMonitorData measuredJointPosition:[double]; measuredTorque:[double]; commandedJointPosition:[double]; commandedTorque:[double]; externalTorque:[double]; + // MessageIpoData + jointStateInterpolated:[double]; -} + timeStamp:TimeEvent; + } table KUKAiiwaState { diff --git a/include/grl/kuka/Kuka.hpp b/include/grl/kuka/Kuka.hpp index 9673a99..4aa6dc8 100644 --- a/include/grl/kuka/Kuka.hpp +++ b/include/grl/kuka/Kuka.hpp @@ -43,20 +43,33 @@ namespace arm { /// do not depend on this struct directly. /// @todo commandedPosition and commandedPosition_goal are used a bit /// ambiguously, figure out the difference and clean it up. +/// @TODO(ahundt) add support for a vector of Transformation data @see FRIMessages.pb.h, +/// but store as quaternon + vector, not rotation matrix struct KukaState { typedef boost::container::static_vector joint_state; + // cartesian state entries consist of a vector x,y,z and a quaternion [x, y, z, w] typedef boost::container::static_vector cartesian_state; typedef std::chrono::time_point time_point_type; + /// measured position, identified by revolute_joint_angle_open_chain_state_tag + /// @see grl::revolute_joint_angle_open_chain_state_tag in grl/tags.hpp, which identifies this data joint_state position; + /// measured torque, identified by revolute_joint_torque_open_chain_state_tag + /// @see grl::revolute_joint_torque_open_chain_state_tag in grl/tags.hpp, which identifies this data joint_state torque; + /// torque applied to the robot from an outside source, + /// @see grl::revolute_joint_torque_open_chain_state_tag in grl/tags.hpp, which identifies this data joint_state externalTorque; + /// measured external force, only available in sunrise OS version 1.9 cartesian_state externalForce; + + /// commanded joint angles + /// @see grl::revolute_joint_angle_open_chain_command_tag in grl/tags.hpp, which identifies this data joint_state commandedPosition; + /// cartesian_wrench_command_tag cartesian_state commandedCartesianWrenchFeedForward; - cartesian_state wrenchJava; // Seven elements joint_state commandedTorque; joint_state ipoJointPosition; @@ -77,6 +90,8 @@ struct KukaState { // The point in time associated with the current measured // state of the arm (position, torque, etc.). When commanding // the arm use commanded_goal_timestamp. + /// TODO(Chunting) remove this and make all code that uses it instead set the time_event_stamp + /// most likely the device_time, but double check the correctness of that time_point_type timestamp; ///////////////////////////////////////////////////////////////////////////////////////////// @@ -102,6 +117,12 @@ struct KukaState { /// velocity limits the arm stops immediately with an error. joint_state velocity_limits; + /// Time event records the hardware time, + /// the local computer time before receiving data + /// and the local computer time after receiving data + /// This makes it possible to more accurately synchronize + /// time between multiple hardware devices. + grl::TimeEvent time_event_stamp; void clear() { position.clear(); @@ -125,7 +146,7 @@ struct KukaState { }; constexpr auto KUKA_LBR_IIWA_14_R820 = "KUKA_LBR_IIWA_14_R820"; -constexpr auto KUKA_LBR_IIWA_7_R800 = "KUKA_LBR_IIWA_7_R800"; +constexpr auto KUKA_LBR_IIWA_7_R8, identified by00 = "KUKA_LBR_IIWA_7_R800"; /// @brief copy vector of joint velocity limits in radians/s /// diff --git a/include/grl/kuka/KukaDriver.hpp b/include/grl/kuka/KukaDriver.hpp index 06b209c..e2a3ef8 100644 --- a/include/grl/kuka/KukaDriver.hpp +++ b/include/grl/kuka/KukaDriver.hpp @@ -447,12 +447,15 @@ namespace grl { namespace robot { namespace arm { } /// @todo(ahundt) replace with standard get() call with tag dispatch like above + /// get 6 element wrench entries + /// [force_x, force_y, force_z, torque_x, torque_y, torque_z] + /// @param output a C++ OutputIterator which adds the values to your output object, + /// see http://en.cppreference.com/w/cpp/iterator/back_inserter template void getWrench(OutputIterator output) { boost::unique_lock lock(jt_mutex); - boost::copy(armState_.wrenchJava,output); - + boost::shared_ptr JAVAdriverP_->getWrench(output); } volatile std::size_t m_haveReceivedRealDataCount = 0; diff --git a/include/grl/kuka/KukaFRIdriver.hpp b/include/grl/kuka/KukaFRIdriver.hpp index 7708d9c..4dbee22 100644 --- a/include/grl/kuka/KukaFRIdriver.hpp +++ b/include/grl/kuka/KukaFRIdriver.hpp @@ -26,6 +26,8 @@ #include "grl/exception.hpp" #include "grl/vector_ostream.hpp" #include "grl/kuka/KukaFRIalgorithm.hpp" +// used for time synchronization +#include "grl/TimeEvent.hpp" /// @todo TODO(ahundt) REMOVE SPDLOG FROM LOW LEVEL CODE @@ -394,14 +396,17 @@ void update_state(boost::asio::ip::udp::socket &socket, std::size_t &receive_bytes_transferred, boost::system::error_code &send_ec, std::size_t &send_bytes_transferred, - boost::asio::ip::udp::endpoint sender_endpoint = - boost::asio::ip::udp::endpoint()) { + grl::TimeEvent& timeEvent, + boost::asio::ip::udp::endpoint sender_endpoint = boost::asio::ip::udp::endpoint()) { static const int message_flags = 0; + // get a local clock timestamp, then the latest frame from the device, then another timestamp + timeEvent.local_request_time = cartographer::common::UniversalTimeScaleClock::now(); receive_bytes_transferred = socket.receive_from( boost::asio::buffer(friData.receiveBuffer, KUKA::FRI::FRI_MONITOR_MSG_MAX_SIZE), sender_endpoint, message_flags, receive_ec); + timeEvent.local_receive_time = cartographer::common::UniversalTimeScaleClock::now(); decode(friData, receive_bytes_transferred); friData.lastSendCounter++; @@ -565,7 +570,8 @@ class KukaFRIClientDataDriver boost::system::error_code &receive_ec, std::size_t &receive_bytes_transferred, boost::system::error_code &send_ec, - std::size_t &send_bytes_transferred) { + std::size_t &send_bytes_transferred, + grl::TimeEvent& timeEvent) { if (exceptionPtr) { /// @note this exception most likely came from the update() call running @@ -615,7 +621,7 @@ class KukaFRIClientDataDriver validFriDataLatestState)) { // all storage is full, return the spare data to the user std::tie(step_alg_params, friData, receive_ec, receive_bytes_transferred, send_ec, - send_bytes_transferred) = validFriDataLatestState; + send_bytes_transferred, timeEvent) = validFriDataLatestState; } } @@ -688,7 +694,8 @@ class KukaFRIClientDataDriver std::get(nextState), std::get(nextState), std::get(nextState), - std::get(nextState)); + std::get(nextState), + std::get(nextState)); /// @todo use atomics to eliminate the global mutex lock for this object // lock the mutex to communicate with the user thread @@ -799,13 +806,18 @@ class KukaFRIClientDataDriver latest_receive_ec, latest_receive_bytes_transferred, latest_send_ec, - latest_send_bytes_transferred + latest_send_bytes_transferred, + latest_time_event_data }; + /// this is the object that stores all data for the latest device state + /// including the KUKA defined ClientData object, and a grl defined TimeEvent + /// which stores the time data needed for synchronization. typedef std::tuple, std::shared_ptr, boost::system::error_code, std::size_t, - boost::system::error_code, std::size_t> + boost::system::error_code, std::size_t, + grl::TimeEvent> LatestState; /// Creates a default LatestState Object @@ -814,7 +826,7 @@ class KukaFRIClientDataDriver std::shared_ptr &clientData) { return std::make_tuple(lowLevelAlgorithmParams, clientData, boost::system::error_code(), std::size_t(), boost::system::error_code(), - std::size_t()); + std::size_t(), grl::TimEvent()); } /// creates a shared_ptr to KUKA::FRI::ClientData with all command message @@ -823,8 +835,7 @@ class KukaFRIClientDataDriver static std::shared_ptr make_shared_valid_ClientData( std::shared_ptr &friData) { if (friData.get() == nullptr) { - friData = - std::make_shared(KUKA::LBRState::NUM_DOF); + friData = std::make_shared(KUKA::LBRState::NUM_DOF); // there is no commandMessage data on a new object friData->resetCommandMessage(); } @@ -844,8 +855,9 @@ class KukaFRIClientDataDriver std::shared_ptr lowLevelAlgorithmParams, std::shared_ptr &friData ) { - if (!friData) + if (!friData) { friData = make_shared_valid_ClientData(); + } return make_LatestState(lowLevelAlgorithmParams,friData); } @@ -1021,17 +1033,17 @@ class KukaFRIdriver : public std::enable_shared_from_this< * */ bool run_one() { + grl::TimeEvent time_event_stamp; // note: this one sends *and* receives the joint data! BOOST_VERIFY(kukaFRIClientDataDriverP_.get() != nullptr); /// @todo use runtime calculation of NUM_JOINTS instead of constant - if (!friData_) - friData_ = - std::make_shared(KUKA::LBRState::NUM_DOF); + if (!friData_) { + friData_ = std::make_shared(KUKA::LBRState::NUM_DOF); + } bool haveNewData = false; - static const std::size_t minimumConsecutiveSuccessesBeforeSendingCommands = - 100; + static const std::size_t minimumConsecutiveSuccessesBeforeSendingCommands = 100; std::shared_ptr lowLevelStepAlgorithmCommandParamsP; @@ -1069,7 +1081,7 @@ class KukaFRIdriver : public std::enable_shared_from_this< // sync with device over network haveNewData = !kukaFRIClientDataDriverP_->update_state( lowLevelStepAlgorithmCommandParamsP, friData_, recv_ec, recv_bytes, send_ec, - send_bytes); + send_bytes, time_event_stamp); m_attemptedCommunicationCount++; if (haveNewData) { @@ -1129,6 +1141,8 @@ class KukaFRIdriver : public std::enable_shared_from_this< armState.sendPeriod = std::chrono::milliseconds( grl::robot::arm::get(friData_->monitoringMsg, grl::time_step_tag())); + armState.time_event_stamp = time_event_stamp; + // std::cout << "Measured Torque: "; // std::cout << std::setw(6); // for (float t:armState.torque) { diff --git a/include/grl/kuka/KukaJAVAdriver.hpp b/include/grl/kuka/KukaJAVAdriver.hpp index 44ce4bb..bfcced4 100644 --- a/include/grl/kuka/KukaJAVAdriver.hpp +++ b/include/grl/kuka/KukaJAVAdriver.hpp @@ -10,7 +10,6 @@ #include #include -#include #include #include #include @@ -47,6 +46,7 @@ #include #include +#include namespace grl { namespace robot { namespace arm { @@ -366,46 +366,41 @@ namespace grl { namespace robot { namespace arm { // packets are available, process them if (FD_ISSET(socket_local, &temp_mask)) { - static const std::size_t udp_size = 1400; - unsigned char recbuf[udp_size]; - static const int flags = 0; - - ret = recvfrom(socket_local, recbuf, sizeof(recbuf), flags, (struct sockaddr *)&dst_sockaddr, &dst_sockaddr_len); - if (ret <= 0) logger_->error("C++ KukaJAVAdriver Error: Receive failed with ret = {}", ret); + // allocate the buffer, should only happen once + if(!java_interface_received_statesP_) { + java_interface_received_statesP_ = std::make_shared>(fbs_tk::Buffer(udp_size_)); + } + if(!java_interface_next_statesP_) { + java_interface_next_statesP_ = std::make_shared>(fbs_tk::Buffer(udp_size_)); + } - if (ret > 0){ - if(debug_) logger_->info("C++ KukaJAVAdriver received message size: {}",ret); + static const int flags = 0; + // get a reference to the buffer object + fbs_tk::Buffer& internal_buffer = java_interface_received_statesP_->get_data() + // receive an update from the java driver over UDP + ret = recvfrom(socket_local, reinterpret_cast(&internal_buffer.get_data()[0]), internal_buffer.size(), flags, (struct sockaddr *)&dst_sockaddr, &dst_sockaddr_len); + if (ret <= 0) { + bool java_state_received_successfully = false; + logger_->error("C++ KukaJAVAdriver Error: Receive failed with ret = {}", ret); + } else { - auto rbPstart = static_cast(recbuf); + if(debug_) logger_->info("C++ KukaJAVAdriver received message size: {}",ret); - auto verifier = flatbuffers::Verifier(rbPstart, ret); - auto bufOK = grl::flatbuffer::VerifyKUKAiiwaStatesBuffer(verifier); + java_interface_received_statesP_->update_root(); // Flatbuffer has been verified as valid - if (bufOK) { - // only reading the wrench data currently - auto bufff = static_cast(rbPstart); - if(debug_) logger_->info("C++ KukaJAVAdriver: flatbuffer verified successfully"); - - auto fbKUKAiiwaStates = grl::flatbuffer::GetKUKAiiwaStates(bufff); - auto wrench = fbKUKAiiwaStates->states()->Get(0)->monitorState()->CartesianWrench(); - - armState_.wrenchJava.clear(); - armState_.wrenchJava.push_back(wrench->force().x()); - armState_.wrenchJava.push_back(wrench->force().y()); - armState_.wrenchJava.push_back(wrench->force().z()); - armState_.wrenchJava.push_back(wrench->torque().x()); - armState_.wrenchJava.push_back(wrench->torque().y()); - armState_.wrenchJava.push_back(wrench->torque().z()); - - + if (java_interface_received_statesP_->valid()) { + bool java_state_received_successfully = true; + std::swap(java_interface_received_statesP_, java_interface_next_statesP_); + if(debug_) logger_->info("C++ KukaJAVAdriver: flatbuffer verified successfully"); } else { - logger_->error("C++ KukaJAVAdriver Error: flatbuff failed verification. bufOk: {}", bufOK); + // TODO(ahundt) consider specific error codes for verifier failure vs udp receive failure + bool java_state_received_successfully = false; + logger_->error("C++ KukaJAVAdriver Error: flatbuff failed verification. bufOk: {}", bufOK); } - } } @@ -600,12 +595,26 @@ namespace grl { namespace robot { namespace arm { state = armState_; } - void getWrench(KukaState & state) + /// get 6 element wrench entries + /// [force_x, force_y, force_z, torque_x, torque_y, torque_z] + template + void getWrench(OutputIterator output) { boost::lock_guard lock(jt_mutex); - if (!armState_.wrenchJava.empty()) { - state.wrenchJava = armState_.wrenchJava; + // make sure the object exists and contains data + if (java_interface_received_statesP_ && java_interface_received_statesP_->valid()) { + + // get the wrench state out + auto wrench = java_interface_received_statesP_->states()->Get(0)->monitorState()->CartesianWrench(); + + // insert the values into the output vector + *output++ = wrench->force().x(); + *output++ = wrench->force().y(); + *output++ = wrench->force().z(); + *output++ = wrench->torque().x(); + *output++ = wrench->torque().y(); + *output++ = wrench->torque().z(); } } @@ -627,6 +636,8 @@ namespace grl { namespace robot { namespace arm { private: + /// @TODO(ahundt) don't assume this fixed size, consider a parameter + static const std::size_t udp_size_ = 1400; std::shared_ptr logger_; int socket_local; int port; @@ -638,6 +649,16 @@ namespace grl { namespace robot { namespace arm { Params params_; KukaState armState_; // structure defined in Kuka.hpp + // this is the data that was received from the remote computer + // with the data from the java interface that gets sent back and forth + // this one will only contain valid received data + std::shared_ptr> java_interface_received_statesP_; + /// indicates if the next state was received successfully in java_interface_next_statesP_ + /// and java_interface_received_statesP_ was updated accordingly. + bool java_state_received_successfully = false; + /// used for receiving the next buffer over the network. + /// if some receive calls fail this one may not contain valid data. + std::shared_ptr> java_interface_next_statesP_; // armControlMode is the current GRL_Driver.java configuration to which the arm is currently set. // Options are: // ArmState_NONE = 0, diff --git a/include/grl/kuka/KukaToFlatbuffer.hpp b/include/grl/kuka/KukaToFlatbuffer.hpp index 650fd79..f5d3ab1 100644 --- a/include/grl/kuka/KukaToFlatbuffer.hpp +++ b/include/grl/kuka/KukaToFlatbuffer.hpp @@ -8,7 +8,8 @@ #include "grl/flatbuffer/KUKAiiwa_generated.h" #include "grl/flatbuffer/LinkObject_generated.h" #include "grl/flatbuffer/Euler_generated.h" -#include "grl/kuka/Kuka.hpp" +#include "Kuka.hpp" +#include "KukaFRIalgorithm.hpp" #include #include @@ -497,6 +498,73 @@ flatbuffers::Offset toFlatBuffer( monitorConfig); } + +/// KUKAiiwa.fbs +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const ::FRISessionState &sessionState, + const ::FRIConnectionQuality &connectionQuality, + const ::ControlMode &controlMode, // enum + const ::MessageHeader &messageHeader, + const ::TimeStamp &timeStamp, + const ::MessageMonitorData &message, + const ::MessageIpoData &ipoData) + +{ + auto _sessionState = toFlatBuffer(sessionState); + auto _connectionQuality = toFlatBuffer(connectionQuality); + auto _controlMode = toFlatBuffer(controlMode); + auto _messageIdentifier = messageHeader.messageIdentifier; + auto _sequenceCounter = messageHeader.sequenceCounter; + auto _reflectedSequenceCounter = messageHeader.reflectedSequenceCounter; + auto _sec = timeStamp.sec; + auto _nanosec = timeStamp.nanosec; + std::vector data; + // get measured joint position + grl::robot::arm::copy(message, std::back_inserter(data), grl::revolute_joint_angle_open_chain_state_tag()); + flatbuffers::Offset> _measuredJointPosition = fbb.CreateVector(data); + data.clear(); + // get measured joint torque + grl::robot::arm::copy(message, std::back_inserter(data), grl::revolute_joint_torque_open_chain_state_tag()); + flatbuffers::Offset> _measuredTorque = fbb.CreateVector(data); + data.clear(); + // get measured joint torque + grl::robot::arm::copy(message, std::back_inserter(data), grl::revolute_joint_torque_open_chain_command_tag()); + flatbuffers::Offset> _commandedJointPosition = fbb.CreateVector(data); + data.clear(); + // get commanded joint torque + grl::robot::arm::copy(message, std::back_inserter(data), grl::revolute_joint_torque_open_chain_command_tag()); + flatbuffers::Offset> _commandedTorque = fbb.CreateVector(data); + data.clear(); + // get measured external torque + grl::robot::arm::copy(message, std::back_inserter(data), grl::revolute_joint_torque_external_open_chain_state_tag()); + flatbuffers::Offset> _externalTorque = fbb.CreateVector(data); + data.clear(); + // get interpolated joint state + grl::robot::arm::copy(message, std::back_inserter(data), grl::revolute_joint_angle_interpolated_open_chain_state_tag()); + flatbuffers::Offset> _jointStateInterpolated = fbb.CreateVector(data); + auto _overlayType = toFlatBuffer(ipoData.overlayType); + return grl::flatbuffer::CreateFRIMessageLog( + fbb, + _sessionState, + _connectionQuality, + _controlMode, + _messageIdentifier, + _sequenceCounter, + _reflectedSequenceCounter, + _sec, + _nanosec, + _measuredJointPosition, + _measuredTorque, + _commandedJointPosition, + _commandedTorque, + _externalTorque, + _jointStateInterpolated, + _overlayType); +} + + + /// ArmControlState.fbs flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder &fbb, diff --git a/include/thirdparty/fbs_tk/fbs_tk.hpp b/include/thirdparty/fbs_tk/fbs_tk.hpp new file mode 100644 index 0000000..ecd6847 --- /dev/null +++ b/include/thirdparty/fbs_tk/fbs_tk.hpp @@ -0,0 +1,425 @@ +#ifndef _FBS_TK_HPP_ +#define _FBS_TK_HPP_ + +#include +#include +#include +#include +#include +#include +#include + +/** + * Add equality to flatbuffers::String + */ +namespace flatbuffers{ +inline bool string_eq(const flatbuffers::String &str1, const flatbuffers::String &str2) { + return str1.str() == str2.str(); +} +inline bool operator==(const flatbuffers::String &str1, const flatbuffers::String &str2) { + return string_eq(str1, str2); +} +inline bool operator!=(const flatbuffers::String &str1, const flatbuffers::String &str2) { + return !string_eq(str1, str2); +} +} + + +namespace fbs_tk { +// http://stackoverflow.com/questions/13037490/ +template +struct range +{ + range(std::istream& in): d_in(in) {} + std::istream& d_in; +}; + +template +std::istream_iterator begin(fbs_tk::range r) { + return std::istream_iterator(r.d_in); +} + +template +std::istream_iterator end(fbs_tk::range) { + return std::istream_iterator(); +} + +// Loads all the contents of the input stream into a string +inline bool load_buffer(std::istream &in, std::string *buf) { + *buf = std::string(std::istreambuf_iterator(in), + std::istreambuf_iterator()); + return !in.bad(); +} + +// Copies a buffer into a string + +inline void buffer_copy(const void *data, size_t size, std::string &dst) { + dst.resize(size); + memcpy(&dst[0], data, size); +} + +struct Buffer { + Buffer() : data() {} + + Buffer(std::size_t size) : data(size) {} + + Buffer(std::initializer_list args) : data(args) {} + + Buffer(std::vector data) : data(std::move(data)) {} + + Buffer(const std::string &str) : data(str.begin(), str.end()) {} + + Buffer(const Buffer &buff) : Buffer(buff.data) {} + + inline void copy_from(const void *buff, size_t buff_size) { + data.resize(buff_size); + memcpy(data.data(), buff, buff_size); + } + + inline void copy_from(const std::string &buff) { + copy_from(buff.data(), buff.size()); + } + + inline void copy_to(void *buff) const { + memcpy(buff, data.data(), data.size()); + } + + inline std::vector &get_data() { + return data; + } + + inline const std::vector &get_data() const { + return data; + } + + inline std::string str() const { + return std::string(data.begin(), data.end()); + } + + inline bool operator==(const Buffer &other) const { + return data == other.data; + } + + inline size_t size() const { + return data.size(); + } + + /** + * Writes all data in this buffer into the output stream. + */ + inline void write_data(std::ostream &out) const { + out.write(reinterpret_cast(data.data()), data.size()); + } + + /** + * Clears the data in the current buffer and then reads a certain number + * of bytes into this buffer. + */ + inline void read_data(std::istream &in, size_t size) { + data.clear(); + data.resize(size); + in.read(reinterpret_cast(data.data()), size); + } + + /** + * Loads all data available in the input stream into this buffer, + * replacing the contents of this buffer. + */ + inline std::istream & read_all_data(std::istream &in) { + data.clear(); + // make sure we do not skip precious bytes + // http://stackoverflow.com/questions/8075795/ + in >> std::noskipws; + data.assign(std::istream_iterator(in), std::istream_iterator()); + return in; + } + + +private: + std::vector data; + + friend std::istream &operator>>(std::istream &in, Buffer &buff) { + char size_buff[sizeof(uint32_t)]; + in.read(size_buff, sizeof(size_buff)); + if (! in) { + return in; + } + auto size = flatbuffers::ReadScalar(size_buff); + buff.read_data(in, size); + return in; + } + + friend std::ostream &operator<<(std::ostream &out, const Buffer &buff) { + uint32_t size = buff.size(); + char size_buff[sizeof(uint32_t)]; + flatbuffers::WriteScalar(size_buff, size); + out.write(size_buff, sizeof(uint32_t)); + buff.write_data(out); + return out; + } +}; + +inline void copy_from(Buffer &buffer, const flatbuffers::FlatBufferBuilder &builder) { + buffer.copy_from(builder.GetBufferPointer(), builder.GetSize()); +} + + +// converts a json object into a binary FBS +bool json_to_bin(flatbuffers::Parser &parser, const char *js, Buffer &bin); +// converts a binary FBS into a json object +std::string bin_to_json(flatbuffers::Parser &parser, const Buffer &bin); + +// converts a json object into a binary FBS +bool json_to_fbs(flatbuffers::Parser &parser, std::istream &in, std::ostream &out); +// converts a binary FBS into a json object +bool fbs_to_json(flatbuffers::Parser &parser, std::istream &in, std::ostream &out); + +// converts a stream of binary FBS into a stream of JSON objects +bool fbs_stream_to_jsonl(const std::string &schema, std::istream &in, std::ostream &out); +// converts a stream of JSON objects into a stream of binary FBS +bool jsonl_to_fbs_stream(const std::string &schema, std::istream &in, std::ostream &out); + +// GetRoot from a string +template +inline const T* get_root(const Buffer &buff) { + if (buff.size() == 0) { + // when the buffer is empty, buff.get_data().data() might not + // be initialized + return nullptr; + } + auto data = buff.get_data().data(); + auto check = flatbuffers::Verifier(data, buff.size()); + return check.VerifyBuffer() ? flatbuffers::GetRoot(data) : nullptr; +} + +template +inline const T* get_root_unsafe(const Buffer &buff) { + return flatbuffers::GetRoot(buff.get_data().data()); +} + + // Copy a FBS object. +template +struct copy { + flatbuffers::Offset operator()(flatbuffers::FlatBufferBuilder &, const T&) const; +}; + +// Create a root object and bundle it with its data +template +struct Root { + Root() : root(nullptr), data() {} + + Root(Buffer buff) : data(std::move(buff)) { + // TODO(ahundt) re-enable safe update by default! + // update_root(); + update_root_unsafe(); + } + + Root(const Root &other) : Root(other.data) {} + + /** + * Finishes the builder with obj as root and + * copies the data from the builder. + */ + Root(flatbuffers::FlatBufferBuilder &builder, flatbuffers::Offset obj) : data() { + builder.Finish(obj); + copy_from(data, builder); + update_root_unsafe(); + } + + const T* operator->() const { + assert(valid()); + return root; + } + + const T& operator*() const { + assert(valid()); + return *root; + } + + bool operator==(const Root &other) const { + assert(valid()); + return *root == *other.root; + } + + bool operator!=(const Root &other) const { + assert(valid()); + return *root != *other.root; + } + + bool valid() const { + return root != nullptr; + } + + const Buffer & get_data() const { + assert(valid()); + return data; + } + + void set_data(Buffer buff) { + data = std::move(buff); + update_root(); + } + + inline void update_root() { + root = get_root(data); + } + + inline void update_root_unsafe() { + root = get_root_unsafe(data); + } +private: + + friend std::istream &operator>>(std::istream &in, Root &root) { + in >> root.data; + root.update_root(); + if (!root.valid()) { + // mark the input stream as invalid + in.setstate(in.badbit); + } + return in; + } + + friend std::ostream &operator<<(std::ostream &out, const Root &root) { + assert(root.valid()); + out << root.data; + return out; + } + + const T* root; + Buffer data; +}; + +namespace root { + /** + * Copies a FBS object as a root object. + */ + template + static inline Root copy(const S& other) { + flatbuffers::FlatBufferBuilder b; + return Root(b, fbs_tk::copy()(b, other)); + } + + /** + * Copies a root object as a shared_copy of a root object. + */ + template + static inline std::shared_ptr> copy_shared(const Root &obj) { + return std::make_shared>(obj); + } + + /** + * Copies a FBS object as a shared root object. + */ + template + static inline std::shared_ptr> copy_shared(const S &obj) { + return root::copy_shared(root::copy(obj)); + } +} + +template +Root open_root(std::string filename) { + std::ifstream ifs(filename, std::ifstream::binary); + if (!ifs.is_open()) { + return Root(); + } + Buffer buff; + buff.read_all_data(ifs); + fbs_tk::Root result(buff); + if (ifs.bad()) { + ifs.close(); + return Root(); + } + ifs.close(); + return result; +} + +} // namespace + +// Define hash code for Root objects +namespace std { +template +struct hash> { + std::size_t operator()(fbs_tk::Root const& obj) const { + return std::hash()(*obj); + } +}; + +#include "fbs_tk/fbs_tk.hpp" + +#include + +using namespace std; +using namespace flatbuffers; + +namespace fbs_tk { + +bool json_to_bin(Parser &parser, const char *js, Buffer &bin) { + if (!parser.Parse(js)) { + return false; + } + copy_from(bin, parser.builder_); + return true; +} + +bool json_to_fbs(flatbuffers::Parser &parser, std::istream &in, std::ostream &out) { + string js; + if (!load_buffer(in, &js)) { + return false; + } + Buffer bin; + if (!json_to_bin(parser, js.c_str(), bin)) { + return false; + } + bin.write_data(out); + return true; +} + +string bin_to_json(Parser &parser, const Buffer &bin) { + string buffer; + GenerateText(parser, bin.get_data().data(), &buffer); + return buffer; +} + +// converts a binary FBS into a json object +bool fbs_to_json(flatbuffers::Parser &parser, std::istream &in, std::ostream &out) { + Buffer bin; + bin.read_all_data(in); + if (in.bad()) { + return false; + } + out << bin_to_json(parser, bin); + return out.good(); +} + +bool fbs_stream_to_jsonl(const string &schema, istream &in, ostream &out) { + Parser parser; + parser.Parse(schema.c_str()); + for (const auto & buff : range(in)) { + out << bin_to_json(parser, buff) << endl; + } + return in.eof(); +} + +bool jsonl_to_fbs_stream(const string &schema, istream &in, ostream &out) { + Parser parser; + parser.Parse(schema.c_str()); + string js; + Buffer bin; + while(getline(in, js)) { + if (!json_to_bin(parser, js.c_str(), bin)) { + return false; + } + out << bin; + if (out.bad()) { + return false; + } + } + return in.eof(); +} + +} // NAMESPACE + + + +} +#endif // _FBS_TK_HPP_ From 58bebbef1fa9d0885dae5725d5259eb4af9eb714 Mon Sep 17 00:00:00 2001 From: Chunting Date: Mon, 27 Nov 2017 00:38:01 -0500 Subject: [PATCH 010/102] NTERMEDIATE CODE: DOES NOT COMPILE, FIX SOME ERRORS --- include/grl/kuka/Kuka.hpp | 9 +- include/grl/kuka/KukaDriver.hpp | 21 ++- include/grl/kuka/KukaFRIdriver.hpp | 11 +- include/grl/kuka/KukaJAVAdriver.hpp | 103 ++++++++++---- include/grl/ros/KukaLBRiiwaROSPlugin.hpp | 6 +- include/thirdparty/fbs_tk/fbs_tk.hpp | 172 ++++++++++++----------- test/KukaFRITest.cpp | 41 +++--- 7 files changed, 219 insertions(+), 144 deletions(-) diff --git a/include/grl/kuka/Kuka.hpp b/include/grl/kuka/Kuka.hpp index 4aa6dc8..cb47546 100644 --- a/include/grl/kuka/Kuka.hpp +++ b/include/grl/kuka/Kuka.hpp @@ -15,6 +15,8 @@ #include "grl/flatbuffer/KUKAiiwa_generated.h" #include "grl/tags.hpp" #include "grl/exception.hpp" +// used for time synchronization +#include "grl/TimeEvent.hpp" #include #include @@ -145,8 +147,10 @@ struct KukaState { } }; +/// constexpr objects are const and are initiallized with values known during compilation constexpr auto KUKA_LBR_IIWA_14_R820 = "KUKA_LBR_IIWA_14_R820"; -constexpr auto KUKA_LBR_IIWA_7_R8, identified by00 = "KUKA_LBR_IIWA_7_R800"; +/// constexpr auto KUKA_LBR_IIWA_7_R8, identified by00 = "KUKA_LBR_IIWA_7_R800"; +constexpr auto KUKA_LBR_IIWA_7_R8 = "KUKA_LBR_IIWA_7_R800"; /// @brief copy vector of joint velocity limits in radians/s /// @@ -178,7 +182,8 @@ copy(std::string model, OutputIterator it, maxVel.push_back(2.356194490192); return boost::copy(maxVel, it); - } else if (boost::iequals(model, KUKA_LBR_IIWA_7_R800)) { + /// KUKA_LBR_IIWA_7_R800 to KUKA_LBR_IIWA_7_R820 + } else if (boost::iequals(model, "KUKA_LBR_IIWA_7_R800")) { /// @RK: updated the right joint velocity information based // on the 800 model from the KUKA manual diff --git a/include/grl/kuka/KukaDriver.hpp b/include/grl/kuka/KukaDriver.hpp index e2a3ef8..9360bda 100644 --- a/include/grl/kuka/KukaDriver.hpp +++ b/include/grl/kuka/KukaDriver.hpp @@ -235,7 +235,7 @@ namespace grl { namespace robot { namespace arm { if( boost::iequals(std::get(params_),std::string("FRI"))) { FRIdriverP_->get(armState_); - JAVAdriverP_->getWrench(armState_); + //JAVAdriverP_->getWrench(armState_); } } @@ -451,12 +451,19 @@ namespace grl { namespace robot { namespace arm { /// [force_x, force_y, force_z, torque_x, torque_y, torque_z] /// @param output a C++ OutputIterator which adds the values to your output object, /// see http://en.cppreference.com/w/cpp/iterator/back_inserter - template - void getWrench(OutputIterator output) - { - boost::unique_lock lock(jt_mutex); - boost::shared_ptr JAVAdriverP_->getWrench(output); - } + // template + // void getWrench(OutputIterator output) + // { + // boost::unique_lock lock(jt_mutex); + // JAVAdriverP_->getWrench(output); + // } + + template + void getWrench(Container output) + { + boost::unique_lock lock(jt_mutex); + JAVAdriverP_->getWrench(output); + } volatile std::size_t m_haveReceivedRealDataCount = 0; volatile std::size_t m_attemptedCommunicationCount = 0; diff --git a/include/grl/kuka/KukaFRIdriver.hpp b/include/grl/kuka/KukaFRIdriver.hpp index 4dbee22..cb3f2d4 100644 --- a/include/grl/kuka/KukaFRIdriver.hpp +++ b/include/grl/kuka/KukaFRIdriver.hpp @@ -585,7 +585,7 @@ class KukaFRIClientDataDriver !std::get(latestStateForUser_)) { // no new data, so immediately return results accordingly std::tie(step_alg_params, friData, receive_ec, receive_bytes_transferred, send_ec, - send_bytes_transferred) = make_LatestState(step_alg_params,friData); + send_bytes_transferred, timeEvent) = make_LatestState(step_alg_params,friData); return !haveNewData; } @@ -615,7 +615,7 @@ class KukaFRIClientDataDriver if (std::get(latestStateForUser_)) { // return the latest state to the caller std::tie(step_alg_params,friData, receive_ec, receive_bytes_transferred, send_ec, - send_bytes_transferred) = std::move(latestStateForUser_); + send_bytes_transferred, timeEvent) = std::move(latestStateForUser_); haveNewData = true; } else if (std::get( validFriDataLatestState)) { @@ -824,9 +824,10 @@ class KukaFRIClientDataDriver static LatestState make_LatestState(std::shared_ptr lowLevelAlgorithmParams, std::shared_ptr &clientData) { - return std::make_tuple(lowLevelAlgorithmParams, clientData, boost::system::error_code(), - std::size_t(), boost::system::error_code(), - std::size_t(), grl::TimEvent()); + return std::make_tuple(lowLevelAlgorithmParams, clientData, + boost::system::error_code(), std::size_t(), + boost::system::error_code(), std::size_t(), + grl::TimeEvent()); } /// creates a shared_ptr to KUKA::FRI::ClientData with all command message diff --git a/include/grl/kuka/KukaJAVAdriver.hpp b/include/grl/kuka/KukaJAVAdriver.hpp index bfcced4..655e77d 100644 --- a/include/grl/kuka/KukaJAVAdriver.hpp +++ b/include/grl/kuka/KukaJAVAdriver.hpp @@ -7,6 +7,7 @@ #include #include #include +#include // Required #include #include @@ -29,6 +30,12 @@ #include #include #include +// C++11, use std::false_type and std::true_type +#include +#include + +#include // Required +#include // Required //#ifdef BOOST_NO_CXX11_ATOMIC_SMART_PTR #include @@ -46,7 +53,7 @@ #include #include -#include +#include namespace grl { namespace robot { namespace arm { @@ -368,19 +375,20 @@ namespace grl { namespace robot { namespace arm { { // allocate the buffer, should only happen once if(!java_interface_received_statesP_) { - java_interface_received_statesP_ = std::make_shared>(fbs_tk::Buffer(udp_size_)); + java_interface_received_statesP_ = std::make_shared>(fbs_tk::Buffer(udp_size_)); } if(!java_interface_next_statesP_) { - java_interface_next_statesP_ = std::make_shared>(fbs_tk::Buffer(udp_size_)); + java_interface_next_statesP_ = std::make_shared>(fbs_tk::Buffer(udp_size_)); } static const int flags = 0; // get a reference to the buffer object - fbs_tk::Buffer& internal_buffer = java_interface_received_statesP_->get_data() + const fbs_tk::Buffer& internal_buffer = java_interface_received_statesP_->get_data(); // receive an update from the java driver over UDP - ret = recvfrom(socket_local, reinterpret_cast(&internal_buffer.get_data()[0]), internal_buffer.size(), flags, (struct sockaddr *)&dst_sockaddr, &dst_sockaddr_len); + std::vector bufferdata = internal_buffer.get_data(); + ret = recvfrom(socket_local, reinterpret_cast(&bufferdata[0]), internal_buffer.size(), flags, (struct sockaddr *)&dst_sockaddr, &dst_sockaddr_len); if (ret <= 0) { bool java_state_received_successfully = false; logger_->error("C++ KukaJAVAdriver Error: Receive failed with ret = {}", ret); @@ -398,7 +406,7 @@ namespace grl { namespace robot { namespace arm { } else { // TODO(ahundt) consider specific error codes for verifier failure vs udp receive failure bool java_state_received_successfully = false; - logger_->error("C++ KukaJAVAdriver Error: flatbuff failed verification. bufOk: {}", bufOK); + logger_->error("C++ KukaJAVAdriver Error: flatbuff failed verification. bufOk: {}", java_state_received_successfully); } } @@ -597,25 +605,57 @@ namespace grl { namespace robot { namespace arm { /// get 6 element wrench entries /// [force_x, force_y, force_z, torque_x, torque_y, torque_z] - template - void getWrench(OutputIterator output) - { - boost::lock_guard lock(jt_mutex); + /* + template + void getWrench(OutputIterator output) + { + boost::lock_guard lock(jt_mutex); // make sure the object exists and contains data - if (java_interface_received_statesP_ && java_interface_received_statesP_->valid()) { - - // get the wrench state out - auto wrench = java_interface_received_statesP_->states()->Get(0)->monitorState()->CartesianWrench(); - - // insert the values into the output vector - *output++ = wrench->force().x(); - *output++ = wrench->force().y(); - *output++ = wrench->force().z(); - *output++ = wrench->torque().x(); - *output++ = wrench->torque().y(); - *output++ = wrench->torque().z(); - } + if (java_interface_received_statesP_ && java_interface_received_statesP_->valid()) + { + std::cout<< "eqTypes Test: " << eqTypes() << std::endl; + // T* operator->() overload + // https://stackoverflow.com/questions/38542766/why-the-t-operator-is-applied-repeatedly-even-if-written-once + auto wrench = (*java_interface_received_statesP_)->states()->Get(0)->monitorState()->CartesianWrench(); + // insert the values into the output vector + *output++ = wrench->force().x(); // double + *output++ = wrench->force().y(); + *output++ = wrench->force().z(); + *output++ = wrench->torque().x(); + *output++ = wrench->torque().y(); + *output++ = wrench->torque().z(); + } + } + */ + template +void insertValues(OutputIterator result) +{ + for (int i = 0; i < 10; i++) + { + *(result++) = i; + } +} + template + void getWrench(Container output) + { + boost::lock_guard lock(jt_mutex); + + // make sure the object exists and contains data + if (java_interface_received_statesP_ && java_interface_received_statesP_->valid()) + { + std::cout<< "eqTypes Test: " << eqTypes() << std::endl; + // T* operator->() overload + // https://stackoverflow.com/questions/38542766/why-the-t-operator-is-applied-repeatedly-even-if-written-once + auto wrench = (*java_interface_received_statesP_)->states()->Get(0)->monitorState()->CartesianWrench(); + // insert the values into the output vector + output.push_back(wrench->force().x()); + output.push_back(wrench->force().y()); + output.push_back(wrench->force().z()); + output.push_back(wrench->torque().x()); + output.push_back(wrench->torque().y()); + output.push_back(wrench->torque().z()); + } } /// set the mode of the arm. Examples: Teach or MoveArmJointServo @@ -634,6 +674,19 @@ namespace grl { namespace robot { namespace arm { armControlMode = armControlMode_; } + ///////////////////////////////////////////////////////////////////// + /* Helper function for checking type equality in compile-time. */ + /// https://stackoverflow.com/questions/16924168/compile-time-function-for-checking-type-equality + template + struct is_same : std::false_type { }; + + template + struct is_same : std::true_type { }; + + template + constexpr bool eqTypes() { return is_same::value; } + ///////////////////////////////////////////////////////////////////// + private: /// @TODO(ahundt) don't assume this fixed size, consider a parameter @@ -652,13 +705,13 @@ namespace grl { namespace robot { namespace arm { // this is the data that was received from the remote computer // with the data from the java interface that gets sent back and forth // this one will only contain valid received data - std::shared_ptr> java_interface_received_statesP_; + std::shared_ptr> java_interface_received_statesP_; /// indicates if the next state was received successfully in java_interface_next_statesP_ /// and java_interface_received_statesP_ was updated accordingly. bool java_state_received_successfully = false; /// used for receiving the next buffer over the network. /// if some receive calls fail this one may not contain valid data. - std::shared_ptr> java_interface_next_statesP_; + std::shared_ptr> java_interface_next_statesP_; // armControlMode is the current GRL_Driver.java configuration to which the arm is currently set. // Options are: // ArmState_NONE = 0, diff --git a/include/grl/ros/KukaLBRiiwaROSPlugin.hpp b/include/grl/ros/KukaLBRiiwaROSPlugin.hpp index 770abc7..682e2af 100644 --- a/include/grl/ros/KukaLBRiiwaROSPlugin.hpp +++ b/include/grl/ros/KukaLBRiiwaROSPlugin.hpp @@ -364,7 +364,9 @@ namespace grl { if(haveNewData) { std::vector wrench_vector; - KukaDriverP_->getWrench(std::back_inserter(wrench_vector)); + // KukaDriverP_->getWrench(std::back_inserter(wrench_vector)); + // Using Container instead of OutputIterator to write into data + KukaDriverP_->getWrench(wrench_vector); if (!wrench_vector.empty()) { current_wrench.force.x = wrench_vector[0]; @@ -403,7 +405,7 @@ namespace grl { bool debug; std::size_t iteration_count_ = 0; - + grl::flatbuffer::ArmState interaction_mode; boost::mutex jt_mutex; diff --git a/include/thirdparty/fbs_tk/fbs_tk.hpp b/include/thirdparty/fbs_tk/fbs_tk.hpp index ecd6847..e34d32f 100644 --- a/include/thirdparty/fbs_tk/fbs_tk.hpp +++ b/include/thirdparty/fbs_tk/fbs_tk.hpp @@ -1,13 +1,13 @@ #ifndef _FBS_TK_HPP_ #define _FBS_TK_HPP_ - -#include #include +#include + #include #include #include #include -#include + /** * Add equality to flatbuffers::String @@ -157,7 +157,7 @@ struct Buffer { buff.write_data(out); return out; } -}; +}; // Struct Buffer inline void copy_from(Buffer &buffer, const flatbuffers::FlatBufferBuilder &builder) { buffer.copy_from(builder.GetBufferPointer(), builder.GetSize()); @@ -287,7 +287,7 @@ struct Root { const T* root; Buffer data; -}; +}; // namespace Root namespace root { /** @@ -333,7 +333,7 @@ Root open_root(std::string filename) { return result; } -} // namespace +} // namespace fbs_tk // Define hash code for Root objects namespace std { @@ -343,83 +343,85 @@ struct hash> { return std::hash()(*obj); } }; - -#include "fbs_tk/fbs_tk.hpp" - -#include - -using namespace std; -using namespace flatbuffers; - -namespace fbs_tk { - -bool json_to_bin(Parser &parser, const char *js, Buffer &bin) { - if (!parser.Parse(js)) { - return false; - } - copy_from(bin, parser.builder_); - return true; -} - -bool json_to_fbs(flatbuffers::Parser &parser, std::istream &in, std::ostream &out) { - string js; - if (!load_buffer(in, &js)) { - return false; - } - Buffer bin; - if (!json_to_bin(parser, js.c_str(), bin)) { - return false; - } - bin.write_data(out); - return true; -} - -string bin_to_json(Parser &parser, const Buffer &bin) { - string buffer; - GenerateText(parser, bin.get_data().data(), &buffer); - return buffer; -} - -// converts a binary FBS into a json object -bool fbs_to_json(flatbuffers::Parser &parser, std::istream &in, std::ostream &out) { - Buffer bin; - bin.read_all_data(in); - if (in.bad()) { - return false; - } - out << bin_to_json(parser, bin); - return out.good(); -} - -bool fbs_stream_to_jsonl(const string &schema, istream &in, ostream &out) { - Parser parser; - parser.Parse(schema.c_str()); - for (const auto & buff : range(in)) { - out << bin_to_json(parser, buff) << endl; - } - return in.eof(); -} - -bool jsonl_to_fbs_stream(const string &schema, istream &in, ostream &out) { - Parser parser; - parser.Parse(schema.c_str()); - string js; - Buffer bin; - while(getline(in, js)) { - if (!json_to_bin(parser, js.c_str(), bin)) { - return false; - } - out << bin; - if (out.bad()) { - return false; - } - } - return in.eof(); -} - -} // NAMESPACE - - - -} +} // namespace std + +// #include "thirdparty/fbs_tk/fbs_tk.hpp" +// #include + +// using namespace std; +// using namespace flatbuffers; + +// /// This fbs_tk is different from the above on, since this one is located in namestd; +// namespace fbs_tk { + +// bool json_to_bin(flatbuffers::Parser &parser, const char *js, Buffer &bin) { + +// if (!parser.Parse(static_cast(js))) { +// return false; +// } +// copy_from(bin, parser.builder_); +// return true; +// } + +// bool json_to_fbs(flatbuffers::Parser &parser, std::istream &in, std::ostream &out) { +// string js; +// if (!load_buffer(in, &js)) { +// return false; +// } +// Buffer bin; +// if (!json_to_bin(parser, js.c_str(), bin)) { +// return false; +// } +// bin.write_data(out); +// return true; +// } + +// string bin_to_json(flatbuffers::Parser &parser, const Buffer &bin) { +// string buffer; +// GenerateText(parser, reinterpret_cast(bin.get_data().data()), &buffer); +// return buffer; +// } + +// // converts a binary FBS into a json object +// bool fbs_to_json(flatbuffers::Parser &parser, std::istream &in, std::ostream &out) { +// Buffer bin; +// bin.read_all_data(in); +// if (in.bad()) { +// return false; +// } +// out << bin_to_json(parser, bin); +// return out.good(); +// } + +// bool fbs_stream_to_jsonl(const string &schema, istream &in, ostream &out) { +// flatbuffers::Parser parser; +// parser.Parse(schema.c_str()); +// for (const auto & buff : range(in)) { +// out << bin_to_json(parser, buff) << endl; +// } +// return in.eof(); +// } + +// bool jsonl_to_fbs_stream(const string &schema, istream &in, ostream &out) { +// flatbuffers::Parser parser; +// parser.Parse(schema.c_str()); +// string js; +// Buffer bin; +// while(getline(in, js)) { +// if (!json_to_bin(parser, js.c_str(), bin)) { +// return false; +// } +// out << bin; +// if (out.bad()) { +// return false; +// } +// } +// return in.eof(); +// } + +// } // NAMESPACE + + + +//} // namespace std #endif // _FBS_TK_HPP_ diff --git a/test/KukaFRITest.cpp b/test/KukaFRITest.cpp index 55732c8..770ef21 100644 --- a/test/KukaFRITest.cpp +++ b/test/KukaFRITest.cpp @@ -95,7 +95,7 @@ int main(int argc, char* argv[]) std::vector ipoJointPos(7,0); std::vector jointOffset(7,0); // length 7, value 0 boost::container::static_vector jointStateToCommand(7,0); - + // Absolute goal position to travel to in some modes of HowToMove // Set all 7 joints to go to a position 1 radian from the center std::vector absoluteGoalPos(7,0.2); @@ -103,13 +103,13 @@ int main(int argc, char* argv[]) /// TODO(ahundt) remove deprecated arm state from here and implementation grl::robot::arm::KukaState armState; std::unique_ptr lowLevelStepAlgorithmP; - + // Need to tell the system how long in milliseconds it has to reach the goal // or it will never move! std::size_t goal_position_command_time_duration = 4; lowLevelStepAlgorithmP.reset(new grl::robot::arm::LinearInterpolation()); // RobotModel (options are KUKA_LBR_IIWA_14_R820, KUKA_LBR_IIWA_7_R800) see grl::robot::arm::KukaState::KUKA_LBR_IIWA_14_R820 - + std::shared_ptr> highLevelDriverClassP; if(driverToUse == DriverToUse::low_level_fri_class) @@ -118,14 +118,14 @@ int main(int argc, char* argv[]) /// IDEA: PASS A LOW LEVEL STEP ALGORITHM PARAMS OBJECT ON EACH UPDATE AND ONLY ONE INSTANCE OF THE ALGORITHM OBJECT ITSELF highLevelDriverClassP = std::make_shared>(io_service, std::make_tuple("KUKA_LBR_IIWA_14_R820",localhost,localport,remotehost,remoteport/*,4 ms per tick*/,grl::robot::arm::KukaFRIClientDataDriver::run_automatically)); - + } - + std::shared_ptr socketP; - + if(driverToUse == DriverToUse::low_level_fri_function) { - + socketP = std::make_shared(io_service, boost::asio::ip::udp::endpoint(boost::asio::ip::address::from_string(localhost), boost::lexical_cast(localport))); boost::asio::ip::udp::resolver resolver(io_service); @@ -135,9 +135,9 @@ int main(int argc, char* argv[]) /// @todo maybe there is a more convienient way to set this that is easier for users? perhaps initializeClientDataForiiwa()? friData->expectedMonitorMsgID = KUKA::LBRState::LBRMONITORMESSAGEID; } - + std::shared_ptr kukaDriverP; - + if(driverToUse == DriverToUse::kuka_driver_high_level_class) { grl::robot::arm::KukaDriver::Params params = std::make_tuple( @@ -160,9 +160,9 @@ int main(int argc, char* argv[]) kukaDriverP->set(grl::flatbuffer::ArmState::MoveArmJointServo); kukaDriverP->set(goal_position_command_time_duration,grl::time_duration_command_tag()); std::cout << "KUKA COMMAND MODE: " << std::get(params) << "\n"; - + } - + unsigned int num_missed = 0; @@ -178,21 +178,26 @@ int main(int argc, char* argv[]) boost::system::error_code send_ec, recv_ec; std::size_t send_bytes_transferred = 0, recv_bytes_transferred = 0; bool haveNewData = false; - + if(driverToUse == DriverToUse::low_level_fri_class) { auto step_commandP = std::make_shared(std::make_tuple(jointStateToCommand,goal_position_command_time_duration)); - haveNewData = !highLevelDriverClassP->update_state(step_commandP, friData, recv_ec, recv_bytes_transferred, send_ec, send_bytes_transferred); + haveNewData = !highLevelDriverClassP->update_state(step_commandP, + friData, + recv_ec, + recv_bytes_transferred, + send_ec, + send_bytes_transferred); } - + if(driverToUse == DriverToUse::low_level_fri_function) { grl::robot::arm::update_state(*socketP,*lowLevelStepAlgorithmP,*friData,send_ec,send_bytes_transferred, recv_ec, recv_bytes_transferred); } - + if(driverToUse == DriverToUse::kuka_driver_high_level_class) { - + kukaDriverP->set( jointStateToCommand, grl::revolute_joint_angle_open_chain_command_tag()); kukaDriverP->run_one(); } @@ -200,7 +205,7 @@ int main(int argc, char* argv[]) // if data didn't arrive correctly, skip and try again if(send_ec || recv_ec ) { - + loggerPG->error("receive error: ", recv_ec, "receive bytes: ", recv_bytes_transferred, " send error: ", send_ec, " send bytes: ", send_bytes_transferred, " iteration: ", i); if(driverToUse == DriverToUse::low_level_fri_class) std::this_thread::sleep_for(std::chrono::milliseconds(1)); continue; @@ -260,7 +265,7 @@ int main(int argc, char* argv[]) boost::copy ( absoluteGoalPos, jointStateToCommand.begin()); } else if (howToMove == HowToMove::absolute_position_with_relative_rotation) { // go to a position relative to the current position - + boost::transform ( absoluteGoalPos, jointOffset, jointStateToCommand.begin(), std::plus()); } } From fb8f67d2a7cb962b6f51ead52166fc5874dc9384 Mon Sep 17 00:00:00 2001 From: Chunting Date: Mon, 27 Nov 2017 18:46:46 -0500 Subject: [PATCH 011/102] Errors fixed, the program can be compiled. HelperToFlatbuffer.hpp contains the common toFlatBuffer functions for both Kuka and fusiontracker. --- include/grl/flatbuffer/HelperToFlatbuffer.hpp | 82 +++++++++++++ include/grl/flatbuffer/KUKAiiwa.fbs | 5 + include/grl/kuka/KukaFRIdriver.hpp | 3 +- include/grl/kuka/KukaToFlatbuffer.hpp | 34 +++--- .../grl/sensor/FusionTrackToFlatbuffer.hpp | 109 +++++++++--------- test/KukaFRITest.cpp | 36 ++++-- 6 files changed, 191 insertions(+), 78 deletions(-) create mode 100644 include/grl/flatbuffer/HelperToFlatbuffer.hpp diff --git a/include/grl/flatbuffer/HelperToFlatbuffer.hpp b/include/grl/flatbuffer/HelperToFlatbuffer.hpp new file mode 100644 index 0000000..d65d04b --- /dev/null +++ b/include/grl/flatbuffer/HelperToFlatbuffer.hpp @@ -0,0 +1,82 @@ +#ifndef GRL_HELPER_TO_FLATBUFFER +#define GRL_HELPER_TO_FLATBUFFER + + +#include "grl/flatbuffer/Time_generated.h" +#include "grl/flatbuffer/Geometry_generated.h" +#include "grl/TimeEvent.hpp" +#include +#include +#include +#include +#include +#include +#include + +namespace grl +{ + +/// Helper function for both KUKA and FusionTrack +grl::flatbuffer::Vector3d toFlatBuffer(const Eigen::Vector3d &pt) +{ + return grl::flatbuffer::Vector3d(pt.x(), pt.y(), pt.z()); +} + +grl::flatbuffer::Quaternion toFlatBuffer(Eigen::Quaterniond q) +{ + return grl::flatbuffer::Quaternion(q.x(), q.y(), q.z(), q.w()); +} + +grl::flatbuffer::Pose toFlatBuffer(Eigen::Affine3d tf) +{ + Eigen::Vector3d pos = tf.translation(); + Eigen::Quaterniond eigenQuat(tf.rotation()); + return grl::flatbuffer::Pose(toFlatBuffer(pos), toFlatBuffer(eigenQuat)); +} + +grl::flatbuffer::Pose toFlatBuffer(Eigen::Affine3f tf) +{ + return toFlatBuffer(tf.cast()); +} + +template +typename T::value_type stringLength(const T &array) +{ + + auto iter = std::find(array.begin(), array.end(), '\0'); + auto len = std::distance(array.begin(), iter); + return len; +} + +flatbuffers::Offset +toFlatBuffer(flatbuffers::FlatBufferBuilder &fbb, + const grl::TimeEvent &timeStamp) +{ + flatbuffers::Offset event_name = fbb.CreateString(const_cast(timeStamp.event_name.begin()), stringLength(timeStamp.event_name)); + /// https://github.com/googlecartographer/cartographer/blob/master/cartographer/common/time.cc + /// convert time to int64 + int64_t local_request_time = cartographer::common::ToUniversal(timeStamp.local_request_time); + flatbuffers::Offset device_clock_id = fbb.CreateString(const_cast(timeStamp.device_clock_id.begin()), stringLength(timeStamp.device_clock_id)); + int64_t device_time = cartographer::common::ToUniversal(timeStamp.device_time); + flatbuffers::Offset local_clock_id = fbb.CreateString(const_cast(timeStamp.local_clock_id.begin()), stringLength(timeStamp.local_clock_id)); + int64_t local_receive_time = cartographer::common::ToUniversal(timeStamp.local_receive_time); + int64_t corrected_local_time = cartographer::common::ToUniversal(timeStamp.corrected_local_time); + int64_t clock_skew = cartographer::common::ToSeconds(timeStamp.clock_skew); + int64_t min_transport_delay = cartographer::common::ToSeconds(timeStamp.min_transport_delay); + return grl::flatbuffer::CreateTimeEvent( + fbb, + event_name, + local_request_time, + device_clock_id, + device_time, + local_clock_id, + local_receive_time, + corrected_local_time, + clock_skew, + min_transport_delay); +} + + +} // End of grl namespace + +#endif // GRL_HELPER_TO_FLATBUFFER diff --git a/include/grl/flatbuffer/KUKAiiwa.fbs b/include/grl/flatbuffer/KUKAiiwa.fbs index 0494130..7675c1e 100644 --- a/include/grl/flatbuffer/KUKAiiwa.fbs +++ b/include/grl/flatbuffer/KUKAiiwa.fbs @@ -284,6 +284,11 @@ table KUKAiiwaMonitorState { } +table FRITimeStamp { + sec: int; + nanosec: int; +} + // a table that is used as a record of all communication over FRI // it is not for user configuration or anything else, it should // simply reflect a history of what was sent and what was received. diff --git a/include/grl/kuka/KukaFRIdriver.hpp b/include/grl/kuka/KukaFRIdriver.hpp index cb3f2d4..86423fe 100644 --- a/include/grl/kuka/KukaFRIdriver.hpp +++ b/include/grl/kuka/KukaFRIdriver.hpp @@ -824,7 +824,8 @@ class KukaFRIClientDataDriver static LatestState make_LatestState(std::shared_ptr lowLevelAlgorithmParams, std::shared_ptr &clientData) { - return std::make_tuple(lowLevelAlgorithmParams, clientData, + return std::make_tuple(lowLevelAlgorithmParams, + clientData, boost::system::error_code(), std::size_t(), boost::system::error_code(), std::size_t(), grl::TimeEvent()); diff --git a/include/grl/kuka/KukaToFlatbuffer.hpp b/include/grl/kuka/KukaToFlatbuffer.hpp index f5d3ab1..5f6dfa6 100644 --- a/include/grl/kuka/KukaToFlatbuffer.hpp +++ b/include/grl/kuka/KukaToFlatbuffer.hpp @@ -8,6 +8,8 @@ #include "grl/flatbuffer/KUKAiiwa_generated.h" #include "grl/flatbuffer/LinkObject_generated.h" #include "grl/flatbuffer/Euler_generated.h" +#include "grl/flatbuffer/HelperToFlatbuffer.hpp" +#include "KukaFRIdriver.hpp" #include "Kuka.hpp" #include "KukaFRIalgorithm.hpp" #include @@ -499,51 +501,51 @@ flatbuffers::Offset toFlatBuffer( } + /// KUKAiiwa.fbs flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder &fbb, const ::FRISessionState &sessionState, const ::FRIConnectionQuality &connectionQuality, const ::ControlMode &controlMode, // enum - const ::MessageHeader &messageHeader, - const ::TimeStamp &timeStamp, - const ::MessageMonitorData &message, - const ::MessageIpoData &ipoData) - + const ::TimeStamp &timeStamp, // ::TimeStamp is defined in FRIMessages.pb.h + const ::FRIMonitoringMessage &friMonitoringMessage, + const grl::TimeEvent &timeEvent) // There are two times (TimeStamp and TimeEvent) here, which one should be kept? both? { auto _sessionState = toFlatBuffer(sessionState); auto _connectionQuality = toFlatBuffer(connectionQuality); auto _controlMode = toFlatBuffer(controlMode); - auto _messageIdentifier = messageHeader.messageIdentifier; - auto _sequenceCounter = messageHeader.sequenceCounter; - auto _reflectedSequenceCounter = messageHeader.reflectedSequenceCounter; + auto _messageIdentifier = friMonitoringMessage.header.messageIdentifier; + auto _sequenceCounter = friMonitoringMessage.header.sequenceCounter; + auto _reflectedSequenceCounter = friMonitoringMessage.header.reflectedSequenceCounter; auto _sec = timeStamp.sec; auto _nanosec = timeStamp.nanosec; std::vector data; // get measured joint position - grl::robot::arm::copy(message, std::back_inserter(data), grl::revolute_joint_angle_open_chain_state_tag()); + grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_angle_open_chain_state_tag()); flatbuffers::Offset> _measuredJointPosition = fbb.CreateVector(data); data.clear(); // get measured joint torque - grl::robot::arm::copy(message, std::back_inserter(data), grl::revolute_joint_torque_open_chain_state_tag()); + grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_torque_open_chain_state_tag()); flatbuffers::Offset> _measuredTorque = fbb.CreateVector(data); data.clear(); // get measured joint torque - grl::robot::arm::copy(message, std::back_inserter(data), grl::revolute_joint_torque_open_chain_command_tag()); + grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_torque_open_chain_command_tag()); flatbuffers::Offset> _commandedJointPosition = fbb.CreateVector(data); data.clear(); // get commanded joint torque - grl::robot::arm::copy(message, std::back_inserter(data), grl::revolute_joint_torque_open_chain_command_tag()); + grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_torque_open_chain_command_tag()); flatbuffers::Offset> _commandedTorque = fbb.CreateVector(data); data.clear(); // get measured external torque - grl::robot::arm::copy(message, std::back_inserter(data), grl::revolute_joint_torque_external_open_chain_state_tag()); + grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_torque_external_open_chain_state_tag()); flatbuffers::Offset> _externalTorque = fbb.CreateVector(data); data.clear(); // get interpolated joint state - grl::robot::arm::copy(message, std::back_inserter(data), grl::revolute_joint_angle_interpolated_open_chain_state_tag()); + grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_angle_interpolated_open_chain_state_tag()); flatbuffers::Offset> _jointStateInterpolated = fbb.CreateVector(data); - auto _overlayType = toFlatBuffer(ipoData.overlayType); + flatbuffers::Offset _timeEvent = grl::toFlatBuffer(fbb, timeEvent); + auto _overlayType = toFlatBuffer(friMonitoringMessage.ipoData.overlayType); return grl::flatbuffer::CreateFRIMessageLog( fbb, _sessionState, @@ -560,7 +562,7 @@ flatbuffers::Offset toFlatBuffer( _commandedTorque, _externalTorque, _jointStateInterpolated, - _overlayType); + _timeEvent); } diff --git a/include/grl/sensor/FusionTrackToFlatbuffer.hpp b/include/grl/sensor/FusionTrackToFlatbuffer.hpp index 1c68ee0..d821b7e 100644 --- a/include/grl/sensor/FusionTrackToFlatbuffer.hpp +++ b/include/grl/sensor/FusionTrackToFlatbuffer.hpp @@ -6,6 +6,7 @@ #include "grl/flatbuffer/FusionTrack_generated.h" #include "grl/flatbuffer/Time_generated.h" #include "grl/flatbuffer/LogKUKAiiwaFusionTrack_generated.h" +#include "grl/flatbuffer/HelperToFlatbuffer.hpp" #include #include #include @@ -46,27 +47,27 @@ grl::flatbuffer::Vector3d toFlatBuffer(const ::ftk3DPoint &pt) return grl::flatbuffer::Vector3d(pt.x, pt.y, pt.z); } -grl::flatbuffer::Vector3d toFlatBuffer(const Eigen::Vector3d &pt) -{ - return grl::flatbuffer::Vector3d(pt.x(), pt.y(), pt.z()); -} +// grl::flatbuffer::Vector3d toFlatBuffer(const Eigen::Vector3d &pt) +// { +// return grl::flatbuffer::Vector3d(pt.x(), pt.y(), pt.z()); +// } -grl::flatbuffer::Quaternion toFlatBuffer(Eigen::Quaterniond q) -{ - return grl::flatbuffer::Quaternion(q.x(), q.y(), q.z(), q.w()); -} +// grl::flatbuffer::Quaternion toFlatBuffer(Eigen::Quaterniond q) +// { +// return grl::flatbuffer::Quaternion(q.x(), q.y(), q.z(), q.w()); +// } -grl::flatbuffer::Pose toFlatBuffer(Eigen::Affine3d tf) -{ - Eigen::Vector3d pos = tf.translation(); - Eigen::Quaterniond eigenQuat(tf.rotation()); - return grl::flatbuffer::Pose(toFlatBuffer(pos), toFlatBuffer(eigenQuat)); -} +// grl::flatbuffer::Pose toFlatBuffer(Eigen::Affine3d tf) +// { +// Eigen::Vector3d pos = tf.translation(); +// Eigen::Quaterniond eigenQuat(tf.rotation()); +// return grl::flatbuffer::Pose(toFlatBuffer(pos), toFlatBuffer(eigenQuat)); +// } -grl::flatbuffer::Pose toFlatBuffer(Eigen::Affine3f tf) -{ - return toFlatBuffer(tf.cast()); -} +// grl::flatbuffer::Pose toFlatBuffer(Eigen::Affine3f tf) +// { +// return toFlatBuffer(tf.cast()); +// } /// the second parameter is merely a tag to uniquely identify this function so it compiles @@ -461,42 +462,42 @@ toFlatBuffer(flatbuffers::FlatBufferBuilder &fbb, fb_m_device_types); } -template -typename T::value_type stringLength(const T &array) -{ - - auto iter = std::find(array.begin(), array.end(), '\0'); - auto len = std::distance(array.begin(), iter); - return len; -} - -flatbuffers::Offset -toFlatBuffer(flatbuffers::FlatBufferBuilder &fbb, - const grl::TimeEvent &timeStamp) -{ - flatbuffers::Offset event_name = fbb.CreateString(const_cast(timeStamp.event_name.begin()), stringLength(timeStamp.event_name)); - /// https://github.com/googlecartographer/cartographer/blob/master/cartographer/common/time.cc - /// convert time to int64 - int64_t local_request_time = cartographer::common::ToUniversal(timeStamp.local_request_time); - flatbuffers::Offset device_clock_id = fbb.CreateString(const_cast(timeStamp.device_clock_id.begin()), stringLength(timeStamp.device_clock_id)); - int64_t device_time = cartographer::common::ToUniversal(timeStamp.device_time); - flatbuffers::Offset local_clock_id = fbb.CreateString(const_cast(timeStamp.local_clock_id.begin()), stringLength(timeStamp.local_clock_id)); - int64_t local_receive_time = cartographer::common::ToUniversal(timeStamp.local_receive_time); - int64_t corrected_local_time = cartographer::common::ToUniversal(timeStamp.corrected_local_time); - int64_t clock_skew = cartographer::common::ToSeconds(timeStamp.clock_skew); - int64_t min_transport_delay = cartographer::common::ToSeconds(timeStamp.min_transport_delay); - return grl::flatbuffer::CreateTimeEvent( - fbb, - event_name, - local_request_time, - device_clock_id, - device_time, - local_clock_id, - local_receive_time, - corrected_local_time, - clock_skew, - min_transport_delay); -} +// template +// typename T::value_type stringLength(const T &array) +// { + +// auto iter = std::find(array.begin(), array.end(), '\0'); +// auto len = std::distance(array.begin(), iter); +// return len; +// } + +// flatbuffers::Offset +// toFlatBuffer(flatbuffers::FlatBufferBuilder &fbb, +// const grl::TimeEvent &timeStamp) +// { +// flatbuffers::Offset event_name = fbb.CreateString(const_cast(timeStamp.event_name.begin()), stringLength(timeStamp.event_name)); +// /// https://github.com/googlecartographer/cartographer/blob/master/cartographer/common/time.cc +// /// convert time to int64 +// int64_t local_request_time = cartographer::common::ToUniversal(timeStamp.local_request_time); +// flatbuffers::Offset device_clock_id = fbb.CreateString(const_cast(timeStamp.device_clock_id.begin()), stringLength(timeStamp.device_clock_id)); +// int64_t device_time = cartographer::common::ToUniversal(timeStamp.device_time); +// flatbuffers::Offset local_clock_id = fbb.CreateString(const_cast(timeStamp.local_clock_id.begin()), stringLength(timeStamp.local_clock_id)); +// int64_t local_receive_time = cartographer::common::ToUniversal(timeStamp.local_receive_time); +// int64_t corrected_local_time = cartographer::common::ToUniversal(timeStamp.corrected_local_time); +// int64_t clock_skew = cartographer::common::ToSeconds(timeStamp.clock_skew); +// int64_t min_transport_delay = cartographer::common::ToSeconds(timeStamp.min_transport_delay); +// return grl::flatbuffer::CreateTimeEvent( +// fbb, +// event_name, +// local_request_time, +// device_clock_id, +// device_time, +// local_clock_id, +// local_receive_time, +// corrected_local_time, +// clock_skew, +// min_transport_delay); +// } flatbuffers::Offset toFlatBuffer(flatbuffers::FlatBufferBuilder &fbb, diff --git a/test/KukaFRITest.cpp b/test/KukaFRITest.cpp index 770ef21..a599b3e 100644 --- a/test/KukaFRITest.cpp +++ b/test/KukaFRITest.cpp @@ -47,7 +47,12 @@ int main(int argc, char* argv[]) std::size_t q_size = 4096; //queue size must be power of 2 spdlog::set_async_mode(q_size); std::shared_ptr loggerPG; - try { loggerPG = spdlog::stdout_logger_mt("console"); } catch (spdlog::spdlog_ex ex) { loggerPG = spdlog::get("console"); } + try { + loggerPG = spdlog::stdout_logger_mt("console"); + } + catch (spdlog::spdlog_ex ex) { + loggerPG = spdlog::get("console"); + } grl::periodic<> callIfMinPeriodPassed; HowToMove howToMove = HowToMove::absolute_position_with_relative_rotation;//HowToMove::absolute_position; HowToMove::relative_position; @@ -75,7 +80,7 @@ int main(int argc, char* argv[]) } std::cout << "using: " << argv[0] << " " << localhost << " " << localport << " " << remotehost << " " << remoteport << "\n"; - + // A single class for an I/O service object. boost::asio::io_service io_service; std::shared_ptr friData(std::make_shared(7)); @@ -117,7 +122,12 @@ int main(int argc, char* argv[]) /// @todo TODO(ahundt) BUG: Need way to supply time to reach specified goal for position control and eliminate this allocation internally in the kuka driver. See similar comment in KukaFRIDriver.hpp /// IDEA: PASS A LOW LEVEL STEP ALGORITHM PARAMS OBJECT ON EACH UPDATE AND ONLY ONE INSTANCE OF THE ALGORITHM OBJECT ITSELF highLevelDriverClassP = std::make_shared>(io_service, - std::make_tuple("KUKA_LBR_IIWA_14_R820",localhost,localport,remotehost,remoteport/*,4 ms per tick*/,grl::robot::arm::KukaFRIClientDataDriver::run_automatically)); + std::make_tuple("KUKA_LBR_IIWA_14_R820", + localhost, + localport, + remotehost, + remoteport/*,4 ms per tick*/, + grl::robot::arm::KukaFRIClientDataDriver::run_automatically)); } @@ -178,21 +188,33 @@ int main(int argc, char* argv[]) boost::system::error_code send_ec, recv_ec; std::size_t send_bytes_transferred = 0, recv_bytes_transferred = 0; bool haveNewData = false; - + grl::TimeEvent time_event_stamp; if(driverToUse == DriverToUse::low_level_fri_class) { - auto step_commandP = std::make_shared(std::make_tuple(jointStateToCommand,goal_position_command_time_duration)); + auto step_commandP = std::make_shared(std::make_tuple(jointStateToCommand, goal_position_command_time_duration)); + haveNewData = !highLevelDriverClassP->update_state(step_commandP, friData, recv_ec, recv_bytes_transferred, send_ec, - send_bytes_transferred); + send_bytes_transferred, + time_event_stamp); } if(driverToUse == DriverToUse::low_level_fri_function) { - grl::robot::arm::update_state(*socketP,*lowLevelStepAlgorithmP,*friData,send_ec,send_bytes_transferred, recv_ec, recv_bytes_transferred); + /// This update_state function is different from the above one. + /// They both are defined in KukaFRIdriver.hpp. + /// Also should the argument time_event_stam be the same with above one? + grl::robot::arm::update_state(*socketP, + *lowLevelStepAlgorithmP, + *friData, + send_ec, + send_bytes_transferred, + recv_ec, + recv_bytes_transferred, + time_event_stamp); } if(driverToUse == DriverToUse::kuka_driver_high_level_class) From 78818906192ce05ef62f33a49f9ae228b65e0b49 Mon Sep 17 00:00:00 2001 From: Chunting Date: Tue, 28 Nov 2017 00:04:17 -0500 Subject: [PATCH 012/102] TimeEvent::device_time is used to replace time_point_type --- include/grl/kuka/Kuka.hpp | 6 +++--- include/grl/kuka/KukaDriver.hpp | 4 ++-- include/grl/kuka/KukaFRIdriver.hpp | 13 +++++++++---- include/grl/kuka/KukaJAVAdriver.hpp | 9 +++++++-- test/KukaFRITest.cpp | 6 ++++-- 5 files changed, 25 insertions(+), 13 deletions(-) diff --git a/include/grl/kuka/Kuka.hpp b/include/grl/kuka/Kuka.hpp index cb47546..e555cd8 100644 --- a/include/grl/kuka/Kuka.hpp +++ b/include/grl/kuka/Kuka.hpp @@ -52,8 +52,8 @@ struct KukaState { joint_state; // cartesian state entries consist of a vector x,y,z and a quaternion [x, y, z, w] typedef boost::container::static_vector cartesian_state; - typedef std::chrono::time_point - time_point_type; + /// Class std::chrono::high_resolution_clock represents the clock with the smallest tick period provided by the implementation. + typedef std::chrono::time_point time_point_type; /// measured position, identified by revolute_joint_angle_open_chain_state_tag /// @see grl::revolute_joint_angle_open_chain_state_tag in grl/tags.hpp, which identifies this data @@ -94,7 +94,7 @@ struct KukaState { // the arm use commanded_goal_timestamp. /// TODO(Chunting) remove this and make all code that uses it instead set the time_event_stamp /// most likely the device_time, but double check the correctness of that - time_point_type timestamp; + /// time_point_type timestamp; ///////////////////////////////////////////////////////////////////////////////////////////// // members below here define the driver state and are not part of the FRI arm diff --git a/include/grl/kuka/KukaDriver.hpp b/include/grl/kuka/KukaDriver.hpp index 9360bda..af0de4d 100644 --- a/include/grl/kuka/KukaDriver.hpp +++ b/include/grl/kuka/KukaDriver.hpp @@ -380,9 +380,9 @@ namespace grl { namespace robot { namespace arm { * @see KukaFRIdriver::set(Range&& range, grl::revolute_joint_angle_open_chain_command_tag) * */ - KukaState::time_point_type get(time_point_tag) { + cartographer::common::Time get(time_point_tag) { boost::lock_guard lock(jt_mutex); - return armState_.timestamp; + return armState_.time_event_stamp.device_time; } /** diff --git a/include/grl/kuka/KukaFRIdriver.hpp b/include/grl/kuka/KukaFRIdriver.hpp index 86423fe..2f963ae 100644 --- a/include/grl/kuka/KukaFRIdriver.hpp +++ b/include/grl/kuka/KukaFRIdriver.hpp @@ -1252,10 +1252,15 @@ class KukaFRIdriver : public std::enable_shared_from_this< * grl::revolute_joint_angle_open_chain_command_tag) * */ - KukaState::time_point_type get(time_point_tag) { - boost::lock_guard lock(jt_mutex); - return armState.timestamp; - } + // KukaState::time_point_type get(time_point_tag) { + // boost::lock_guard lock(jt_mutex); + // return armState.timestamp; + // } + + cartographer::common::Time get(time_point_tag) { + boost::lock_guard lock(jt_mutex); + return armState.time_event_stamp.device_time; + } /** * \brief Set the applied joint torques for the current interpolation step. diff --git a/include/grl/kuka/KukaJAVAdriver.hpp b/include/grl/kuka/KukaJAVAdriver.hpp index 655e77d..e915f44 100644 --- a/include/grl/kuka/KukaJAVAdriver.hpp +++ b/include/grl/kuka/KukaJAVAdriver.hpp @@ -542,13 +542,18 @@ namespace grl { namespace robot { namespace arm { * @see KukaFRIdriver::set(Range&& range, grl::revolute_joint_angle_open_chain_command_tag) * */ - KukaState::time_point_type get(time_point_tag) { + // KukaState::time_point_type get(time_point_tag) { + // boost::lock_guard lock(jt_mutex); + // return armState_.timestamp; + // } + cartographer::common::Time get(time_point_tag) { boost::lock_guard lock(jt_mutex); - return armState_.timestamp; + return armState_.time_event_stamp.device_time; } + /** * \brief Set the applied joint torques for the current interpolation step. * diff --git a/test/KukaFRITest.cpp b/test/KukaFRITest.cpp index a599b3e..a8c399f 100644 --- a/test/KukaFRITest.cpp +++ b/test/KukaFRITest.cpp @@ -84,7 +84,8 @@ int main(int argc, char* argv[]) boost::asio::io_service io_service; std::shared_ptr friData(std::make_shared(7)); - std::chrono::time_point startTime; + /// std::chrono::time_point startTime; + cartographer::common::Time startTime; BOOST_VERIFY(friData); @@ -299,7 +300,8 @@ int main(int argc, char* argv[]) loggerPG->info( "position: {}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}{}", armState.position, " commanded Position: ", jointStateToCommand, - " us: ", std::chrono::duration_cast(armState.timestamp - startTime).count(), + /// " us: ", std::chrono::duration_cast(armState.timestamp - startTime).count(), + " us: ", std::chrono::duration_cast(armState.time_event_stamp.device_time - startTime).count(), " connectionQuality: ", EnumNameEConnectionQuality(armState.connectionQuality), " operationMode: ", EnumNameEOperationMode(armState.operationMode), " sessionState: ", EnumNameESessionState(armState.sessionState), From 1c30197e8066bf660530e60a8e39308c8b6b71b1 Mon Sep 17 00:00:00 2001 From: Chunting Date: Tue, 28 Nov 2017 12:01:32 -0500 Subject: [PATCH 013/102] Some toFlatBuffer funstions are added for ArmControlState.fbs --- include/grl/flatbuffer/KUKAiiwa.fbs | 6 +- include/grl/kuka/Kuka.hpp | 3 + include/grl/kuka/KukaToFlatbuffer.hpp | 120 ++++++++++++------ .../grl/sensor/FusionTrackToFlatbuffer.hpp | 4 + 4 files changed, 89 insertions(+), 44 deletions(-) diff --git a/include/grl/flatbuffer/KUKAiiwa.fbs b/include/grl/flatbuffer/KUKAiiwa.fbs index 7675c1e..87f2684 100644 --- a/include/grl/flatbuffer/KUKAiiwa.fbs +++ b/include/grl/flatbuffer/KUKAiiwa.fbs @@ -301,9 +301,9 @@ table FRIMessageLog { messageIdentifier:int; sequenceCounter:int; reflectedSequenceCounter:int; - // hardware timestamps - sec:int; - nanosec:int; // one billionth of a second + // hardware timestamps, replaced by the device_time in TimeEvent + // sec:int; + // nanosec:int; // one billionth of a second // MessageMonitorData measuredJointPosition:[double]; measuredTorque:[double]; diff --git a/include/grl/kuka/Kuka.hpp b/include/grl/kuka/Kuka.hpp index e555cd8..aa48663 100644 --- a/include/grl/kuka/Kuka.hpp +++ b/include/grl/kuka/Kuka.hpp @@ -94,6 +94,9 @@ struct KukaState { // the arm use commanded_goal_timestamp. /// TODO(Chunting) remove this and make all code that uses it instead set the time_event_stamp /// most likely the device_time, but double check the correctness of that + + /// This parameter is replaced by grl::TimeEvent time_event_stamp, + /// then how to integrate the TimeStamp in FRIMessage.pb.h? /// time_point_type timestamp; ///////////////////////////////////////////////////////////////////////////////////////////// diff --git a/include/grl/kuka/KukaToFlatbuffer.hpp b/include/grl/kuka/KukaToFlatbuffer.hpp index 5f6dfa6..5a89bc6 100644 --- a/include/grl/kuka/KukaToFlatbuffer.hpp +++ b/include/grl/kuka/KukaToFlatbuffer.hpp @@ -1,6 +1,10 @@ #ifndef GRL_KUKA_TO_FLATBUFFER #define GRL_KUKA_TO_FLATBUFFER +/// Before including any FlatBuffers related headers, you can add this #define. +/// You'll get an assert whenever the verifier fails, whose stack-trace can tell you exactly what check failed an on what field etc. +#define FLATBUFFERS_DEBUG_VERIFICATION_FAILURE + #include #include "grl/flatbuffer/JointState_generated.h" @@ -508,7 +512,7 @@ flatbuffers::Offset toFlatBuffer( const ::FRISessionState &sessionState, const ::FRIConnectionQuality &connectionQuality, const ::ControlMode &controlMode, // enum - const ::TimeStamp &timeStamp, // ::TimeStamp is defined in FRIMessages.pb.h + /// const ::TimeStamp &timeStamp, // ::TimeStamp is defined in FRIMessages.pb.h const ::FRIMonitoringMessage &friMonitoringMessage, const grl::TimeEvent &timeEvent) // There are two times (TimeStamp and TimeEvent) here, which one should be kept? both? { @@ -518,8 +522,8 @@ flatbuffers::Offset toFlatBuffer( auto _messageIdentifier = friMonitoringMessage.header.messageIdentifier; auto _sequenceCounter = friMonitoringMessage.header.sequenceCounter; auto _reflectedSequenceCounter = friMonitoringMessage.header.reflectedSequenceCounter; - auto _sec = timeStamp.sec; - auto _nanosec = timeStamp.nanosec; + // auto _sec = timeStamp.sec; + // auto _nanosec = timeStamp.nanosec; std::vector data; // get measured joint position grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_angle_open_chain_state_tag()); @@ -554,8 +558,8 @@ flatbuffers::Offset toFlatBuffer( _messageIdentifier, _sequenceCounter, _reflectedSequenceCounter, - _sec, - _nanosec, + // _sec, + // _nanosec, _measuredJointPosition, _measuredTorque, _commandedJointPosition, @@ -566,61 +570,95 @@ flatbuffers::Offset toFlatBuffer( } +// /// ArmControlState.fbs +// flatbuffers::Offset toFlatBuffer( +// flatbuffers::FlatBufferBuilder &fbb) +// { +// return grl::flatbuffer::CreateStartArm(fbb); +// } +// /// ArmControlState.fbs +// flatbuffers::Offset toFlatBuffer( +// flatbuffers::FlatBufferBuilder &fbb) +// { +// return grl::flatbuffer::CreateStopArm(fbb); +// } +// /// ArmControlState.fbs +// flatbuffers::Offset toFlatBuffer( +// flatbuffers::FlatBufferBuilder &fbb) +// { +// return grl::flatbuffer::CreatePauseArm(fbb); +// } +// /// ArmControlState.fbs +// flatbuffers::Offset toFlatBuffer( +// flatbuffers::FlatBufferBuilder &fbb) +// { +// return grl::flatbuffer::CreateTeachArm(fbb); +// } +// /// ArmControlState.fbs +// flatbuffers::Offset toFlatBuffer( +// flatbuffers::FlatBufferBuilder &fbb) +// { +// return grl::flatbuffer::CreateShutdownArm(fbb); +// } /// ArmControlState.fbs flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder &fbb, - std::vector> &traj - ) + const std::vector> &traj) { - flatbuffers::Offset>> traj_vector = fbb.CreateVector(traj.data(), traj.size()); return grl::flatbuffer::CreateMoveArmTrajectory( fbb, - traj_vector); + traj.empty()?fbb.CreateVector>(traj):0); } /// ArmControlState.fbs flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder &fbb, - flatbuffers::Offset &goal - ) + flatbuffers::Offset &goal) +{ + return grl::flatbuffer::CreateMoveArmJointServo (fbb, goal); +} +// /// ArmControlState.fbs +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const std::string &parent, + const grl::flatbuffer::Pose &goal) { - return grl::flatbuffer::CreateMoveArmJointServo( + return grl::flatbuffer::CreateMoveArmCartesianServo( fbb, - goal); + fbb.CreateString(parent), + std::addressof(goal)); } -/// ArmControlState.fbs -// flatbuffers::Offset toFlatBuffer( -// flatbuffers::FlatBufferBuilder &fbb, -// const std::string &parent, -// const grl::flatbuffer::Pose &goal) -// { -// return grl::flatbuffer::CreateMoveArmJointServo( -// fbb, -// fbb.CreateString(parent), -// std::addressof(goal) -// ); -// } +/// ArmControlState.fbs +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const std::string &name, + int64_t sequenceNumber, + double timeStamp) +{ + // The parameter of ArmState is undetermined. + grl::flatbuffer::ArmState armstate_type = grl::flatbuffer::ArmState::StartArm; + auto command = grl::flatbuffer::CreateStartArm(fbb); + return grl::flatbuffer::CreateArmControlState( + fbb, + fbb.CreateString(name), + sequenceNumber, + timeStamp, + armstate_type, + command.Union()); +} /// ArmControlState.fbs -// flatbuffers::Offset toFlatBuffer( -// flatbuffers::FlatBufferBuilder &fbb, -// const std::string &name, -// int64_t sequenceNumber, -// double timeStamp) -// { -// // The parameter of ArmState is undetermined. -// grl::flatbuffer::ArmState state_type = grl::flatbuffer::ArmStat::StartArm; -// return grl::flatbuffer::CreateArmControlState( -// fbb, -// fbb.CreateString(name), -// sequenceNumber, -// timeStamp, -// _state_type, -// _state_type.Union()); -// } +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const std::vector> &armcontrolstates) +{ + return grl::flatbuffer::CreateArmControlSeries( + fbb, + armcontrolstates.empty()?fbb.CreateVector>(armcontrolstates):0); +} } // End of arm namespace } // End of robot namespace } // End of grl namespace diff --git a/include/grl/sensor/FusionTrackToFlatbuffer.hpp b/include/grl/sensor/FusionTrackToFlatbuffer.hpp index d821b7e..bdad1aa 100644 --- a/include/grl/sensor/FusionTrackToFlatbuffer.hpp +++ b/include/grl/sensor/FusionTrackToFlatbuffer.hpp @@ -1,6 +1,10 @@ #ifndef GRL_ATRACSYS_FUSION_TRACK_TO_FLATBUFFER #define GRL_ATRACSYS_FUSION_TRACK_TO_FLATBUFFER +/// Before including any FlatBuffers related headers, you can add this #define. +/// You'll get an assert whenever the verifier fails, whose stack-trace can tell you exactly what check failed an on what field etc. +#define FLATBUFFERS_DEBUG_VERIFICATION_FAILURE + #include "FusionTrackToEigen.hpp" #include "FusionTrack.hpp" #include "grl/flatbuffer/FusionTrack_generated.h" From ea0be8f81fcf612588fcf2057ab0331671670fd4 Mon Sep 17 00:00:00 2001 From: Chunting Date: Tue, 28 Nov 2017 13:19:16 -0500 Subject: [PATCH 014/102] All the toFlatBuffer functions can compile successfully. --- include/grl/kuka/KukaToFlatbuffer.hpp | 342 ++++++++++++-------------- 1 file changed, 155 insertions(+), 187 deletions(-) diff --git a/include/grl/kuka/KukaToFlatbuffer.hpp b/include/grl/kuka/KukaToFlatbuffer.hpp index 5a89bc6..ea192ed 100644 --- a/include/grl/kuka/KukaToFlatbuffer.hpp +++ b/include/grl/kuka/KukaToFlatbuffer.hpp @@ -148,7 +148,7 @@ grl::flatbuffer::EOverlayType toFlatBuffer(const ::OverlayType overlayType) { } return grl::flatbuffer::EOverlayType::NO_OVERLAY; } - +/// Euler.fbs flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder &fbb, const double x, @@ -157,8 +157,7 @@ flatbuffers::Offset toFlatBuffer( { return grl::flatbuffer::CreateEulerTranslationParams(fbb, x, y, z); } -/// Euler.fbs struct EulerRotationParams -/// enum EulerOrder also defined in Euler.fbs +/// Euler.fbs flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder &fbb, const double r1, @@ -168,14 +167,7 @@ flatbuffers::Offset toFlatBuffer( { return grl::flatbuffer::CreateEulerRotationParams(fbb, r1, r2, r3, eulerOrder); } -/////////////////////////////////////////////////////////////////// -// Helper function is also defined in FusionTrackToFlatbuffer.hpp -grl::flatbuffer::Vector3d toFlatBuffer(const Eigen::Vector3d &pt) -{ - return grl::flatbuffer::Vector3d(pt.x(), pt.y(), pt.z()); -} -////////////////////////////////////////////////////////////////// /// Euler.fbs, struct EulerRotation grl::flatbuffer::EulerRotation toFlatBuffer( const Eigen::Vector3d &pt, @@ -190,6 +182,7 @@ grl::flatbuffer::EulerPose toFlatBuffer( { return grl::flatbuffer::EulerPose(positon, eulerRotation); } +/// Euler.fbs /// Overload the above function /// TODO (@Chunting) Check if ptr has the same physical meaning with pt, if so, discard it. grl::flatbuffer::EulerPose toFlatBuffer( @@ -197,14 +190,14 @@ grl::flatbuffer::EulerPose toFlatBuffer( const Eigen::Vector3d &ptr, grl::flatbuffer::EulerOrder eulerOrder) { - auto positon = toFlatBuffer(pt); + auto positon = grl::toFlatBuffer(pt); auto eulerRotation = toFlatBuffer(ptr,eulerOrder); return grl::flatbuffer::EulerPose(positon, eulerRotation); } +/// Euler.fbs /// tables EulerPose and EulerPoseParams are both defined in Euler.fbs. -/// The same thing with EulerPose, this function can be overloaded with Eigen arguments. -/// Note that the parameters should be passed by pointer, if by reference, it can't be compiled. +/// Like EulerPose, this function can be also overloaded with Eigen arguments. flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder &fbb, const grl::flatbuffer::Vector3d &position, @@ -213,18 +206,120 @@ flatbuffers::Offset toFlatBuffer( return grl::flatbuffer::CreateEulerPoseParams(fbb, &position, &rotation); } -// grl::flatbuffer::Pose toFlatBuffer(Eigen::Affine3d tf) -// { -// Eigen::Vector3d pos = tf.translation(); -// Eigen::Quaterniond eigenQuat(tf.rotation()); -// return grl::flatbuffer::Pose(toFlatBuffer(pos), toFlatBuffer(eigenQuat)); -// } +/// LinkObject.fbs +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder fbb, + const std::string &name, + const std::string &parent, + const grl::flatbuffer::Pose &pose, // Using the helper function toFlatBuffer to get this parameter? + const grl::flatbuffer::Inertia &inertia) +{ + return grl::flatbuffer::CreateLinkObject( + fbb, + fbb.CreateString(name), + fbb.CreateString(parent), + std::addressof(pose), + std::addressof(inertia)); +} -// grl::flatbuffer::Pose toFlatBuffer(Eigen::Affine3f tf) -// { -// return toFlatBuffer(tf.cast()); -// } +/// JointState.fbs +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const std::vector &position, + const std::vector &velocity, + const std::vector &acceleration, + const std::vector &torque) +{ + return grl::flatbuffer::CreateJointState( + fbb, + position.empty() ? fbb.CreateVector(position) : 0, + velocity.empty() ? fbb.CreateVector(velocity) : 0, + acceleration.empty() ? fbb.CreateVector(acceleration) : 0, + torque.empty() ? fbb.CreateVector(torque) : 0); +} +/// JointState.fbs +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const std::vector& kukaStates, + const std::vector &velocity, + const std::vector &acceleration) +{ + std::vector position; + std::vector torque; + std::size_t sizeofStates = kukaStates.size(); + for(auto &kukaState : kukaStates){ + boost::copy(kukaState.position, &position[0]); + boost::copy(kukaState.torque, &torque[0]); + } + return grl::flatbuffer::CreateJointState( + fbb, + position.empty() ? fbb.CreateVector(position) : 0, + velocity.empty() ? fbb.CreateVector(velocity) : 0, + acceleration.empty() ? fbb.CreateVector(acceleration) : 0, + torque.empty() ? fbb.CreateVector(torque) : 0); +} + + +/// ArmControlState.fbs +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const std::vector> &traj) +{ + return grl::flatbuffer::CreateMoveArmTrajectory( + fbb, + traj.empty()?fbb.CreateVector>(traj):0); +} + +/// ArmControlState.fbs +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + flatbuffers::Offset &goal) +{ + return grl::flatbuffer::CreateMoveArmJointServo (fbb, goal); +} +// /// ArmControlState.fbs +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const std::string &parent, + const grl::flatbuffer::Pose &goal) +{ + return grl::flatbuffer::CreateMoveArmCartesianServo( + fbb, + fbb.CreateString(parent), + std::addressof(goal)); +} + + +/// ArmControlState.fbs +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const std::string &name, + int64_t sequenceNumber, + double timeStamp) +{ + // The parameter of ArmState is undetermined. + grl::flatbuffer::ArmState armstate_type = grl::flatbuffer::ArmState::StartArm; + auto command = grl::flatbuffer::CreateStartArm(fbb); + return grl::flatbuffer::CreateArmControlState( + fbb, + fbb.CreateString(name), + sequenceNumber, + timeStamp, + armstate_type, + command.Union()); +} + +/// ArmControlState.fbs +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const std::vector> &armcontrolstates) +{ + return grl::flatbuffer::CreateArmControlSeries( + fbb, + armcontrolstates.empty()?fbb.CreateVector>(armcontrolstates):0); +} +/// KUKAiiwa.fbs flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder &fbb, const grl::flatbuffer::EulerPose& stiffness, @@ -247,7 +342,7 @@ flatbuffers::Offset toFlatBuffer std::addressof(maxControlForce), maxControlForceExceededStop); } - +/// KUKAiiwa.fbs flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder &fbb, std::vector &joint_stiffness, @@ -257,7 +352,7 @@ flatbuffers::Offset toFlatBuffer( auto jointDampingBuffer = fbb.CreateVector(joint_damping.data(),joint_damping.size()); return grl::flatbuffer::CreateJointImpedenceControlMode(fbb, jointStiffnessBuffer, jointDampingBuffer); } - +/// KUKAiiwa.fbs flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder &fbb, const ::OverlayType &overlayType, @@ -326,29 +421,9 @@ flatbuffers::Offset toFlatBuffer( fbb.CreateString(unit), fbb.CreateString(value), shouldRemove, - shouldUpdate - ); + shouldUpdate); } -/// LinkObject.fbs -flatbuffers::Offset toFlatBuffer( - flatbuffers::FlatBufferBuilder fbb, - const std::string &name, - const std::string &parent, - const grl::flatbuffer::Pose &pose, // Using the helper function toFlatBuffer to get this parameter? - const grl::flatbuffer::Inertia &inertia) - { - return grl::flatbuffer::CreateLinkObject( - fbb, - fbb.CreateString(name), - fbb.CreateString(parent), - std::addressof(pose), - std::addressof(inertia) - ); - } - - - /// KUKAiiwa.fbs flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder &fbb, @@ -386,44 +461,7 @@ flatbuffers::Offset toFlatBuffer( requestMonitorProcessData); } -/// JointState.fbs -/// JointValues in FRIMessages.pb.h, the same thing? -flatbuffers::Offset toFlatBuffer( - flatbuffers::FlatBufferBuilder &fbb, - const std::vector &position, - const std::vector &velocity, - const std::vector &acceleration, - const std::vector &torque) -{ - return grl::flatbuffer::CreateJointState( - fbb, - position.empty() ? fbb.CreateVector(position) : 0, - velocity.empty() ? fbb.CreateVector(velocity) : 0, - acceleration.empty() ? fbb.CreateVector(acceleration) : 0, - torque.empty() ? fbb.CreateVector(torque) : 0); -} - -flatbuffers::Offset toFlatBuffer( - flatbuffers::FlatBufferBuilder &fbb, - const std::vector& kukaStates, - const std::vector &velocity, - const std::vector &acceleration) -{ - std::vector position; - std::vector torque; - std::size_t sizeofStates = kukaStates.size(); - for(auto &kukaState : kukaStates){ - boost::copy(kukaState.position, &position[0]); - boost::copy(kukaState.torque, &torque[0]); - } - return grl::flatbuffer::CreateJointState( - fbb, - position.empty() ? fbb.CreateVector(position) : 0, - velocity.empty() ? fbb.CreateVector(velocity) : 0, - acceleration.empty() ? fbb.CreateVector(acceleration) : 0, - torque.empty() ? fbb.CreateVector(torque) : 0); -} /// KUKAiiwa.fbs flatbuffers::Offset toFlatBuffer( @@ -451,27 +489,6 @@ flatbuffers::Offset toFlatBuffer( ); } -// flatbuffers::Offset toFlatBuffer( -// flatbuffers::FlatBufferBuilder fbb, -// flatbuffers::Offset &measuredState, -// const grl::flatbuffer::Pose &cartesianFlangePose, -// flatbuffers::Offset &jointStateReal, -// flatbuffers::Offset &jointStateInterpolated, -// flatbuffers::Offset &externalState, -// const ::OperationMode &operationMode, -// const grl::flatbuffer::Wrench &CartesianWrench) -// { -// return grl::flatbuffer::CreateJointState( -// fbb, -// measuredState, -// cartesianFlangePose, -// jointStateReal, -// jointStateInterpolated, -// externalState, -// toFlatBuffer(operationMode), -// CartesianWrench); -// } - /// KUKAiiwa.fbs flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder &fbb, @@ -568,96 +585,47 @@ flatbuffers::Offset toFlatBuffer( _jointStateInterpolated, _timeEvent); } - - -// /// ArmControlState.fbs -// flatbuffers::Offset toFlatBuffer( -// flatbuffers::FlatBufferBuilder &fbb) -// { -// return grl::flatbuffer::CreateStartArm(fbb); -// } -// /// ArmControlState.fbs -// flatbuffers::Offset toFlatBuffer( -// flatbuffers::FlatBufferBuilder &fbb) -// { -// return grl::flatbuffer::CreateStopArm(fbb); -// } -// /// ArmControlState.fbs -// flatbuffers::Offset toFlatBuffer( -// flatbuffers::FlatBufferBuilder &fbb) -// { -// return grl::flatbuffer::CreatePauseArm(fbb); -// } -// /// ArmControlState.fbs -// flatbuffers::Offset toFlatBuffer( -// flatbuffers::FlatBufferBuilder &fbb) -// { -// return grl::flatbuffer::CreateTeachArm(fbb); -// } -// /// ArmControlState.fbs -// flatbuffers::Offset toFlatBuffer( -// flatbuffers::FlatBufferBuilder &fbb) -// { -// return grl::flatbuffer::CreateShutdownArm(fbb); -// } - -/// ArmControlState.fbs -flatbuffers::Offset toFlatBuffer( - flatbuffers::FlatBufferBuilder &fbb, - const std::vector> &traj) -{ - return grl::flatbuffer::CreateMoveArmTrajectory( - fbb, - traj.empty()?fbb.CreateVector>(traj):0); -} - -/// ArmControlState.fbs -flatbuffers::Offset toFlatBuffer( - flatbuffers::FlatBufferBuilder &fbb, - flatbuffers::Offset &goal) -{ - return grl::flatbuffer::CreateMoveArmJointServo (fbb, goal); -} -// /// ArmControlState.fbs -flatbuffers::Offset toFlatBuffer( - flatbuffers::FlatBufferBuilder &fbb, - const std::string &parent, - const grl::flatbuffer::Pose &goal) -{ - return grl::flatbuffer::CreateMoveArmCartesianServo( - fbb, - fbb.CreateString(parent), - std::addressof(goal)); -} - - -/// ArmControlState.fbs -flatbuffers::Offset toFlatBuffer( +/// KUKAiiwa.fbs +flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder &fbb, const std::string &name, - int64_t sequenceNumber, - double timeStamp) + const std::string &destination, + const std::string &source, + const double timestamp, + const bool setArmControlState, + const flatbuffers::Offset &armControlState, + const bool setArmConfiguration, + const flatbuffers::Offset &armConfiguration, + const bool hasMonitorState, + const flatbuffers::Offset &monitorState, + const bool hasMonitorConfig, + const flatbuffers::Offset &monitorConfig, + const flatbuffers::Offset &FRIMessage) { - // The parameter of ArmState is undetermined. - grl::flatbuffer::ArmState armstate_type = grl::flatbuffer::ArmState::StartArm; - auto command = grl::flatbuffer::CreateStartArm(fbb); - return grl::flatbuffer::CreateArmControlState( - fbb, - fbb.CreateString(name), - sequenceNumber, - timeStamp, - armstate_type, - command.Union()); + return grl::flatbuffer::CreateKUKAiiwaState( + fbb, + fbb.CreateString(name), + fbb.CreateString(destination), + fbb.CreateString(source), + timestamp, + setArmControlState, + armControlState, + setArmConfiguration, + armConfiguration, + hasMonitorState, + monitorState, + hasMonitorConfig, + monitorConfig, + FRIMessage); } - -/// ArmControlState.fbs -flatbuffers::Offset toFlatBuffer( +/// KUKAiiwa.fbs +flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder &fbb, - const std::vector> &armcontrolstates) + const std::vector> &kukaiiwastates) { - return grl::flatbuffer::CreateArmControlSeries( + return grl::flatbuffer::CreateKUKAiiwaStates( fbb, - armcontrolstates.empty()?fbb.CreateVector>(armcontrolstates):0); + kukaiiwastates.empty()?fbb.CreateVector>(kukaiiwastates):0); } } // End of arm namespace } // End of robot namespace From 8ec6db0dcd84708883ae42e6072b06cfea77578b Mon Sep 17 00:00:00 2001 From: Chunting Date: Wed, 29 Nov 2017 00:23:03 -0500 Subject: [PATCH 015/102] NOT FINISHED: kukaiiwaTest.hpp Unit Test for toFlatBuffer functions --- include/grl/kuka/KukaJAVAdriver.hpp | 14 +--- include/grl/kuka/KukaToFlatbuffer.hpp | 102 ++++++++++++++++++++++---- test/kukaiiwaTest.cpp | 99 +++++++++++++++++++++++++ 3 files changed, 192 insertions(+), 23 deletions(-) diff --git a/include/grl/kuka/KukaJAVAdriver.hpp b/include/grl/kuka/KukaJAVAdriver.hpp index e915f44..e2f6feb 100644 --- a/include/grl/kuka/KukaJAVAdriver.hpp +++ b/include/grl/kuka/KukaJAVAdriver.hpp @@ -444,8 +444,10 @@ namespace grl { namespace robot { namespace arm { // TODO: define custom flatbuffer for Cartesion Quantities void setCartesianImpedanceMode( - const grl::flatbuffer::EulerPose cart_stiffness, const grl::flatbuffer::EulerPose cart_damping, - const double nullspace_stiffness, const double nullspace_damping, + const grl::flatbuffer::EulerPose cart_stiffness, + const grl::flatbuffer::EulerPose cart_damping, + const double nullspace_stiffness, + const double nullspace_damping, const grl::flatbuffer::EulerPose cart_max_path_deviation, const grl::flatbuffer::EulerPose cart_max_ctrl_vel, const grl::flatbuffer::EulerPose cart_max_ctrl_force, @@ -633,14 +635,6 @@ namespace grl { namespace robot { namespace arm { } } */ - template -void insertValues(OutputIterator result) -{ - for (int i = 0; i < 10; i++) - { - *(result++) = i; - } -} template void getWrench(Container output) { diff --git a/include/grl/kuka/KukaToFlatbuffer.hpp b/include/grl/kuka/KukaToFlatbuffer.hpp index ea192ed..a842471 100644 --- a/include/grl/kuka/KukaToFlatbuffer.hpp +++ b/include/grl/kuka/KukaToFlatbuffer.hpp @@ -20,7 +20,7 @@ #include #include -namespace grl { namespace robot { namespace arm { +namespace grl { /// faltbuffer enum objects /// 1. Which element in the enum should be selected as the defaut return value? @@ -296,18 +296,94 @@ flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder &fbb, const std::string &name, int64_t sequenceNumber, - double timeStamp) + double timeStamp, + grl::flatbuffer::ArmState &armControlMode) { // The parameter of ArmState is undetermined. - grl::flatbuffer::ArmState armstate_type = grl::flatbuffer::ArmState::StartArm; - auto command = grl::flatbuffer::CreateStartArm(fbb); - return grl::flatbuffer::CreateArmControlState( - fbb, - fbb.CreateString(name), - sequenceNumber, - timeStamp, - armstate_type, - command.Union()); + // grl::flatbuffer::ArmState armstate_type = grl::flatbuffer::ArmState::armstate; + // auto command = grl::flatbuffer::CreateStartArm(fbb); + switch (armControlMode) { + case grl::flatbuffer::ArmState::StartArm: { + return grl::flatbuffer::CreateArmControlState( + fbb, + fbb.CreateString(name), + sequenceNumber, + timeStamp, + armControlMode, + grl::flatbuffer::CreateStartArm(fbb).Union()); + } + case grl::flatbuffer::ArmState::TeachArm: { + return grl::flatbuffer::CreateArmControlState( + fbb, + fbb.CreateString(name), + sequenceNumber, + timeStamp, + armControlMode, + grl::flatbuffer::CreateTeachArm(fbb).Union()); + } + case grl::flatbuffer::ArmState::PauseArm: { + return grl::flatbuffer::CreateArmControlState( + fbb, + fbb.CreateString(name), + sequenceNumber, + timeStamp, + armControlMode, + grl::flatbuffer::CreatePauseArm(fbb).Union()); + } + case grl::flatbuffer::ArmState::StopArm: { + return grl::flatbuffer::CreateArmControlState( + fbb, + fbb.CreateString(name), + sequenceNumber, + timeStamp, + armControlMode, + grl::flatbuffer::CreateStopArm(fbb).Union()); + } + case grl::flatbuffer::ArmState::ShutdownArm: { + return grl::flatbuffer::CreateArmControlState( + fbb, + fbb.CreateString(name), + sequenceNumber, + timeStamp, + armControlMode, + grl::flatbuffer::CreateShutdownArm(fbb).Union()); + } + case grl::flatbuffer::ArmState::NONE: { + std::cout << "Waiting for interation mode... (currently NONE)" << std::endl; + break; + } + default: + std::cout<< "C++ KukaJAVAdriver: unsupported use case: " << EnumNameArmState(armControlMode) << std::endl; + } +} + +/// ArmControlState.fbs +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const std::string &name, + int64_t sequenceNumber, + double timeStamp, + grl::robot::arm::KukaState &armState, + grl::flatbuffer::ArmState &armControlMode) +{ + switch (armControlMode) { + std::cout<< "C++ KukaJAVAdriver: sending armposition command: " << armState.commandedPosition_goal << std::endl; + case grl::flatbuffer::ArmState::MoveArmJointServo: { + auto armPositionBuffer = fbb.CreateVector(armState.commandedPosition_goal.data(), armState.commandedPosition_goal.size()); + auto commandedTorque = fbb.CreateVector(armState.commandedTorque.data(), armState.commandedTorque.size()); + auto goalJointState = grl::flatbuffer::CreateJointState(fbb,armPositionBuffer,0/*no velocity*/,0/*no acceleration*/,commandedTorque); + auto moveArmJointServo = grl::flatbuffer::CreateMoveArmJointServo(fbb,goalJointState); + return grl::flatbuffer::CreateArmControlState( + fbb, + fbb.CreateString(name), + sequenceNumber, + timeStamp, + armControlMode, + moveArmJointServo.Union()); + } + default: + toFlatBuffer(fbb, name, sequenceNumber, timeStamp, armControlMode); + } } /// ArmControlState.fbs @@ -627,8 +703,8 @@ flatbuffers::Offset toFlatBuffer( fbb, kukaiiwastates.empty()?fbb.CreateVector>(kukaiiwastates):0); } -} // End of arm namespace -} // End of robot namespace +// } // End of arm namespace +// } // End of robot namespace } // End of grl namespace diff --git a/test/kukaiiwaTest.cpp b/test/kukaiiwaTest.cpp index 228b587..33f6913 100644 --- a/test/kukaiiwaTest.cpp +++ b/test/kukaiiwaTest.cpp @@ -8,6 +8,9 @@ #include #include "grl/kuka/KukaToFlatbuffer.hpp" +#include "grl/kuka/KukaJAVAdriver.hpp" +#include "grl/kuka/Kuka.hpp" +#include "grl/kuka/KukaDriver.hpp" BOOST_AUTO_TEST_SUITE(KukaTest) @@ -19,6 +22,102 @@ BOOST_AUTO_TEST_CASE(runRepeatedly) // grl::robot::arm::KukaJAVAdriver::Parms javaDriverP = grl::robot::arm::KukaJAVAdriver::defaultParams(); // rl::robot::arm::KukaJAVAdriver javaDriver(javaDriverP); // javaDriver.construct(); + std::shared_ptr fbbP; + fbbP = std::make_shared(); + double duration = boost::chrono::high_resolution_clock::now().time_since_epoch().count(); + grl::robot::arm::KukaDriver::Params params = std::make_tuple( + "Robotiiwa" , // RobotName, + "KUKA_LBR_IIWA_14_R820" , // RobotModel (options are KUKA_LBR_IIWA_14_R820, KUKA_LBR_IIWA_7_R800) + "0.0.0.0" , // LocalUDPAddress + "30010" , // LocalUDPPort + "172.31.1.147" , // RemoteUDPAddress + "192.170.10.100" , // LocalHostKukaKoniUDPAddress, + "30200" , // LocalHostKukaKoniUDPPort, + "192.170.10.2" , // RemoteHostKukaKoniUDPAddress, + "30200" , // RemoteHostKukaKoniUDPPort + "JAVA" , // KukaCommandMode (options are FRI, JAVA) + "FRI" // KukaMonitorMode (options are FRI, JAVA) + ); + std::string RobotName("Robotiiwa" ); + std::string basename = RobotName; //std::get<0>(params); + + flatbuffers::Offset controlState; + grl::flatbuffer::ArmState armControlMode_(grl::flatbuffer::ArmState::StartArm); + + grl::robot::arm::KukaState armState; + std::shared_ptr friData(std::make_shared(7)); + cartographer::common::Time startTime; + double delta = -0.0005; + double delta_sum = 0; + /// consider moving joint angles based on time + int joint_to_move = 6; + + std::vector ipoJointPos(7,0); + std::vector jointOffset(7,0); // length 7, value 0 + boost::container::static_vector jointStateToCommand(7,0); + + // Absolute goal position to travel to in some modes of HowToMove + // Set all 7 joints to go to a position 1 radian from the center + std::vector absoluteGoalPos(7,0.2); + std::unique_ptr lowLevelStepAlgorithmP; + + // Need to tell the system how long in milliseconds it has to reach the goal + // or it will never move! + std::size_t goal_position_command_time_duration = 4; + lowLevelStepAlgorithmP.reset(new grl::robot::arm::LinearInterpolation()); + // RobotModel (options are KUKA_LBR_IIWA_14_R820, KUKA_LBR_IIWA_7_R800) see grl::robot::arm::KukaState::KUKA_LBR_IIWA_14_R820 + + std::shared_ptr> highLevelDriverClassP; + std::shared_ptr kukaDriverP; + + kukaDriverP=std::make_shared(params); + kukaDriverP->set(grl::flatbuffer::ArmState::MoveArmJointServo); + kukaDriverP->set(goal_position_command_time_duration,grl::time_duration_command_tag()); + std::cout << "KUKA COMMAND MODE: " << std::get(params) << "\n"; + int64_t sequenceNumber = 0; + controlState = grl::toFlatBuffer(*fbbP, basename, sequenceNumber++, duration, armState, armControlMode_); + + auto clientCommandMode = grl::flatbuffer::EClientCommandMode::POSITION; + auto overlayType = grl::flatbuffer::EOverlayType::NO_OVERLAY; + //TODO: Custom flatbuffer type. Load defaults from params/config + //Cartesian Impedance Values + // grl::flatbuffer::EulerOrder eulerOrder = grl::flatbuffer::EulerOrder::xyz; + // grl::flatbuffer::Vector3d cart_stiffness_trans_ = grl::flatbuffer::Vector3d(500,500,500); + // grl::flatbuffer::EulerRotation cart_stifness_rot_ = grl::toFlatBuffer(cart_stiffness_trans_, eulerOrder); + + // grl::flatbuffer::Vector3d cart_damping_trans_ = grl::flatbuffer::Vector3d(0.3,0.3,0.3); + // grl::flatbuffer::EulerRotation cart_damping_rot_ = grl::toFlatBuffer(cart_damping_trans_, eulerOrder); + + // grl::flatbuffer::EulerPose cart_stiffness_ = grl::toFlatBuffer(cart_stiffness_trans_, cart_stifness_rot_); + // grl::flatbuffer::EulerPose cart_damping_ = grl::toFlatBuffer(cart_damping_trans_, cart_damping_rot_); + + // grl::flatbuffer::Vector3d cart_max_path_deviation_trans_ = grl::flatbuffer::Vector3d(1000,1000,1000); + // grl::flatbuffer::Vector3d cart_max_path_deviation_rot_ = grl::flatbuffer::Vector3d(5., 5., 5.); + // grl::flatbuffer::Vector3d cart_max_ctrl_force_trans_ = grl::flatbuffer::Vector3d(200,200,200); + // grl::flatbuffer::Vector3d cart_max_ctrl_force_rot_ = grl::flatbuffer::Vector3d(6.3,6.3,6.3); + // grl::flatbuffer::EulerPose cart_max_path_deviation_ = grl::flatbuffer::EulerPose(cart_max_path_deviation_trans_, grl::toFlatBuffer(cart_max_path_deviation_rot_, eulerOrder)); + // grl::flatbuffer::EulerPose cart_max_ctrl_vel_ = grl::toFlatBuffer(cart_max_path_deviation_trans_, grl::toFlatBuffer(cart_max_ctrl_force_rot_, eulerOrder)); + // grl::flatbuffer::EulerPose cart_max_ctrl_force_ = grl::toFlatBuffer(cart_max_ctrl_force_trans_, grl::toFlatBuffer(cart_max_ctrl_force_trans_, eulerOrder)); + // double nullspace_stiffness_ = 2.0; + // double nullspace_damping_ = 0.5; + + + + + //auto stiffnessPose = flatbuffer::CreateEulerPoseParams(*fbbP,&cart_stiffness_trans_,&cart_stiffness_rot_); + //auto dampingPose = flatbuffer::CreateEulerPoseParams(*fbbP,&cart_damping_trans_,&cart_damping_rot_) + // auto setCartesianImpedance = grl::toFlatBuffer(*fbbP,) + + // grl::flatbuffer::CreateCartesianImpedenceControlMode(*fbbP, &cart_stiffness_, &cart_damping_, + // nullspace_stiffness_, nullspace_damping_, &cart_max_path_deviation_, &cart_max_ctrl_vel_, &cart_max_ctrl_force_, max_control_force_stop_) + // auto jointStiffnessBuffer = fbbP->CreateVector(joint_stiffness_.data(),joint_stiffness_.size()); + // auto jointDampingBuffer = fbbP->CreateVector(joint_damping_.data(),joint_damping_.size()) + // auto setJointImpedance = grl::flatbuffer::CreateJointImpedenceControlMode(*fbbP, jointStiffnessBuffer, jointDampingBuffer) + // auto kukaiiwaArmConfiguration = flatbuffer::CreateKUKAiiwaArmConfiguration(*fbbP,name,commandInterface_,monitorInterface_, clientCommandMode, overlayType, + // controlMode_, setCartesianImpedance, setJointImpedance); + + + } BOOST_AUTO_TEST_SUITE_END() From a1efda8dc0e5b5490f6d5197db379a940755a5e3 Mon Sep 17 00:00:00 2001 From: Chunting Date: Wed, 29 Nov 2017 18:50:09 -0500 Subject: [PATCH 016/102] NOT FINISHED: kukaiiwaTest.hpp for unit test --- include/grl/kuka/KukaToFlatbuffer.hpp | 15 ++- test/kukaiiwaTest.cpp | 152 ++++++++++++++++---------- 2 files changed, 100 insertions(+), 67 deletions(-) diff --git a/include/grl/kuka/KukaToFlatbuffer.hpp b/include/grl/kuka/KukaToFlatbuffer.hpp index a842471..aea1801 100644 --- a/include/grl/kuka/KukaToFlatbuffer.hpp +++ b/include/grl/kuka/KukaToFlatbuffer.hpp @@ -208,7 +208,7 @@ flatbuffers::Offset toFlatBuffer( /// LinkObject.fbs flatbuffers::Offset toFlatBuffer( - flatbuffers::FlatBufferBuilder fbb, + flatbuffers::FlatBufferBuilder &fbb, const std::string &name, const std::string &parent, const grl::flatbuffer::Pose &pose, // Using the helper function toFlatBuffer to get this parameter? @@ -514,10 +514,10 @@ flatbuffers::Offset toFlatBuffer( flatbuffers::Offset &setJointImpedance, flatbuffers::Offset &smartServoConfig, flatbuffers::Offset &FRIConfig, - flatbuffers::Offset>> &tools, - flatbuffers::Offset>> &processData, + const std::vector> &tools, + const std::vector> &processData, const std::string ¤tMotionCenter, - const bool requestMonitorProcessData) + bool requestMonitorProcessData = false) { return grl::flatbuffer::CreateKUKAiiwaArmConfiguration( fbb, @@ -531,9 +531,9 @@ flatbuffers::Offset toFlatBuffer( setJointImpedance, smartServoConfig, FRIConfig, - tools, - processData, - fbb.CreateString(currentMotionCenter), + tools.empty() ? fbb.CreateVector>(tools):0, + processData.empty() ? fbb.CreateVector>(processData) : 0, + currentMotionCenter.empty() ? fbb.CreateString(currentMotionCenter) : 0, requestMonitorProcessData); } @@ -605,7 +605,6 @@ flatbuffers::Offset toFlatBuffer( const ::FRISessionState &sessionState, const ::FRIConnectionQuality &connectionQuality, const ::ControlMode &controlMode, // enum - /// const ::TimeStamp &timeStamp, // ::TimeStamp is defined in FRIMessages.pb.h const ::FRIMonitoringMessage &friMonitoringMessage, const grl::TimeEvent &timeEvent) // There are two times (TimeStamp and TimeEvent) here, which one should be kept? both? { diff --git a/test/kukaiiwaTest.cpp b/test/kukaiiwaTest.cpp index 33f6913..5669a91 100644 --- a/test/kukaiiwaTest.cpp +++ b/test/kukaiiwaTest.cpp @@ -19,9 +19,6 @@ BOOST_AUTO_TEST_CASE(runRepeatedly) { bool debug = true; if (debug) std::cout << "starting KukaTest" << std::endl; - // grl::robot::arm::KukaJAVAdriver::Parms javaDriverP = grl::robot::arm::KukaJAVAdriver::defaultParams(); - // rl::robot::arm::KukaJAVAdriver javaDriver(javaDriverP); - // javaDriver.construct(); std::shared_ptr fbbP; fbbP = std::make_shared(); double duration = boost::chrono::high_resolution_clock::now().time_since_epoch().count(); @@ -39,34 +36,37 @@ BOOST_AUTO_TEST_CASE(runRepeatedly) "FRI" // KukaMonitorMode (options are FRI, JAVA) ); std::string RobotName("Robotiiwa" ); + std::string destination("where this message is going (URI)"); + std::string source("where this message came from (URI)"); std::string basename = RobotName; //std::get<0>(params); - + bool setArmConfiguration_ = true; // set the arm config first time + bool max_control_force_stop_ = false; + double nullspace_stiffness_ = 2.0; + double nullspace_damping_ = 0.5; flatbuffers::Offset controlState; grl::flatbuffer::ArmState armControlMode_(grl::flatbuffer::ArmState::StartArm); - + grl::flatbuffer::KUKAiiwaInterface commandInterface_ = grl::flatbuffer::KUKAiiwaInterface::SmartServo;// KUKAiiwaInterface::SmartServo; + grl::flatbuffer::KUKAiiwaInterface monitorInterface_ = grl::flatbuffer::KUKAiiwaInterface::FRI; + ::ControlMode controlMode_ = ::ControlMode::ControlMode_POSITION_CONTROLMODE; + ::ClientCommandMode clientCommandMode = ::ClientCommandMode::ClientCommandMode_POSITION; + ::OverlayType overlayType = ::OverlayType::OverlayType_NO_OVERLAY; + ::ConnectionInfo connectionInfo{::FRISessionState::FRISessionState_IDLE, + ::FRIConnectionQuality::FRIConnectionQuality_POOR, + true, (uint32_t)4, false, (uint32_t)0}; grl::robot::arm::KukaState armState; std::shared_ptr friData(std::make_shared(7)); cartographer::common::Time startTime; - double delta = -0.0005; - double delta_sum = 0; - /// consider moving joint angles based on time - int joint_to_move = 6; - std::vector ipoJointPos(7,0); - std::vector jointOffset(7,0); // length 7, value 0 - boost::container::static_vector jointStateToCommand(7,0); + std::vector joint_stiffness_ = {0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7}; + std::vector joint_damping_ = {1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0}; + std::vector joint_AccelerationRel_(7,0.5); + std::vector joint_VelocityRel_(7,1.0); + bool updateMinimumTrajectoryExecutionTime = false; + double minimumTrajectoryExecutionTime = 4; - // Absolute goal position to travel to in some modes of HowToMove - // Set all 7 joints to go to a position 1 radian from the center std::vector absoluteGoalPos(7,0.2); - std::unique_ptr lowLevelStepAlgorithmP; - // Need to tell the system how long in milliseconds it has to reach the goal - // or it will never move! std::size_t goal_position_command_time_duration = 4; - lowLevelStepAlgorithmP.reset(new grl::robot::arm::LinearInterpolation()); - // RobotModel (options are KUKA_LBR_IIWA_14_R820, KUKA_LBR_IIWA_7_R800) see grl::robot::arm::KukaState::KUKA_LBR_IIWA_14_R820 - std::shared_ptr> highLevelDriverClassP; std::shared_ptr kukaDriverP; @@ -77,46 +77,80 @@ BOOST_AUTO_TEST_CASE(runRepeatedly) int64_t sequenceNumber = 0; controlState = grl::toFlatBuffer(*fbbP, basename, sequenceNumber++, duration, armState, armControlMode_); - auto clientCommandMode = grl::flatbuffer::EClientCommandMode::POSITION; - auto overlayType = grl::flatbuffer::EOverlayType::NO_OVERLAY; - //TODO: Custom flatbuffer type. Load defaults from params/config //Cartesian Impedance Values - // grl::flatbuffer::EulerOrder eulerOrder = grl::flatbuffer::EulerOrder::xyz; - // grl::flatbuffer::Vector3d cart_stiffness_trans_ = grl::flatbuffer::Vector3d(500,500,500); - // grl::flatbuffer::EulerRotation cart_stifness_rot_ = grl::toFlatBuffer(cart_stiffness_trans_, eulerOrder); - - // grl::flatbuffer::Vector3d cart_damping_trans_ = grl::flatbuffer::Vector3d(0.3,0.3,0.3); - // grl::flatbuffer::EulerRotation cart_damping_rot_ = grl::toFlatBuffer(cart_damping_trans_, eulerOrder); - - // grl::flatbuffer::EulerPose cart_stiffness_ = grl::toFlatBuffer(cart_stiffness_trans_, cart_stifness_rot_); - // grl::flatbuffer::EulerPose cart_damping_ = grl::toFlatBuffer(cart_damping_trans_, cart_damping_rot_); - - // grl::flatbuffer::Vector3d cart_max_path_deviation_trans_ = grl::flatbuffer::Vector3d(1000,1000,1000); - // grl::flatbuffer::Vector3d cart_max_path_deviation_rot_ = grl::flatbuffer::Vector3d(5., 5., 5.); - // grl::flatbuffer::Vector3d cart_max_ctrl_force_trans_ = grl::flatbuffer::Vector3d(200,200,200); - // grl::flatbuffer::Vector3d cart_max_ctrl_force_rot_ = grl::flatbuffer::Vector3d(6.3,6.3,6.3); - // grl::flatbuffer::EulerPose cart_max_path_deviation_ = grl::flatbuffer::EulerPose(cart_max_path_deviation_trans_, grl::toFlatBuffer(cart_max_path_deviation_rot_, eulerOrder)); - // grl::flatbuffer::EulerPose cart_max_ctrl_vel_ = grl::toFlatBuffer(cart_max_path_deviation_trans_, grl::toFlatBuffer(cart_max_ctrl_force_rot_, eulerOrder)); - // grl::flatbuffer::EulerPose cart_max_ctrl_force_ = grl::toFlatBuffer(cart_max_ctrl_force_trans_, grl::toFlatBuffer(cart_max_ctrl_force_trans_, eulerOrder)); - // double nullspace_stiffness_ = 2.0; - // double nullspace_damping_ = 0.5; - - - - - //auto stiffnessPose = flatbuffer::CreateEulerPoseParams(*fbbP,&cart_stiffness_trans_,&cart_stiffness_rot_); - //auto dampingPose = flatbuffer::CreateEulerPoseParams(*fbbP,&cart_damping_trans_,&cart_damping_rot_) - // auto setCartesianImpedance = grl::toFlatBuffer(*fbbP,) - - // grl::flatbuffer::CreateCartesianImpedenceControlMode(*fbbP, &cart_stiffness_, &cart_damping_, - // nullspace_stiffness_, nullspace_damping_, &cart_max_path_deviation_, &cart_max_ctrl_vel_, &cart_max_ctrl_force_, max_control_force_stop_) - // auto jointStiffnessBuffer = fbbP->CreateVector(joint_stiffness_.data(),joint_stiffness_.size()); - // auto jointDampingBuffer = fbbP->CreateVector(joint_damping_.data(),joint_damping_.size()) - // auto setJointImpedance = grl::flatbuffer::CreateJointImpedenceControlMode(*fbbP, jointStiffnessBuffer, jointDampingBuffer) - // auto kukaiiwaArmConfiguration = flatbuffer::CreateKUKAiiwaArmConfiguration(*fbbP,name,commandInterface_,monitorInterface_, clientCommandMode, overlayType, - // controlMode_, setCartesianImpedance, setJointImpedance); - - + grl::flatbuffer::Vector3d cart_stiffness_trans_ = grl::flatbuffer::Vector3d(500,500,500); + grl::flatbuffer::EulerRotation cart_stifness_rot_ = grl::flatbuffer::EulerRotation(200,200,200,grl::flatbuffer::EulerOrder::xyz); + grl::flatbuffer::Vector3d cart_damping_trans_ = grl::flatbuffer::Vector3d(0.3,0.3,0.3); + grl::flatbuffer::EulerRotation cart_damping_rot_ = grl::flatbuffer::EulerRotation(0.3,0.3,0.3,grl::flatbuffer::EulerOrder::xyz); + grl::flatbuffer::EulerPose cart_stiffness_ = grl::flatbuffer::EulerPose(cart_stiffness_trans_, cart_stifness_rot_); + grl::flatbuffer::EulerPose cart_damping_ = grl::flatbuffer::EulerPose(cart_damping_trans_, cart_damping_rot_); + + grl::flatbuffer::EulerPose cart_max_path_deviation_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(1000,1000,100), grl::flatbuffer::EulerRotation(5.,5.,5., grl::flatbuffer::EulerOrder::xyz)); + grl::flatbuffer::EulerPose cart_max_ctrl_vel_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(1000,1000,1000), grl::flatbuffer::EulerRotation(6.3,6.3,6.3, grl::flatbuffer::EulerOrder::xyz)); + grl::flatbuffer::EulerPose cart_max_ctrl_force_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(200,200,200), grl::flatbuffer::EulerRotation(200.,200.,200., grl::flatbuffer::EulerOrder::xyz)); + + + + auto setCartesianImpedance = grl::toFlatBuffer(*fbbP, cart_stiffness_, cart_damping_, + nullspace_stiffness_, nullspace_damping_, cart_max_path_deviation_, cart_max_ctrl_vel_, cart_max_ctrl_force_, max_control_force_stop_); + auto setJointImpedance = grl::toFlatBuffer(*fbbP, joint_stiffness_, joint_damping_); + auto setSmartServo = grl::toFlatBuffer(*fbbP, joint_AccelerationRel_, joint_VelocityRel_, updateMinimumTrajectoryExecutionTime, minimumTrajectoryExecutionTime); + auto FRIConfig = grl::toFlatBuffer(*fbbP, overlayType, connectionInfo, false, 3501, false, 3502); + std::vector> tools; + std::vector> processData; + for(int i=0; i<7; i++) { + std::string linkname = "Link" + std::to_string(i); + std::string parent = i==0?"Base":("Link" + std::to_string(i-1)); + grl::flatbuffer::Pose pose = grl::flatbuffer::Pose(grl::flatbuffer::Vector3d(i,i,i), grl::flatbuffer::Quaternion(i,i,i,i)); + grl::flatbuffer::Inertia inertia = grl::flatbuffer::Inertia(1, pose, 1, 2, 3, 4, 5, 6); + flatbuffers::Offset linkObject = grl::toFlatBuffer(*fbbP, linkname, parent, pose, inertia); + tools.push_back(linkObject); + processData.push_back( + grl::toFlatBuffer(*fbbP, + "dataType"+ std::to_string(i), + "defaultValue"+ std::to_string(i), + "displayName"+ std::to_string(i), + "id"+ std::to_string(i), + "min"+ std::to_string(i), + "max"+ std::to_string(i), + "unit"+ std::to_string(i), + "value"+ std::to_string(i))); + + } + std::string currentMotionCenter = "currentMotionCenter"; + auto kukaiiwaArmConfiguration = grl::toFlatBuffer( + *fbbP, + RobotName, + commandInterface_, + monitorInterface_, + clientCommandMode, + overlayType, + controlMode_, + setCartesianImpedance, + setJointImpedance, + setSmartServo, + FRIConfig, + tools, + processData, + "currentMotionCenter", + true); + bool setArmControlState = true; // only actually change the arm state when this is true. + // ::MessageHeader messageHeader = ::MessageHeader(1,2,3); + // ::RobotInfo robotInfo = ::RobotInfo(true, 7, true, ::SafetyState::SafetyState_NORMAL_OPERATION) + // ::FRIMonitoringMessage friMonitoringMessage = ::FRIMonitoringMessage + + + // auto friMessageLog = grl::toFlatBuffer( + // *fbbP, + // connectionInfo.sessionState, + // connectionInfo.quality, + // controlMode_, + // ); + + // auto kukaiiwastate = flatbuffer::CreateKUKAiiwaState(*fbbP, RobotName, destination, source, duration, controlState, setArmConfiguration_,kukaiiwaArmConfiguration); + // auto kukaiiwaStateVec = fbbP->CreateVector(&kukaiiwastate, 1); + // auto states = flatbuffer::CreateKUKAiiwaStates(*fbbP,kukaiiwaStateVec); + // grl::flatbuffer::FinishKUKAiiwaStatesBuffer(*fbbP, states); } From 986d2573ea861ce2b1ba26e0ee292efc9f17cf6a Mon Sep 17 00:00:00 2001 From: Chunting Date: Thu, 30 Nov 2017 17:24:38 -0500 Subject: [PATCH 017/102] kukaiiwaTest.hpp Unit Test starts to work. --- include/grl/kuka/KukaToFlatbuffer.hpp | 4 - test/kukaiiwaTest.cpp | 104 +++++++++++++++++++++----- 2 files changed, 84 insertions(+), 24 deletions(-) diff --git a/include/grl/kuka/KukaToFlatbuffer.hpp b/include/grl/kuka/KukaToFlatbuffer.hpp index aea1801..9700a81 100644 --- a/include/grl/kuka/KukaToFlatbuffer.hpp +++ b/include/grl/kuka/KukaToFlatbuffer.hpp @@ -433,14 +433,11 @@ flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder &fbb, const ::OverlayType &overlayType, const ::ConnectionInfo &connectionInfo, - // const int32_t sendPeriodMillisec, - // const int32_t setReceiveMultiplier, const bool updatePortOnRemote, const int16_t portOnRemote, const bool updatePortOnController, const int16_t portOnController) { - // auto _overlayType = toFlatBuffer(overlayType); return grl::flatbuffer::CreateFRI( fbb, toFlatBuffer(overlayType), @@ -506,7 +503,6 @@ flatbuffers::Offset toFlatBuffer( const std::string &name, const grl::flatbuffer::KUKAiiwaInterface &commandInterface, // enum defined in KUKAiiwa.fbs const grl::flatbuffer::KUKAiiwaInterface &monitorInterface, - // const grl::flatbuffer::ClientCommandMode clientCommandMode, const ::ClientCommandMode &clientCommandMode, // enum const ::OverlayType &overlayType, // enum const ::ControlMode &controlMode, // enum diff --git a/test/kukaiiwaTest.cpp b/test/kukaiiwaTest.cpp index 5669a91..262dc4f 100644 --- a/test/kukaiiwaTest.cpp +++ b/test/kukaiiwaTest.cpp @@ -12,6 +12,26 @@ #include "grl/kuka/Kuka.hpp" #include "grl/kuka/KukaDriver.hpp" +/// Make reference to void map_repeatedDouble() in pb_frimessages_callback.c and pb_frimessages_callbacks.h +pb_callback_t makeupJointValues(std::vector &jointvalue) { + /// Make reference to void map_repeatedDouble() in pb_frimessages_callback.c and pb_frimessages_callbacks.h + pb_callback_t *values = new pb_callback_t; + int numDOF = 7; + values->funcs.encode = &encode_repeatedDouble; + values->funcs.decode = &decode_repeatedDouble; + tRepeatedDoubleArguments *arg = new tRepeatedDoubleArguments; + // map the local container data to the message data fields + arg->max_size = numDOF; + arg->size = 0; + arg->value = (double*) malloc(numDOF * sizeof(double)); + + for(int i=0; ivalue[i] = jointvalue[i]; + } + values->arg = arg; + return *values; + +} BOOST_AUTO_TEST_SUITE(KukaTest) @@ -43,16 +63,16 @@ BOOST_AUTO_TEST_CASE(runRepeatedly) bool max_control_force_stop_ = false; double nullspace_stiffness_ = 2.0; double nullspace_damping_ = 0.5; - flatbuffers::Offset controlState; + grl::flatbuffer::ArmState armControlMode_(grl::flatbuffer::ArmState::StartArm); grl::flatbuffer::KUKAiiwaInterface commandInterface_ = grl::flatbuffer::KUKAiiwaInterface::SmartServo;// KUKAiiwaInterface::SmartServo; grl::flatbuffer::KUKAiiwaInterface monitorInterface_ = grl::flatbuffer::KUKAiiwaInterface::FRI; - ::ControlMode controlMode_ = ::ControlMode::ControlMode_POSITION_CONTROLMODE; + ::ControlMode controlMode = ::ControlMode::ControlMode_POSITION_CONTROLMODE; ::ClientCommandMode clientCommandMode = ::ClientCommandMode::ClientCommandMode_POSITION; ::OverlayType overlayType = ::OverlayType::OverlayType_NO_OVERLAY; ::ConnectionInfo connectionInfo{::FRISessionState::FRISessionState_IDLE, ::FRIConnectionQuality::FRIConnectionQuality_POOR, - true, (uint32_t)4, false, (uint32_t)0}; + true, 4, false, 0}; grl::robot::arm::KukaState armState; std::shared_ptr friData(std::make_shared(7)); cartographer::common::Time startTime; @@ -75,7 +95,7 @@ BOOST_AUTO_TEST_CASE(runRepeatedly) kukaDriverP->set(goal_position_command_time_duration,grl::time_duration_command_tag()); std::cout << "KUKA COMMAND MODE: " << std::get(params) << "\n"; int64_t sequenceNumber = 0; - controlState = grl::toFlatBuffer(*fbbP, basename, sequenceNumber++, duration, armState, armControlMode_); + auto controlState = grl::toFlatBuffer(*fbbP, basename, sequenceNumber++, duration, armState, armControlMode_); //Cartesian Impedance Values grl::flatbuffer::Vector3d cart_stiffness_trans_ = grl::flatbuffer::Vector3d(500,500,500); @@ -125,7 +145,7 @@ BOOST_AUTO_TEST_CASE(runRepeatedly) monitorInterface_, clientCommandMode, overlayType, - controlMode_, + controlMode, setCartesianImpedance, setJointImpedance, setSmartServo, @@ -135,23 +155,67 @@ BOOST_AUTO_TEST_CASE(runRepeatedly) "currentMotionCenter", true); bool setArmControlState = true; // only actually change the arm state when this is true. - // ::MessageHeader messageHeader = ::MessageHeader(1,2,3); - // ::RobotInfo robotInfo = ::RobotInfo(true, 7, true, ::SafetyState::SafetyState_NORMAL_OPERATION) - // ::FRIMonitoringMessage friMonitoringMessage = ::FRIMonitoringMessage - + ::MessageHeader messageHeader{1,2,3}; + ::RobotInfo robotInfo{true, 7, true, ::SafetyState::SafetyState_NORMAL_OPERATION}; + ::JointValues strjointValues; + std::vector jointValues(7, 0.1); + + + + strjointValues.value = makeupJointValues(jointValues); + ::MessageMonitorData messageMonitorData{ + true, strjointValues, + true, strjointValues, + true, strjointValues, + true, strjointValues, + true, strjointValues, + true, ::TimeStamp{1,2} + }; + ::MessageIpoData ipoData{ + true, strjointValues, + true, clientCommandMode, + true, overlayType, + true, 4.0 + }; + ::Transformation transformation{ "Transformation", 12, {1,2,3,4,5,6,7,8,9,10,11,12}, true, ::TimeStamp{3,4}}; + /// Transformation requestedTransformations[5] = { transformation, transformation, transformation, transformation, transformation }; + ::MessageEndOf endOfMessageData{true, 32, true, ::Checksum{true, 32}}; + ::FRIMonitoringMessage friMonitoringMessage {messageHeader, + true, robotInfo, + true, messageMonitorData, + true, connectionInfo, + true, ipoData, + 5, + { transformation, transformation, transformation, transformation, transformation }, + true, endOfMessageData}; + grl::TimeEvent time_event_stamp; + auto friMessageLog = grl::toFlatBuffer( + *fbbP, + ::FRISessionState::FRISessionState_IDLE, + ::FRIConnectionQuality::FRIConnectionQuality_POOR, + controlMode, + friMonitoringMessage, + time_event_stamp); + auto kukaiiwaState = grl::toFlatBuffer( + *fbbP, + RobotName, + destination, + source, + duration, + true, controlState, + true, kukaiiwaArmConfiguration, + false, 0, + false, 0, + friMessageLog); + std::vector> kukaiiwaStateVec; + kukaiiwaStateVec.push_back(kukaiiwaState); + auto states = grl::toFlatBuffer(*fbbP, kukaiiwaStateVec); + grl::flatbuffer::FinishKUKAiiwaStatesBuffer(*fbbP, states); + flatbuffers::Verifier verifier(fbbP->GetBufferPointer(),fbbP->GetSize()); + std::cout<< "VerifyKUKAiiwaStatesBuffer: " << grl::flatbuffer::VerifyKUKAiiwaStatesBuffer(verifier) << std::endl; - // auto friMessageLog = grl::toFlatBuffer( - // *fbbP, - // connectionInfo.sessionState, - // connectionInfo.quality, - // controlMode_, - // ); +} - // auto kukaiiwastate = flatbuffer::CreateKUKAiiwaState(*fbbP, RobotName, destination, source, duration, controlState, setArmConfiguration_,kukaiiwaArmConfiguration); - // auto kukaiiwaStateVec = fbbP->CreateVector(&kukaiiwastate, 1); - // auto states = flatbuffer::CreateKUKAiiwaStates(*fbbP,kukaiiwaStateVec); - // grl::flatbuffer::FinishKUKAiiwaStatesBuffer(*fbbP, states); -} BOOST_AUTO_TEST_SUITE_END() From e18b1e558e1ccdf20f37b13e1398ae1198f22534 Mon Sep 17 00:00:00 2001 From: Chunting Date: Fri, 1 Dec 2017 20:10:37 -0500 Subject: [PATCH 018/102] Generated Binary file successfully, but json failed. --- test/CMakeLists.txt | 7 +- test/kukaiiwaTest.cpp | 227 +++++++++++++++++++++++++++--------------- 2 files changed, 148 insertions(+), 86 deletions(-) diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index d6e9655..d71a5d7 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -78,10 +78,11 @@ endif () # For KUKA IIWA FRI Libraries if(TARGET KukaFRIClient OR FRI_Client_SDK_Cpp_FOUND) - basis_include_directories(${FRI_Client_SDK_Cpp_PROJECT_INCLUDE_DIRS} ${Boost_REGEX_LIBRARY} ${FRI_Client_SDK_Cpp_INCLUDE_DIRS} ${PROJECT_INCLUDE_DIR}/thirdparty/vrep/include) + basis_include_directories(${FRI_Client_SDK_Cpp_PROJECT_INCLUDE_DIRS} ${Boost_REGEX_LIBRARY} ${FRI_Client_SDK_Cpp_INCLUDE_DIRS} ${PROJECT_INCLUDE_DIR}/thirdparty/vrep/include + ${FLATBUFFERS_INCLUDE_DIRS} ${FUSIONTRACK_INCLUDE_DIRS}) basis_add_executable(KukaFRITest.cpp)# ${GRL_FLATBUFFERS_OUTPUTS}) - basis_target_link_libraries(KukaFRITest ${Boost_LIBRARIES} ${Boost_REGEX_LIBRARY} ${FRI_Client_SDK_Cpp_LIBRARIES} ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} KukaFRIClient) + basis_target_link_libraries(KukaFRITest ${Boost_LIBRARIES} ${Boost_REGEX_LIBRARY} ${FRI_Client_SDK_Cpp_LIBRARIES} ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} KukaFRIClient ) if(UNIX AND NOT APPLE) set(LINUX_ONLY_LIBS ${LIBDL_LIBRARIES}) @@ -92,7 +93,7 @@ if(TARGET KukaFRIClient OR FRI_Client_SDK_Cpp_FOUND) v_repLib KukaFRIClient ${LINUX_ONLY_LIBS} ) basis_add_executable(kukaiiwaTest.cpp)# ${GRL_FLATBUFFERS_OUTPUTS}) - basis_target_link_libraries(kukaiiwaTest ${Boost_LIBRARIES} ${Boost_REGEX_LIBRARY} ${FRI_Client_SDK_Cpp_LIBRARIES} ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} KukaFRIClient) + basis_target_link_libraries(kukaiiwaTest ${Boost_LIBRARIES} ${Boost_REGEX_LIBRARY} ${FRI_Client_SDK_Cpp_LIBRARIES} ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} KukaFRIClient ${FLATBUFFERS_STATIC_LIB}) endif() diff --git a/test/kukaiiwaTest.cpp b/test/kukaiiwaTest.cpp index 262dc4f..4a6801f 100644 --- a/test/kukaiiwaTest.cpp +++ b/test/kukaiiwaTest.cpp @@ -7,8 +7,10 @@ #include #include +#include "grl/flatbuffer/flatbuffer.hpp" #include "grl/kuka/KukaToFlatbuffer.hpp" #include "grl/kuka/KukaJAVAdriver.hpp" + #include "grl/kuka/Kuka.hpp" #include "grl/kuka/KukaDriver.hpp" @@ -30,18 +32,32 @@ pb_callback_t makeupJointValues(std::vector &jointvalue) { } values->arg = arg; return *values; +} + +volatile std::sig_atomic_t signalStatusG = 0; +// called when the user presses ctrl+c +void signal_handler(int signal) +{ + signalStatusG = signal; } BOOST_AUTO_TEST_SUITE(KukaTest) BOOST_AUTO_TEST_CASE(runRepeatedly) { + const std::size_t MegaByte = 1024*1024; + // If we write too large a flatbuffer + const std::size_t single_buffer_limit_bytes = 2047*MegaByte; + // Install a signal handler to catch a signal when CONTROL+C + std::signal(SIGINT, signal_handler); + bool OK = false; bool debug = true; if (debug) std::cout << "starting KukaTest" << std::endl; std::shared_ptr fbbP; fbbP = std::make_shared(); double duration = boost::chrono::high_resolution_clock::now().time_since_epoch().count(); + if (debug) std::cout << "duration: " << duration << std::endl; grl::robot::arm::KukaDriver::Params params = std::make_tuple( "Robotiiwa" , // RobotName, "KUKA_LBR_IIWA_14_R820" , // RobotModel (options are KUKA_LBR_IIWA_14_R820, KUKA_LBR_IIWA_7_R800) @@ -61,12 +77,10 @@ BOOST_AUTO_TEST_CASE(runRepeatedly) std::string basename = RobotName; //std::get<0>(params); bool setArmConfiguration_ = true; // set the arm config first time bool max_control_force_stop_ = false; - double nullspace_stiffness_ = 2.0; - double nullspace_damping_ = 0.5; - grl::flatbuffer::ArmState armControlMode_(grl::flatbuffer::ArmState::StartArm); - grl::flatbuffer::KUKAiiwaInterface commandInterface_ = grl::flatbuffer::KUKAiiwaInterface::SmartServo;// KUKAiiwaInterface::SmartServo; - grl::flatbuffer::KUKAiiwaInterface monitorInterface_ = grl::flatbuffer::KUKAiiwaInterface::FRI; + grl::flatbuffer::ArmState armControlMode(grl::flatbuffer::ArmState::StartArm); + grl::flatbuffer::KUKAiiwaInterface commandInterface = grl::flatbuffer::KUKAiiwaInterface::SmartServo;// KUKAiiwaInterface::SmartServo; + grl::flatbuffer::KUKAiiwaInterface monitorInterface = grl::flatbuffer::KUKAiiwaInterface::FRI; ::ControlMode controlMode = ::ControlMode::ControlMode_POSITION_CONTROLMODE; ::ClientCommandMode clientCommandMode = ::ClientCommandMode::ClientCommandMode_POSITION; ::OverlayType overlayType = ::OverlayType::OverlayType_NO_OVERLAY; @@ -84,18 +98,11 @@ BOOST_AUTO_TEST_CASE(runRepeatedly) bool updateMinimumTrajectoryExecutionTime = false; double minimumTrajectoryExecutionTime = 4; - std::vector absoluteGoalPos(7,0.2); - std::size_t goal_position_command_time_duration = 4; std::shared_ptr> highLevelDriverClassP; - std::shared_ptr kukaDriverP; - kukaDriverP=std::make_shared(params); - kukaDriverP->set(grl::flatbuffer::ArmState::MoveArmJointServo); - kukaDriverP->set(goal_position_command_time_duration,grl::time_duration_command_tag()); std::cout << "KUKA COMMAND MODE: " << std::get(params) << "\n"; int64_t sequenceNumber = 0; - auto controlState = grl::toFlatBuffer(*fbbP, basename, sequenceNumber++, duration, armState, armControlMode_); //Cartesian Impedance Values grl::flatbuffer::Vector3d cart_stiffness_trans_ = grl::flatbuffer::Vector3d(500,500,500); @@ -108,60 +115,13 @@ BOOST_AUTO_TEST_CASE(runRepeatedly) grl::flatbuffer::EulerPose cart_max_path_deviation_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(1000,1000,100), grl::flatbuffer::EulerRotation(5.,5.,5., grl::flatbuffer::EulerOrder::xyz)); grl::flatbuffer::EulerPose cart_max_ctrl_vel_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(1000,1000,1000), grl::flatbuffer::EulerRotation(6.3,6.3,6.3, grl::flatbuffer::EulerOrder::xyz)); grl::flatbuffer::EulerPose cart_max_ctrl_force_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(200,200,200), grl::flatbuffer::EulerRotation(200.,200.,200., grl::flatbuffer::EulerOrder::xyz)); + double nullspace_stiffness_ = 0.1; + double nullspace_damping_ = 0.1; - - - auto setCartesianImpedance = grl::toFlatBuffer(*fbbP, cart_stiffness_, cart_damping_, - nullspace_stiffness_, nullspace_damping_, cart_max_path_deviation_, cart_max_ctrl_vel_, cart_max_ctrl_force_, max_control_force_stop_); - auto setJointImpedance = grl::toFlatBuffer(*fbbP, joint_stiffness_, joint_damping_); - auto setSmartServo = grl::toFlatBuffer(*fbbP, joint_AccelerationRel_, joint_VelocityRel_, updateMinimumTrajectoryExecutionTime, minimumTrajectoryExecutionTime); - auto FRIConfig = grl::toFlatBuffer(*fbbP, overlayType, connectionInfo, false, 3501, false, 3502); - std::vector> tools; - std::vector> processData; - for(int i=0; i<7; i++) { - std::string linkname = "Link" + std::to_string(i); - std::string parent = i==0?"Base":("Link" + std::to_string(i-1)); - grl::flatbuffer::Pose pose = grl::flatbuffer::Pose(grl::flatbuffer::Vector3d(i,i,i), grl::flatbuffer::Quaternion(i,i,i,i)); - grl::flatbuffer::Inertia inertia = grl::flatbuffer::Inertia(1, pose, 1, 2, 3, 4, 5, 6); - flatbuffers::Offset linkObject = grl::toFlatBuffer(*fbbP, linkname, parent, pose, inertia); - tools.push_back(linkObject); - processData.push_back( - grl::toFlatBuffer(*fbbP, - "dataType"+ std::to_string(i), - "defaultValue"+ std::to_string(i), - "displayName"+ std::to_string(i), - "id"+ std::to_string(i), - "min"+ std::to_string(i), - "max"+ std::to_string(i), - "unit"+ std::to_string(i), - "value"+ std::to_string(i))); - - } - std::string currentMotionCenter = "currentMotionCenter"; - auto kukaiiwaArmConfiguration = grl::toFlatBuffer( - *fbbP, - RobotName, - commandInterface_, - monitorInterface_, - clientCommandMode, - overlayType, - controlMode, - setCartesianImpedance, - setJointImpedance, - setSmartServo, - FRIConfig, - tools, - processData, - "currentMotionCenter", - true); - bool setArmControlState = true; // only actually change the arm state when this is true. ::MessageHeader messageHeader{1,2,3}; ::RobotInfo robotInfo{true, 7, true, ::SafetyState::SafetyState_NORMAL_OPERATION}; ::JointValues strjointValues; std::vector jointValues(7, 0.1); - - - strjointValues.value = makeupJointValues(jointValues); ::MessageMonitorData messageMonitorData{ true, strjointValues, @@ -189,30 +149,131 @@ BOOST_AUTO_TEST_CASE(runRepeatedly) { transformation, transformation, transformation, transformation, transformation }, true, endOfMessageData}; grl::TimeEvent time_event_stamp; - auto friMessageLog = grl::toFlatBuffer( - *fbbP, - ::FRISessionState::FRISessionState_IDLE, - ::FRIConnectionQuality::FRIConnectionQuality_POOR, - controlMode, - friMonitoringMessage, - time_event_stamp); - auto kukaiiwaState = grl::toFlatBuffer( - *fbbP, - RobotName, - destination, - source, - duration, - true, controlState, - true, kukaiiwaArmConfiguration, - false, 0, - false, 0, - friMessageLog); std::vector> kukaiiwaStateVec; - kukaiiwaStateVec.push_back(kukaiiwaState); - auto states = grl::toFlatBuffer(*fbbP, kukaiiwaStateVec); - grl::flatbuffer::FinishKUKAiiwaStatesBuffer(*fbbP, states); - flatbuffers::Verifier verifier(fbbP->GetBufferPointer(),fbbP->GetSize()); + std::size_t builder_size_bytes = 0; + std::size_t previous_size = 0; + + + // while(!signalStatusG && OK && builder_size_bytes < single_buffer_limit_bytes ) + // { + auto controlState = grl::toFlatBuffer(*fbbP, basename, sequenceNumber++, duration, armState, armControlMode); + + auto setCartesianImpedance = grl::toFlatBuffer(*fbbP, cart_stiffness_, cart_damping_, + nullspace_stiffness_, nullspace_damping_, cart_max_path_deviation_, cart_max_ctrl_vel_, cart_max_ctrl_force_, max_control_force_stop_); + auto setJointImpedance = grl::toFlatBuffer(*fbbP, joint_stiffness_, joint_damping_); + auto setSmartServo = grl::toFlatBuffer(*fbbP, joint_AccelerationRel_, joint_VelocityRel_, updateMinimumTrajectoryExecutionTime, minimumTrajectoryExecutionTime); + auto FRIConfig = grl::toFlatBuffer(*fbbP, overlayType, connectionInfo, false, 3501, false, 3502); + std::vector> tools; + std::vector> processData; + for(int i=0; i<7; i++) { + std::string linkname = "Link" + std::to_string(i); + std::string parent = i==0?"Base":("Link" + std::to_string(i-1)); + grl::flatbuffer::Pose pose = grl::flatbuffer::Pose(grl::flatbuffer::Vector3d(i,i,i), grl::flatbuffer::Quaternion(i,i,i,i)); + grl::flatbuffer::Inertia inertia = grl::flatbuffer::Inertia(1, pose, 1, 2, 3, 4, 5, 6); + flatbuffers::Offset linkObject = grl::toFlatBuffer(*fbbP, linkname, parent, pose, inertia); + tools.push_back(linkObject); + processData.push_back( + grl::toFlatBuffer(*fbbP, + "dataType"+ std::to_string(i), + "defaultValue"+ std::to_string(i), + "displayName"+ std::to_string(i), + "id"+ std::to_string(i), + "min"+ std::to_string(i), + "max"+ std::to_string(i), + "unit"+ std::to_string(i), + "value"+ std::to_string(i))); + + } + + auto kukaiiwaArmConfiguration = grl::toFlatBuffer( + *fbbP, + RobotName, + commandInterface, + monitorInterface, + clientCommandMode, + overlayType, + controlMode, + setCartesianImpedance, + setJointImpedance, + setSmartServo, + FRIConfig, + tools, + processData, + "currentMotionCenter", + true); + + auto friMessageLog = grl::toFlatBuffer( + *fbbP, + ::FRISessionState::FRISessionState_IDLE, + ::FRIConnectionQuality::FRIConnectionQuality_POOR, + controlMode, + friMonitoringMessage, + time_event_stamp); + + auto kukaiiwaState = grl::toFlatBuffer( + *fbbP, + RobotName, + destination, + source, + duration, + true, controlState, + true, kukaiiwaArmConfiguration, + false, 0, + false, 0, + friMessageLog); + + kukaiiwaStateVec.push_back(kukaiiwaState); + + builder_size_bytes = fbbP->GetSize(); + std::size_t newData = builder_size_bytes - previous_size; + previous_size = builder_size_bytes; + std::cout<< "single message data size (bytes): " << newData << " Buffer size: " << builder_size_bytes/MegaByte <<" MB" << std::endl; + // } + + std::cout<< "kukaiiwaStateVec:" << kukaiiwaStateVec.size() << std::endl; + + flatbuffers::Offset kukaStates = grl::toFlatBuffer(*fbbP, kukaiiwaStateVec); + + + grl::flatbuffer::FinishKUKAiiwaStatesBuffer(*fbbP, kukaStates); + + uint8_t *buf = fbbP->GetBufferPointer(); + std::size_t bufsize = fbbP->GetSize(); + flatbuffers::Verifier verifier(buf,fbbP->GetSize()); std::cout<< "VerifyKUKAiiwaStatesBuffer: " << grl::flatbuffer::VerifyKUKAiiwaStatesBuffer(verifier) << std::endl; + std::string binary_file_path = "Kuka_test_binary.iiwa"; + std::string json_file_path = "kuka_test_text.json"; + + + OK = flatbuffers::SaveFile(binary_file_path.c_str(), reinterpret_cast(buf), bufsize, true); + + // Get the current working directory + std::string fbs_filename("KUKAiiwa.fbs"); + flatbuffers::Parser parser; + std::vector includePaths; + + OK = OK && grl::ParseSchemaFile(parser, fbs_filename, includePaths, false); + std::string jsongen; + // now generate text from the flatbuffer binary + + OK = OK && GenerateText(parser, buf, &jsongen); + // Write the data get from flatbuffer binary to json file on disk. + std::cout << "buffer :" << (unsigned)strlen(reinterpret_cast(buf)) << std::endl; + std::ofstream out(json_file_path); + out << jsongen.c_str(); + out.close(); + + std::cout << jsongen.c_str() << std::endl; + // OK = OK && grl::SaveFlatBufferFile( + // buf, + // fbbP->GetSize(), + // binary_file_path, + // fbs_filename, + // json_file_path); + std::cout << "Save json file correctly? " << OK << " Buffer size saved to binary file: " << bufsize << std::endl; + std::cout << "End of the program" << std::endl; + + } From 94f4b46ea10e94d254e5ff29cee0371014ea62c3 Mon Sep 17 00:00:00 2001 From: Chunting Date: Wed, 6 Dec 2017 11:37:39 -0500 Subject: [PATCH 019/102] kukaiiwaTest.cpp can be compiled and generate the binary file and json file successfully --- example/kukaiiwaExample.cpp | 231 +++++++++++++++++++++++ include/grl/flatbuffer/KUKAiiwa.fbs | 7 +- include/grl/flatbuffer/flatbuffer.hpp | 64 ++++--- include/grl/kuka/KukaToFlatbuffer.hpp | 257 +++++++++++++++---------- test/kukaiiwaTest.cpp | 258 +++++++++++++++++++------- 5 files changed, 619 insertions(+), 198 deletions(-) create mode 100644 example/kukaiiwaExample.cpp diff --git a/example/kukaiiwaExample.cpp b/example/kukaiiwaExample.cpp new file mode 100644 index 0000000..291a7e5 --- /dev/null +++ b/example/kukaiiwaExample.cpp @@ -0,0 +1,231 @@ +// system includes +#include +#include +#include +#include + + + +#include "grl/kuka/KukaToFlatbuffer.hpp" +#include "grl/kuka/KukaJAVAdriver.hpp" +#include "grl/kuka/Kuka.hpp" +#include "grl/kuka/KukaDriver.hpp" + +#include "flatbuffers/util.h" + + +// #include "grl/flatbuffer/flatbuffer.hpp" +/// Make reference to void map_repeatedDouble() in pb_frimessages_callback.c and pb_frimessages_callbacks.h +pb_callback_t makeupJointValues(std::vector &jointvalue) { + /// Make reference to void map_repeatedDouble() in pb_frimessages_callback.c and pb_frimessages_callbacks.h + pb_callback_t *values = new pb_callback_t; + int numDOF = 7; + values->funcs.encode = &encode_repeatedDouble; + values->funcs.decode = &decode_repeatedDouble; + tRepeatedDoubleArguments *arg = new tRepeatedDoubleArguments; + // map the local container data to the message data fields + arg->max_size = numDOF; + arg->size = 0; + arg->value = (double*) malloc(numDOF * sizeof(double)); + + for(int i=0; ivalue[i] = jointvalue[i]; + } + values->arg = arg; + return *values; +} + +int main(int argc, char **argv) +{ + + bool debug = true; + if (debug) std::cout << "starting KukaTest" << std::endl; + std::shared_ptr fbbP; + fbbP = std::make_shared(); + double duration = boost::chrono::high_resolution_clock::now().time_since_epoch().count(); + grl::robot::arm::KukaDriver::Params params = std::make_tuple( + "Robotiiwa" , // RobotName, + "KUKA_LBR_IIWA_14_R820" , // RobotModel (options are KUKA_LBR_IIWA_14_R820, KUKA_LBR_IIWA_7_R800) + "0.0.0.0" , // LocalUDPAddress + "30010" , // LocalUDPPort + "172.31.1.147" , // RemoteUDPAddress + "192.170.10.100" , // LocalHostKukaKoniUDPAddress, + "30200" , // LocalHostKukaKoniUDPPort, + "192.170.10.2" , // RemoteHostKukaKoniUDPAddress, + "30200" , // RemoteHostKukaKoniUDPPort + "JAVA" , // KukaCommandMode (options are FRI, JAVA) + "FRI" // KukaMonitorMode (options are FRI, JAVA) + ); + std::string RobotName("Robotiiwa" ); + std::string destination("where this message is going (URI)"); + std::string source("where this message came from (URI)"); + std::string basename = RobotName; //std::get<0>(params); + bool setArmConfiguration_ = true; // set the arm config first time + bool max_control_force_stop_ = false; + double nullspace_stiffness_ = 2.0; + double nullspace_damping_ = 0.5; + + grl::flatbuffer::ArmState armControlMode_(grl::flatbuffer::ArmState::StartArm); + grl::flatbuffer::KUKAiiwaInterface commandInterface_ = grl::flatbuffer::KUKAiiwaInterface::SmartServo;// KUKAiiwaInterface::SmartServo; + grl::flatbuffer::KUKAiiwaInterface monitorInterface_ = grl::flatbuffer::KUKAiiwaInterface::FRI; + ::ControlMode controlMode = ::ControlMode::ControlMode_POSITION_CONTROLMODE; + ::ClientCommandMode clientCommandMode = ::ClientCommandMode::ClientCommandMode_POSITION; + ::OverlayType overlayType = ::OverlayType::OverlayType_NO_OVERLAY; + ::ConnectionInfo connectionInfo{::FRISessionState::FRISessionState_IDLE, + ::FRIConnectionQuality::FRIConnectionQuality_POOR, + true, 4, false, 0}; + grl::robot::arm::KukaState armState; + std::shared_ptr friData(std::make_shared(7)); + cartographer::common::Time startTime; + + std::vector joint_stiffness_ = {0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7}; + std::vector joint_damping_ = {1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0}; + std::vector joint_AccelerationRel_(7,0.5); + std::vector joint_VelocityRel_(7,1.0); + bool updateMinimumTrajectoryExecutionTime = false; + double minimumTrajectoryExecutionTime = 4; + + std::vector absoluteGoalPos(7,0.2); + + std::size_t goal_position_command_time_duration = 4; + std::shared_ptr> highLevelDriverClassP; + std::shared_ptr kukaDriverP; + + kukaDriverP=std::make_shared(params); + kukaDriverP->set(grl::flatbuffer::ArmState::MoveArmJointServo); + kukaDriverP->set(goal_position_command_time_duration,grl::time_duration_command_tag()); + std::cout << "KUKA COMMAND MODE: " << std::get(params) << "\n"; + int64_t sequenceNumber = 0; + auto controlState = grl::toFlatBuffer(*fbbP, basename, sequenceNumber++, duration, armState, armControlMode_); + + //Cartesian Impedance Values + grl::flatbuffer::Vector3d cart_stiffness_trans_ = grl::flatbuffer::Vector3d(500,500,500); + grl::flatbuffer::EulerRotation cart_stifness_rot_ = grl::flatbuffer::EulerRotation(200,200,200,grl::flatbuffer::EulerOrder::xyz); + grl::flatbuffer::Vector3d cart_damping_trans_ = grl::flatbuffer::Vector3d(0.3,0.3,0.3); + grl::flatbuffer::EulerRotation cart_damping_rot_ = grl::flatbuffer::EulerRotation(0.3,0.3,0.3,grl::flatbuffer::EulerOrder::xyz); + grl::flatbuffer::EulerPose cart_stiffness_ = grl::flatbuffer::EulerPose(cart_stiffness_trans_, cart_stifness_rot_); + grl::flatbuffer::EulerPose cart_damping_ = grl::flatbuffer::EulerPose(cart_damping_trans_, cart_damping_rot_); + + grl::flatbuffer::EulerPose cart_max_path_deviation_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(1000,1000,100), grl::flatbuffer::EulerRotation(5.,5.,5., grl::flatbuffer::EulerOrder::xyz)); + grl::flatbuffer::EulerPose cart_max_ctrl_vel_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(1000,1000,1000), grl::flatbuffer::EulerRotation(6.3,6.3,6.3, grl::flatbuffer::EulerOrder::xyz)); + grl::flatbuffer::EulerPose cart_max_ctrl_force_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(200,200,200), grl::flatbuffer::EulerRotation(200.,200.,200., grl::flatbuffer::EulerOrder::xyz)); + + + + auto setCartesianImpedance = grl::toFlatBuffer(*fbbP, cart_stiffness_, cart_damping_, + nullspace_stiffness_, nullspace_damping_, cart_max_path_deviation_, cart_max_ctrl_vel_, cart_max_ctrl_force_, max_control_force_stop_); + auto setJointImpedance = grl::toFlatBuffer(*fbbP, joint_stiffness_, joint_damping_); + auto setSmartServo = grl::toFlatBuffer(*fbbP, joint_AccelerationRel_, joint_VelocityRel_, updateMinimumTrajectoryExecutionTime, minimumTrajectoryExecutionTime); + auto FRIConfig = grl::toFlatBuffer(*fbbP, overlayType, connectionInfo, false, 3501, false, 3502); + std::vector> tools; + std::vector> processData; + for(int i=0; i<7; i++) { + std::string linkname = "Link" + std::to_string(i); + std::string parent = i==0?"Base":("Link" + std::to_string(i-1)); + grl::flatbuffer::Pose pose = grl::flatbuffer::Pose(grl::flatbuffer::Vector3d(i,i,i), grl::flatbuffer::Quaternion(i,i,i,i)); + grl::flatbuffer::Inertia inertia = grl::flatbuffer::Inertia(1, pose, 1, 2, 3, 4, 5, 6); + flatbuffers::Offset linkObject = grl::toFlatBuffer(*fbbP, linkname, parent, pose, inertia); + tools.push_back(linkObject); + processData.push_back( + grl::toFlatBuffer(*fbbP, + "dataType"+ std::to_string(i), + "defaultValue"+ std::to_string(i), + "displayName"+ std::to_string(i), + "id"+ std::to_string(i), + "min"+ std::to_string(i), + "max"+ std::to_string(i), + "unit"+ std::to_string(i), + "value"+ std::to_string(i))); + + } + std::string currentMotionCenter = "currentMotionCenter"; + auto kukaiiwaArmConfiguration = grl::toFlatBuffer( + *fbbP, + RobotName, + commandInterface_, + monitorInterface_, + clientCommandMode, + overlayType, + controlMode, + setCartesianImpedance, + setJointImpedance, + setSmartServo, + FRIConfig, + tools, + processData, + "currentMotionCenter", + true); + bool setArmControlState = true; // only actually change the arm state when this is true. + ::MessageHeader messageHeader{1,2,3}; + ::RobotInfo robotInfo{true, 7, true, ::SafetyState::SafetyState_NORMAL_OPERATION}; + ::JointValues strjointValues; + std::vector jointValues(7, 0.1); + + + + strjointValues.value = makeupJointValues(jointValues); + ::MessageMonitorData messageMonitorData{ + true, strjointValues, + true, strjointValues, + true, strjointValues, + true, strjointValues, + true, strjointValues, + true, ::TimeStamp{1,2} + }; + ::MessageIpoData ipoData{ + true, strjointValues, + true, clientCommandMode, + true, overlayType, + true, 4.0 + }; + ::Transformation transformation{ "Transformation", 12, {1,2,3,4,5,6,7,8,9,10,11,12}, true, ::TimeStamp{3,4}}; + /// Transformation requestedTransformations[5] = { transformation, transformation, transformation, transformation, transformation }; + ::MessageEndOf endOfMessageData{true, 32, true, ::Checksum{true, 32}}; + ::FRIMonitoringMessage friMonitoringMessage {messageHeader, + true, robotInfo, + true, messageMonitorData, + true, connectionInfo, + true, ipoData, + 5, + { transformation, transformation, transformation, transformation, transformation }, + true, endOfMessageData}; + grl::TimeEvent time_event_stamp; + auto friMessageLog = grl::toFlatBuffer( + *fbbP, + ::FRISessionState::FRISessionState_IDLE, + ::FRIConnectionQuality::FRIConnectionQuality_POOR, + controlMode, + friMonitoringMessage, + time_event_stamp); + auto kukaiiwaState = grl::toFlatBuffer( + *fbbP, + RobotName, + destination, + source, + duration, + true, controlState, + true, kukaiiwaArmConfiguration, + false, 0, + false, 0, + friMessageLog); + std::vector> kukaiiwaStateVec; + kukaiiwaStateVec.push_back(kukaiiwaState); + auto states = grl::toFlatBuffer(*fbbP, kukaiiwaStateVec); + grl::flatbuffer::FinishKUKAiiwaStatesBuffer(*fbbP, states); + flatbuffers::Verifier verifier(fbbP->GetBufferPointer(),fbbP->GetSize()); + std::cout<< "VerifyKUKAiiwaStatesBuffer: " << grl::flatbuffer::VerifyKUKAiiwaStatesBuffer(verifier) << std::endl; + + + std::string binary_file_path = "Kuka_test_binary.flik"; + std::string json_file_path = "kuka_test_text.json"; + + std::string fbs_filename("KUKAiiwa.fbs"); + grl::SaveFlatBufferFile( + fbbP->GetBufferPointer(), + fbbP->GetSize(), + binary_file_path, + fbs_filename, + json_file_path); + std::cout << "End of the program" << std::endl; + return success; +} // End of main function diff --git a/include/grl/flatbuffer/KUKAiiwa.fbs b/include/grl/flatbuffer/KUKAiiwa.fbs index 87f2684..c6c1ce6 100644 --- a/include/grl/flatbuffer/KUKAiiwa.fbs +++ b/include/grl/flatbuffer/KUKAiiwa.fbs @@ -240,6 +240,7 @@ table KUKAiiwaMonitorConfiguration { } // Get state data for the arm (things that change often) +// This table isn't used any more, since the main information of this table is the same with table KUKAiiwaMonitorState { // this is the measured current state of the arm joints // as seen in monitorState and _FRIMonitoringMessage @@ -292,6 +293,7 @@ table FRITimeStamp { // a table that is used as a record of all communication over FRI // it is not for user configuration or anything else, it should // simply reflect a history of what was sent and what was received. +/* table FRIMessageLog { sessionState:ESessionState; @@ -315,7 +317,7 @@ table FRIMessageLog { timeStamp:TimeEvent; } - +*/ table KUKAiiwaState { name:string; // The name of the robot to update (identifier used when applicable, doesn't ever change or set the name) @@ -335,7 +337,7 @@ table KUKAiiwaState { hasMonitorConfig:bool = false; monitorConfig:KUKAiiwaMonitorConfiguration; - FRIMessage:FRIMessageLog; + // FRIMessage:FRIMessageLog; } @@ -344,6 +346,7 @@ table KUKAiiwaState { // This is used because they can also be // accumulated and saved to disk as a log // very easily! + table KUKAiiwaStates { states:[KUKAiiwaState]; } diff --git a/include/grl/flatbuffer/flatbuffer.hpp b/include/grl/flatbuffer/flatbuffer.hpp index 5787e42..8d11464 100644 --- a/include/grl/flatbuffer/flatbuffer.hpp +++ b/include/grl/flatbuffer/flatbuffer.hpp @@ -2,32 +2,35 @@ #define GRL_FLATBUFFER_HPP #include + #include -namespace grl { -// loads a json flatbuffer from a file -bool LoadJSONFlatbuffer(flatbuffers::Parser& parser, std::string schemaPath, std::string jsonPath, std::string include_directory, bool binary = false) -{ - // source: https://github.com/google/flatbuffers/blob/master/samples/sample_text.cpp - // load FlatBuffer schema (.fbs) and JSON from disk - std::string schemafile; - std::string jsonfile; - bool ok = flatbuffers::LoadFile(schemaPath.c_str(), binary, &schemafile) && - flatbuffers::LoadFile(jsonPath.c_str(), binary, &jsonfile); - if (!ok) { - printf("couldn't load files!\n"); - return nullptr; - } +namespace grl { - // parse schema first, so we can use it to parse the data after - const char *include_directories[] = { include_directory.c_str(), "flatbuffer", nullptr }; - ok = parser.Parse(schemafile.c_str(), include_directories) && - parser.Parse(jsonfile.c_str(), include_directories); - assert(ok); - return ok; -} +// loads a json flatbuffer from a file +// bool LoadJSONFlatbuffer(flatbuffers::Parser& parser, std::string schemaPath, std::string jsonPath, std::string include_directory, bool binary = false) +// { +// // source: https://github.com/google/flatbuffers/blob/master/samples/sample_text.cpp + +// // load FlatBuffer schema (.fbs) and JSON from disk +// std::string schemafile; +// std::string jsonfile; +// bool ok = flatbuffers::LoadFile(schemaPath.c_str(), binary, &schemafile) && +// flatbuffers::LoadFile(jsonPath.c_str(), binary, &jsonfile); +// if (!ok) { +// printf("couldn't load files!\n"); +// return nullptr; +// } + +// // parse schema first, so we can use it to parse the data after +// const char *include_directories[] = { include_directory.c_str(), "flatbuffer", nullptr }; +// ok = parser.Parse(schemafile.c_str(), include_directories) && +// parser.Parse(jsonfile.c_str(), include_directories); +// assert(ok); +// return ok; +// } /// @brief Loads a flatbuffers schema file from disk into a flatbuffers parser. /// @@ -54,12 +57,13 @@ bool ParseSchemaFile( if(includePaths.empty()) { - std::string current_working_dir; - char buff[2048]; - /// Get the current working directory - getcwd(buff, 2048); - current_working_dir = std::string(buff); - includePaths.push_back(current_working_dir); + std::string current_working_dir; + char buff[2048]; + /// Get the current working directory + getcwd(buff, 2048); + current_working_dir = std::string(buff); + std::cout << "The current working dir: " << current_working_dir << std::endl; + includePaths.push_back(current_working_dir); } std::string fbs_fullpath; @@ -69,7 +73,7 @@ bool ParseSchemaFile( } else { - // a full path wasn't passed, so check all the include paths + std::cout << "a full path wasn't passed, so check all the include paths" << std::endl; for(auto includePath : includePaths) { std::string fbs_trypath = flatbuffers::ConCatPathFileName(includePath, fbs_filename); @@ -158,11 +162,15 @@ bool SaveFlatBufferFile( ok = ok && ParseSchemaFile(parser, fbs_filename, includePaths, read_binary_schema); std::string jsongen; // now generate text from the flatbuffer binary + ok = ok && GenerateText(parser, buffer, &jsongen); // Write the data get from flatbuffer binary to json file on disk. + std::ofstream out(json_file_path); out << jsongen.c_str(); out.close(); + std::cout << "json string :" << jsongen.size() << std::endl; + std::cout << jsongen.c_str() << std::endl; } return ok; } diff --git a/include/grl/kuka/KukaToFlatbuffer.hpp b/include/grl/kuka/KukaToFlatbuffer.hpp index 9700a81..fcc13e0 100644 --- a/include/grl/kuka/KukaToFlatbuffer.hpp +++ b/include/grl/kuka/KukaToFlatbuffer.hpp @@ -6,7 +6,7 @@ #define FLATBUFFERS_DEBUG_VERIFICATION_FAILURE #include - +#include "flatbuffers/flatbuffers.h" #include "grl/flatbuffer/JointState_generated.h" #include "grl/flatbuffer/ArmControlState_generated.h" #include "grl/flatbuffer/KUKAiiwa_generated.h" @@ -533,11 +533,26 @@ flatbuffers::Offset toFlatBuffer( requestMonitorProcessData); } - +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const std::string &hardwareVersion, + const std::vector &torqueSensorLimits, + bool isReadyToMove, + bool isMastered, + const std::vector> &processData) + { + return grl::flatbuffer::CreateKUKAiiwaMonitorConfiguration( + fbb, + hardwareVersion.empty() ? fbb.CreateString(hardwareVersion) : 0, + torqueSensorLimits.empty() ? fbb.CreateVector(torqueSensorLimits) : 0, + isReadyToMove, + isMastered, + processData.empty() ? fbb.CreateVector>(processData) : 0); + } /// KUKAiiwa.fbs flatbuffers::Offset toFlatBuffer( - flatbuffers::FlatBufferBuilder fbb, + flatbuffers::FlatBufferBuilder &fbb, flatbuffers::Offset &measuredState, const grl::flatbuffer::Pose &cartesianFlangePose, flatbuffers::Offset &jointStateReal, @@ -562,100 +577,142 @@ flatbuffers::Offset toFlatBuffer( } /// KUKAiiwa.fbs -flatbuffers::Offset toFlatBuffer( - flatbuffers::FlatBufferBuilder &fbb, - const std::string &name, - const std::string &destination, - const std::string &source, - const double timestamp, - const bool setArmControlState, - flatbuffers::Offset &armControlState, - const bool setArmConfiguration, - flatbuffers::Offset &armConfiguration, - const bool hasMonitorState, // MessageMonitorData monitorData in FRIMessage.pb.h - flatbuffers::Offset &monitorState, - const bool hasMonitorConfig, - flatbuffers::Offset &monitorConfig) -{ - return grl::flatbuffer::CreateKUKAiiwaState( - fbb, - fbb.CreateString(name), - fbb.CreateString(destination), - fbb.CreateString(source), - timestamp, - setArmControlState, - armControlState, - setArmConfiguration, - armConfiguration, - hasMonitorState, - monitorState, - hasMonitorConfig, - monitorConfig); -} +// flatbuffers::Offset toFlatBuffer( +// flatbuffers::FlatBufferBuilder &fbb, +// const std::string &name, +// const std::string &destination, +// const std::string &source, +// const double timestamp, +// const bool setArmControlState, +// flatbuffers::Offset &armControlState, +// const bool setArmConfiguration, +// flatbuffers::Offset &armConfiguration, +// const bool hasMonitorState, // MessageMonitorData monitorData in FRIMessage.pb.h +// flatbuffers::Offset &monitorState, +// const bool hasMonitorConfig, +// flatbuffers::Offset &monitorConfig) +// { +// return grl::flatbuffer::CreateKUKAiiwaState( +// fbb, +// fbb.CreateString(name), +// fbb.CreateString(destination), +// fbb.CreateString(source), +// timestamp, +// setArmControlState, +// armControlState, +// setArmConfiguration, +// armConfiguration, +// hasMonitorState, +// monitorState, +// hasMonitorConfig, +// monitorConfig); +// } /// KUKAiiwa.fbs -flatbuffers::Offset toFlatBuffer( - flatbuffers::FlatBufferBuilder &fbb, - const ::FRISessionState &sessionState, - const ::FRIConnectionQuality &connectionQuality, - const ::ControlMode &controlMode, // enum - const ::FRIMonitoringMessage &friMonitoringMessage, - const grl::TimeEvent &timeEvent) // There are two times (TimeStamp and TimeEvent) here, which one should be kept? both? -{ - auto _sessionState = toFlatBuffer(sessionState); - auto _connectionQuality = toFlatBuffer(connectionQuality); - auto _controlMode = toFlatBuffer(controlMode); - auto _messageIdentifier = friMonitoringMessage.header.messageIdentifier; - auto _sequenceCounter = friMonitoringMessage.header.sequenceCounter; - auto _reflectedSequenceCounter = friMonitoringMessage.header.reflectedSequenceCounter; - // auto _sec = timeStamp.sec; - // auto _nanosec = timeStamp.nanosec; - std::vector data; - // get measured joint position - grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_angle_open_chain_state_tag()); - flatbuffers::Offset> _measuredJointPosition = fbb.CreateVector(data); - data.clear(); - // get measured joint torque - grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_torque_open_chain_state_tag()); - flatbuffers::Offset> _measuredTorque = fbb.CreateVector(data); - data.clear(); - // get measured joint torque - grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_torque_open_chain_command_tag()); - flatbuffers::Offset> _commandedJointPosition = fbb.CreateVector(data); - data.clear(); - // get commanded joint torque - grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_torque_open_chain_command_tag()); - flatbuffers::Offset> _commandedTorque = fbb.CreateVector(data); - data.clear(); - // get measured external torque - grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_torque_external_open_chain_state_tag()); - flatbuffers::Offset> _externalTorque = fbb.CreateVector(data); - data.clear(); - // get interpolated joint state - grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_angle_interpolated_open_chain_state_tag()); - flatbuffers::Offset> _jointStateInterpolated = fbb.CreateVector(data); - flatbuffers::Offset _timeEvent = grl::toFlatBuffer(fbb, timeEvent); - auto _overlayType = toFlatBuffer(friMonitoringMessage.ipoData.overlayType); - return grl::flatbuffer::CreateFRIMessageLog( - fbb, - _sessionState, - _connectionQuality, - _controlMode, - _messageIdentifier, - _sequenceCounter, - _reflectedSequenceCounter, - // _sec, - // _nanosec, - _measuredJointPosition, - _measuredTorque, - _commandedJointPosition, - _commandedTorque, - _externalTorque, - _jointStateInterpolated, - _timeEvent); -} +// flatbuffers::Offset toFlatBuffer( +// flatbuffers::FlatBufferBuilder &fbb, +// const ::FRISessionState &sessionState, +// const ::FRIConnectionQuality &connectionQuality, +// const ::ControlMode &controlMode, // enum +// const ::FRIMonitoringMessage &friMonitoringMessage, +// const grl::TimeEvent &timeEvent) // There are two times (TimeStamp and TimeEvent) here, which one should be kept? both? +// { +// auto _sessionState = toFlatBuffer(sessionState); +// auto _connectionQuality = toFlatBuffer(connectionQuality); +// auto _controlMode = toFlatBuffer(controlMode); +// auto _messageIdentifier = friMonitoringMessage.header.messageIdentifier; +// auto _sequenceCounter = friMonitoringMessage.header.sequenceCounter; +// auto _reflectedSequenceCounter = friMonitoringMessage.header.reflectedSequenceCounter; +// // auto _sec = timeStamp.sec; +// // auto _nanosec = timeStamp.nanosec; +// std::vector data; +// // get measured joint position +// grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_angle_open_chain_state_tag()); +// flatbuffers::Offset> _measuredJointPosition = fbb.CreateVector(data); + +// data.clear(); +// // get measured joint torque +// grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_torque_open_chain_state_tag()); +// flatbuffers::Offset> _measuredTorque = fbb.CreateVector(data); + +// data.clear(); +// // get measured joint torque +// grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_torque_open_chain_command_tag()); +// flatbuffers::Offset> _commandedJointPosition = fbb.CreateVector(data); + +// data.clear(); +// // get commanded joint torque +// grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_torque_open_chain_command_tag()); +// flatbuffers::Offset> _commandedTorque = fbb.CreateVector(data); + +// data.clear(); +// // get measured external torque +// grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_torque_external_open_chain_state_tag()); +// flatbuffers::Offset> _externalTorque = fbb.CreateVector(data); + +// data.clear(); +// // get interpolated joint state +// grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_angle_interpolated_open_chain_state_tag()); +// flatbuffers::Offset> _jointStateInterpolated = fbb.CreateVector(data); + +// flatbuffers::Offset _timeEvent = grl::toFlatBuffer(fbb, timeEvent); + +// auto _overlayType = toFlatBuffer(friMonitoringMessage.ipoData.overlayType); + +// //////////////////////////////////////////////////////////////////////////// +// /*** This block is to verify the specific flatbuffer object ***/ +// flatbuffers::FlatBufferBuilder _fbb; +// flatbuffers::Offset> test_measuredJointPosition = _fbb.CreateVector(data); +// flatbuffers::Offset> test_measuredTorque = _fbb.CreateVector(data); +// flatbuffers::Offset> test_commandedJointPosition = _fbb.CreateVector(data); +// flatbuffers::Offset> test_commandedTorque = _fbb.CreateVector(data); +// flatbuffers::Offset> test_externalTorque = _fbb.CreateVector(data); +// flatbuffers::Offset> test_jointStateInterpolated = _fbb.CreateVector(data); +// flatbuffers::Offset test_timeEvent = grl::toFlatBuffer(_fbb, timeEvent); +// auto _friMessageLog = grl::flatbuffer::CreateFRIMessageLog( +// _fbb, +// _sessionState, +// _connectionQuality, +// _controlMode, +// _messageIdentifier, +// _sequenceCounter, +// _reflectedSequenceCounter, +// // _sec, +// // _nanosec, +// test_measuredJointPosition, +// test_measuredTorque, +// test_commandedJointPosition, +// test_commandedTorque, +// test_externalTorque, +// test_jointStateInterpolated, +// test_timeEvent); +// _fbb.Finish(_friMessageLog); +// auto verifier = flatbuffers::Verifier(_fbb.GetBufferPointer(), _fbb.GetSize()); +// bool success = verifier.VerifyBuffer(); +// std::cout <<" verifier success for grl::flatbuffer::FRIMessageLog: " << success << std::endl; +// /////////////////////////////////////////////////////////////////////////// + + +// return grl::flatbuffer::CreateFRIMessageLog( +// fbb, +// _sessionState, +// _connectionQuality, +// _controlMode, +// _messageIdentifier, +// _sequenceCounter, +// _reflectedSequenceCounter, +// // _sec, +// // _nanosec, +// _measuredJointPosition, +// _measuredTorque, +// _commandedJointPosition, +// _commandedTorque, +// _externalTorque, +// _jointStateInterpolated, +// _timeEvent); +// } /// KUKAiiwa.fbs flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder &fbb, @@ -670,8 +727,9 @@ flatbuffers::Offset toFlatBuffer( const bool hasMonitorState, const flatbuffers::Offset &monitorState, const bool hasMonitorConfig, - const flatbuffers::Offset &monitorConfig, - const flatbuffers::Offset &FRIMessage) + const flatbuffers::Offset &monitorConfig) + //const flatbuffers::Offset &FRIMessage + { return grl::flatbuffer::CreateKUKAiiwaState( fbb, @@ -686,17 +744,20 @@ flatbuffers::Offset toFlatBuffer( hasMonitorState, monitorState, hasMonitorConfig, - monitorConfig, - FRIMessage); + monitorConfig + // FRIMessage + ); } /// KUKAiiwa.fbs flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder &fbb, const std::vector> &kukaiiwastates) { + auto states = fbb.CreateVector>(kukaiiwastates); + // std::cout<< "states (flatbuffer vector):" << states.size() << std::endl; return grl::flatbuffer::CreateKUKAiiwaStates( fbb, - kukaiiwastates.empty()?fbb.CreateVector>(kukaiiwastates):0); + states); } // } // End of arm namespace // } // End of robot namespace diff --git a/test/kukaiiwaTest.cpp b/test/kukaiiwaTest.cpp index 4a6801f..8fa8a47 100644 --- a/test/kukaiiwaTest.cpp +++ b/test/kukaiiwaTest.cpp @@ -15,11 +15,12 @@ #include "grl/kuka/KukaDriver.hpp" /// Make reference to void map_repeatedDouble() in pb_frimessages_callback.c and pb_frimessages_callbacks.h -pb_callback_t makeupJointValues(std::vector &jointvalue) { +std::shared_ptr makeupJointValues(std::vector &jointvalue) { /// Make reference to void map_repeatedDouble() in pb_frimessages_callback.c and pb_frimessages_callbacks.h - pb_callback_t *values = new pb_callback_t; + std::shared_ptr values(std::make_shared()); + // pb_callback_t *values = new pb_callback_t; int numDOF = 7; - values->funcs.encode = &encode_repeatedDouble; + values->funcs.encode = &encode_repeatedDouble; values->funcs.decode = &decode_repeatedDouble; tRepeatedDoubleArguments *arg = new tRepeatedDoubleArguments; // map the local container data to the message data fields @@ -31,7 +32,7 @@ pb_callback_t makeupJointValues(std::vector &jointvalue) { arg->value[i] = jointvalue[i]; } values->arg = arg; - return *values; + return values; } volatile std::sig_atomic_t signalStatusG = 0; @@ -42,21 +43,33 @@ void signal_handler(int signal) signalStatusG = signal; } +int charP_size(const char* buf) +{ + int Size = 0; + while (buf[Size] != '\0') Size++; + return Size; +} + BOOST_AUTO_TEST_SUITE(KukaTest) BOOST_AUTO_TEST_CASE(runRepeatedly) { const std::size_t MegaByte = 1024*1024; // If we write too large a flatbuffer - const std::size_t single_buffer_limit_bytes = 2047*MegaByte; + const std::size_t single_buffer_limit_bytes = 3*MegaByte; // Install a signal handler to catch a signal when CONTROL+C std::signal(SIGINT, signal_handler); - bool OK = false; + bool OK = true; bool debug = true; if (debug) std::cout << "starting KukaTest" << std::endl; - std::shared_ptr fbbP; - fbbP = std::make_shared(); + // std::shared_ptr fbbP; + flatbuffers::FlatBufferBuilder fbb; + // fbbP = std::make_shared(); + // std::shared_ptr friData(std::make_shared(7)); + // std::shared_ptr> highLevelDriverClassP; double duration = boost::chrono::high_resolution_clock::now().time_since_epoch().count(); + + if (debug) std::cout << "duration: " << duration << std::endl; grl::robot::arm::KukaDriver::Params params = std::make_tuple( "Robotiiwa" , // RobotName, @@ -78,7 +91,7 @@ BOOST_AUTO_TEST_CASE(runRepeatedly) bool setArmConfiguration_ = true; // set the arm config first time bool max_control_force_stop_ = false; - grl::flatbuffer::ArmState armControlMode(grl::flatbuffer::ArmState::StartArm); + grl::flatbuffer::ArmState armControlMode = grl::flatbuffer::ArmState::StartArm; grl::flatbuffer::KUKAiiwaInterface commandInterface = grl::flatbuffer::KUKAiiwaInterface::SmartServo;// KUKAiiwaInterface::SmartServo; grl::flatbuffer::KUKAiiwaInterface monitorInterface = grl::flatbuffer::KUKAiiwaInterface::FRI; ::ControlMode controlMode = ::ControlMode::ControlMode_POSITION_CONTROLMODE; @@ -87,23 +100,18 @@ BOOST_AUTO_TEST_CASE(runRepeatedly) ::ConnectionInfo connectionInfo{::FRISessionState::FRISessionState_IDLE, ::FRIConnectionQuality::FRIConnectionQuality_POOR, true, 4, false, 0}; - grl::robot::arm::KukaState armState; - std::shared_ptr friData(std::make_shared(7)); - cartographer::common::Time startTime; - std::vector joint_stiffness_ = {0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7}; - std::vector joint_damping_ = {1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0}; - std::vector joint_AccelerationRel_(7,0.5); - std::vector joint_VelocityRel_(7,1.0); + + std::vector joint_stiffness_(7, 0.2); + std::vector joint_damping_(7, 0.3); + std::vector joint_AccelerationRel_(7, 0.5); + std::vector joint_VelocityRel_(7, 1.0); bool updateMinimumTrajectoryExecutionTime = false; double minimumTrajectoryExecutionTime = 4; - std::size_t goal_position_command_time_duration = 4; - std::shared_ptr> highLevelDriverClassP; std::cout << "KUKA COMMAND MODE: " << std::get(params) << "\n"; int64_t sequenceNumber = 0; - //Cartesian Impedance Values grl::flatbuffer::Vector3d cart_stiffness_trans_ = grl::flatbuffer::Vector3d(500,500,500); grl::flatbuffer::EulerRotation cart_stifness_rot_ = grl::flatbuffer::EulerRotation(200,200,200,grl::flatbuffer::EulerOrder::xyz); @@ -122,7 +130,49 @@ BOOST_AUTO_TEST_CASE(runRepeatedly) ::RobotInfo robotInfo{true, 7, true, ::SafetyState::SafetyState_NORMAL_OPERATION}; ::JointValues strjointValues; std::vector jointValues(7, 0.1); - strjointValues.value = makeupJointValues(jointValues); + boost::container::static_vector jointstate; + for(int i = 0; i<7; i++) { + jointstate.push_back(jointValues[i]); + } + /// Make reference to void map_repeatedDouble() in pb_frimessages_callback.c and pb_frimessages_callbacks.h + std::shared_ptr values(std::make_shared()); + // pb_callback_t *values = new pb_callback_t; + int numDOF = 7; + values->funcs.encode = &encode_repeatedDouble; + values->funcs.decode = &decode_repeatedDouble; + tRepeatedDoubleArguments *arg = new tRepeatedDoubleArguments; + // map the local container data to the message data fields + arg->max_size = numDOF; + arg->size = 0; + arg->value = (double*) malloc(numDOF * sizeof(double)); + + for(int i=0; ivalue[i] = jointstate[i]; + } + values->arg = arg; + strjointValues.value = *values; + // strjointValues.value = *makeupJointValues(jointValues); + grl::robot::arm::KukaState::joint_state kukajointstate(jointstate); + + //////////////////////////////////////////////////////////// + // Double check the initialization of this parameter ////// + ////////////////////////////////////////////////////////// + grl::robot::arm::KukaState armState; + armState.position = kukajointstate; + armState.torque = kukajointstate; + armState.externalTorque = kukajointstate; + armState.commandedPosition = kukajointstate; + armState.commandedTorque = kukajointstate; + armState.ipoJointPosition = kukajointstate; + armState.ipoJointPositionOffsets = kukajointstate; + armState.commandedCartesianWrenchFeedForward = kukajointstate; + armState.externalForce = kukajointstate; + armState.sessionState = grl::toFlatBuffer(connectionInfo.sessionState); + armState.connectionQuality = grl::toFlatBuffer(connectionInfo.quality); + armState.safetyState = grl::toFlatBuffer(::SafetyState::SafetyState_NORMAL_OPERATION); + armState.operationMode = grl::toFlatBuffer(OperationMode::OperationMode_TEST_MODE_1); + armState.driveState = grl::toFlatBuffer(::DriveState::DriveState_OFF); + ::MessageMonitorData messageMonitorData{ true, strjointValues, true, strjointValues, @@ -140,40 +190,47 @@ BOOST_AUTO_TEST_CASE(runRepeatedly) ::Transformation transformation{ "Transformation", 12, {1,2,3,4,5,6,7,8,9,10,11,12}, true, ::TimeStamp{3,4}}; /// Transformation requestedTransformations[5] = { transformation, transformation, transformation, transformation, transformation }; ::MessageEndOf endOfMessageData{true, 32, true, ::Checksum{true, 32}}; - ::FRIMonitoringMessage friMonitoringMessage {messageHeader, - true, robotInfo, - true, messageMonitorData, - true, connectionInfo, - true, ipoData, - 5, - { transformation, transformation, transformation, transformation, transformation }, - true, endOfMessageData}; + // ::FRIMonitoringMessage friMonitoringMessage { + // messageHeader, + // true, robotInfo, + // true, messageMonitorData, + // true, connectionInfo, + // true, ipoData, + // 5, + // { transformation, transformation, transformation, transformation, transformation }, + // true, endOfMessageData}; grl::TimeEvent time_event_stamp; std::vector> kukaiiwaStateVec; std::size_t builder_size_bytes = 0; std::size_t previous_size = 0; - // while(!signalStatusG && OK && builder_size_bytes < single_buffer_limit_bytes ) - // { - auto controlState = grl::toFlatBuffer(*fbbP, basename, sequenceNumber++, duration, armState, armControlMode); - - auto setCartesianImpedance = grl::toFlatBuffer(*fbbP, cart_stiffness_, cart_damping_, + while(!signalStatusG && OK && builder_size_bytes < single_buffer_limit_bytes ) + { + std::cout <<"Step into loop..." << std::endl; + // grl::robot::arm::copy(friMonitoringMessage, armState); + flatbuffers::Offset controlState = grl::toFlatBuffer(fbb, basename, sequenceNumber++, duration, armState, armControlMode); + // if(controlState == 0) { + // assert(("controlState is empty?", controlState == 0)); + // } + flatbuffers::Offset setCartesianImpedance = grl::toFlatBuffer(fbb, cart_stiffness_, cart_damping_, nullspace_stiffness_, nullspace_damping_, cart_max_path_deviation_, cart_max_ctrl_vel_, cart_max_ctrl_force_, max_control_force_stop_); - auto setJointImpedance = grl::toFlatBuffer(*fbbP, joint_stiffness_, joint_damping_); - auto setSmartServo = grl::toFlatBuffer(*fbbP, joint_AccelerationRel_, joint_VelocityRel_, updateMinimumTrajectoryExecutionTime, minimumTrajectoryExecutionTime); - auto FRIConfig = grl::toFlatBuffer(*fbbP, overlayType, connectionInfo, false, 3501, false, 3502); + flatbuffers::Offset setJointImpedance = grl::toFlatBuffer(fbb, joint_stiffness_, joint_damping_); + flatbuffers::Offset setSmartServo = grl::toFlatBuffer(fbb, joint_AccelerationRel_, joint_VelocityRel_, updateMinimumTrajectoryExecutionTime, minimumTrajectoryExecutionTime); + flatbuffers::Offset FRIConfig = grl::toFlatBuffer(fbb, overlayType, connectionInfo, false, 3501, false, 3502); + std::vector> tools; std::vector> processData; + for(int i=0; i<7; i++) { std::string linkname = "Link" + std::to_string(i); std::string parent = i==0?"Base":("Link" + std::to_string(i-1)); grl::flatbuffer::Pose pose = grl::flatbuffer::Pose(grl::flatbuffer::Vector3d(i,i,i), grl::flatbuffer::Quaternion(i,i,i,i)); grl::flatbuffer::Inertia inertia = grl::flatbuffer::Inertia(1, pose, 1, 2, 3, 4, 5, 6); - flatbuffers::Offset linkObject = grl::toFlatBuffer(*fbbP, linkname, parent, pose, inertia); + flatbuffers::Offset linkObject = grl::toFlatBuffer(fbb, linkname, parent, pose, inertia); tools.push_back(linkObject); processData.push_back( - grl::toFlatBuffer(*fbbP, + grl::toFlatBuffer(fbb, "dataType"+ std::to_string(i), "defaultValue"+ std::to_string(i), "displayName"+ std::to_string(i), @@ -182,11 +239,14 @@ BOOST_AUTO_TEST_CASE(runRepeatedly) "max"+ std::to_string(i), "unit"+ std::to_string(i), "value"+ std::to_string(i))); - } - auto kukaiiwaArmConfiguration = grl::toFlatBuffer( - *fbbP, + + // copy the state data into a more accessible object + /// TODO(ahundt) switch from this copy to a non-deprecated call + + flatbuffers::Offset kukaiiwaArmConfiguration = grl::toFlatBuffer( + fbb, RobotName, commandInterface, monitorInterface, @@ -202,45 +262,78 @@ BOOST_AUTO_TEST_CASE(runRepeatedly) "currentMotionCenter", true); - auto friMessageLog = grl::toFlatBuffer( - *fbbP, + // flatbuffers::Offset friMessageLog = grl::toFlatBuffer( + // fbb, + // ::FRISessionState::FRISessionState_IDLE, + // ::FRIConnectionQuality::FRIConnectionQuality_POOR, + // controlMode, + // friMonitoringMessage, + // time_event_stamp); + grl::flatbuffer::Wrench cartesianWrench{grl::flatbuffer::Vector3d(1,1,1),grl::flatbuffer::Vector3d(1,1,1),grl::flatbuffer::Vector3d(1,1,1)}; + + + flatbuffers::Offset jointStatetab = grl::toFlatBuffer(fbb, jointValues, jointValues, jointValues, jointValues); + grl::flatbuffer::Pose cartesianFlangePose = grl::flatbuffer::Pose(grl::flatbuffer::Vector3d(1,1,1), grl::flatbuffer::Quaternion(2,3,4,5)); + flatbuffers::Offset kukaiiwaMonitorState = grl::toFlatBuffer( + fbb, + jointStatetab, // flatbuffers::Offset &measuredState, + cartesianFlangePose, + jointStatetab, // flatbuffers::Offset &jointStateReal, + jointStatetab, // flatbuffers::Offset &jointStateInterpolated, + jointStatetab, // flatbuffers::Offset &externalState, ::FRISessionState::FRISessionState_IDLE, - ::FRIConnectionQuality::FRIConnectionQuality_POOR, - controlMode, - friMonitoringMessage, - time_event_stamp); - - auto kukaiiwaState = grl::toFlatBuffer( - *fbbP, + ::OperationMode::OperationMode_TEST_MODE_1, + cartesianWrench); + std::vector torqueSensorLimits(7,0.5); + flatbuffers::Offset monitorConfig = grl::toFlatBuffer( + fbb, + "hardvareVersion", + torqueSensorLimits, + true, + false, + processData); + flatbuffers::Offset kukaiiwaState = grl::toFlatBuffer( + fbb, RobotName, destination, source, duration, true, controlState, true, kukaiiwaArmConfiguration, - false, 0, - false, 0, - friMessageLog); + true, kukaiiwaMonitorState, + false, monitorConfig + // , friMessageLog + ); kukaiiwaStateVec.push_back(kukaiiwaState); - builder_size_bytes = fbbP->GetSize(); + builder_size_bytes = fbb.GetSize(); std::size_t newData = builder_size_bytes - previous_size; previous_size = builder_size_bytes; std::cout<< "single message data size (bytes): " << newData << " Buffer size: " << builder_size_bytes/MegaByte <<" MB" << std::endl; - // } + } std::cout<< "kukaiiwaStateVec:" << kukaiiwaStateVec.size() << std::endl; - flatbuffers::Offset kukaStates = grl::toFlatBuffer(*fbbP, kukaiiwaStateVec); + flatbuffers::Offset kukaStates = grl::toFlatBuffer(fbb, kukaiiwaStateVec); + + + grl::flatbuffer::FinishKUKAiiwaStatesBuffer(fbb, kukaStates); + uint8_t *buf = fbb.GetBufferPointer(); + std::size_t bufsize = fbb.GetSize(); - grl::flatbuffer::FinishKUKAiiwaStatesBuffer(*fbbP, kukaStates); + auto KUKAiiwaStatesP= grl::flatbuffer::GetKUKAiiwaStates(buf); + auto states = KUKAiiwaStatesP->states(); + assert(states); + // for (flatbuffers::uoffset_t i = 0; i < bufsize; i++) { + // printf("%d ", buf[i]); + // } - uint8_t *buf = fbbP->GetBufferPointer(); - std::size_t bufsize = fbbP->GetSize(); - flatbuffers::Verifier verifier(buf,fbbP->GetSize()); + flatbuffers::Verifier verifier(buf, bufsize); std::cout<< "VerifyKUKAiiwaStatesBuffer: " << grl::flatbuffer::VerifyKUKAiiwaStatesBuffer(verifier) << std::endl; + std::cout << " bufsize: " << bufsize << std::endl; + std::string binary_file_path = "Kuka_test_binary.iiwa"; std::string json_file_path = "kuka_test_text.json"; @@ -249,24 +342,48 @@ BOOST_AUTO_TEST_CASE(runRepeatedly) // Get the current working directory std::string fbs_filename("KUKAiiwa.fbs"); + + std::cout << "fbs exist? " << flatbuffers::FileExists(fbs_filename.c_str()) << std::endl; flatbuffers::Parser parser; - std::vector includePaths; + std::vector includePaths = std::vector(); + + + std::string current_working_dir; + char buff[2048]; + /// Get the current working directory + getcwd(buff, 2048); + current_working_dir = std::string(buff); + std::cout << "The current working dir: " << current_working_dir << std::endl; + includePaths.push_back(current_working_dir); + std::string schemafile; + flatbuffers::LoadFile(fbs_filename.c_str(), false, &schemafile); + // std::cout<<"Schema File: " << std::endl << schemafile << std::endl; + // parse fbs schema, so we can use it to parse the data after + // create a list of char* pointers so we can call Parse + std::vector include_directories; + for(int i = 0; i < includePaths.size(); i++){ + include_directories.push_back(includePaths[i].c_str()); + } + include_directories.push_back(nullptr); - OK = OK && grl::ParseSchemaFile(parser, fbs_filename, includePaths, false); + OK = OK && parser.Parse(schemafile.c_str(), &include_directories[0]); + + // OK = OK && grl::ParseSchemaFile(parser, fbs_filename, includePaths, false); std::string jsongen; - // now generate text from the flatbuffer binary + // now generate text from the flatbuffer binary - OK = OK && GenerateText(parser, buf, &jsongen); + OK = OK && flatbuffers::GenerateText(parser, buf, &jsongen); + std::cout << "json string :" << jsongen.size() << std::endl; + std::cout << jsongen.c_str() << std::endl; // Write the data get from flatbuffer binary to json file on disk. - std::cout << "buffer :" << (unsigned)strlen(reinterpret_cast(buf)) << std::endl; - std::ofstream out(json_file_path); - out << jsongen.c_str(); - out.close(); + std::cout << "buffer :" << charP_size(reinterpret_cast(buf)) << std::endl; + std::ofstream out(json_file_path); + out << jsongen.c_str(); + out.close(); - std::cout << jsongen.c_str() << std::endl; // OK = OK && grl::SaveFlatBufferFile( // buf, - // fbbP->GetSize(), + // fbb.GetSize(), // binary_file_path, // fbs_filename, // json_file_path); @@ -279,4 +396,5 @@ BOOST_AUTO_TEST_CASE(runRepeatedly) + BOOST_AUTO_TEST_SUITE_END() From 59181b9b1043799abb323c1806fd993bfc5f8edf Mon Sep 17 00:00:00 2001 From: Chunting Date: Wed, 6 Dec 2017 15:56:33 -0500 Subject: [PATCH 020/102] /usr/local/include/flatbuffers/flatbuffer.h, _max_tables is changed to 1000000 (ten times of the defaut value), then the single buffer size can reach 2045MB. ( if beyond 2046MB, an assert might be lanched) --- include/grl/flatbuffer/KUKAiiwa.fbs | 6 +- include/grl/flatbuffer/flatbuffer.hpp | 44 +++-- include/grl/kuka/KukaToFlatbuffer.hpp | 242 +++++++++++--------------- test/kukaiiwaTest.cpp | 142 ++++----------- 4 files changed, 161 insertions(+), 273 deletions(-) diff --git a/include/grl/flatbuffer/KUKAiiwa.fbs b/include/grl/flatbuffer/KUKAiiwa.fbs index c6c1ce6..3774019 100644 --- a/include/grl/flatbuffer/KUKAiiwa.fbs +++ b/include/grl/flatbuffer/KUKAiiwa.fbs @@ -293,7 +293,7 @@ table FRITimeStamp { // a table that is used as a record of all communication over FRI // it is not for user configuration or anything else, it should // simply reflect a history of what was sent and what was received. -/* + table FRIMessageLog { sessionState:ESessionState; @@ -317,7 +317,7 @@ table FRIMessageLog { timeStamp:TimeEvent; } -*/ + table KUKAiiwaState { name:string; // The name of the robot to update (identifier used when applicable, doesn't ever change or set the name) @@ -337,7 +337,7 @@ table KUKAiiwaState { hasMonitorConfig:bool = false; monitorConfig:KUKAiiwaMonitorConfiguration; - // FRIMessage:FRIMessageLog; + FRIMessage:FRIMessageLog; } diff --git a/include/grl/flatbuffer/flatbuffer.hpp b/include/grl/flatbuffer/flatbuffer.hpp index 8d11464..fa1c80f 100644 --- a/include/grl/flatbuffer/flatbuffer.hpp +++ b/include/grl/flatbuffer/flatbuffer.hpp @@ -10,27 +10,27 @@ namespace grl { // loads a json flatbuffer from a file -// bool LoadJSONFlatbuffer(flatbuffers::Parser& parser, std::string schemaPath, std::string jsonPath, std::string include_directory, bool binary = false) -// { -// // source: https://github.com/google/flatbuffers/blob/master/samples/sample_text.cpp - -// // load FlatBuffer schema (.fbs) and JSON from disk -// std::string schemafile; -// std::string jsonfile; -// bool ok = flatbuffers::LoadFile(schemaPath.c_str(), binary, &schemafile) && -// flatbuffers::LoadFile(jsonPath.c_str(), binary, &jsonfile); -// if (!ok) { -// printf("couldn't load files!\n"); -// return nullptr; -// } - -// // parse schema first, so we can use it to parse the data after -// const char *include_directories[] = { include_directory.c_str(), "flatbuffer", nullptr }; -// ok = parser.Parse(schemafile.c_str(), include_directories) && -// parser.Parse(jsonfile.c_str(), include_directories); -// assert(ok); -// return ok; -// } +bool LoadJSONFlatbuffer(flatbuffers::Parser& parser, std::string schemaPath, std::string jsonPath, std::string include_directory, bool binary = false) +{ + // source: https://github.com/google/flatbuffers/blob/master/samples/sample_text.cpp + + // load FlatBuffer schema (.fbs) and JSON from disk + std::string schemafile; + std::string jsonfile; + bool ok = flatbuffers::LoadFile(schemaPath.c_str(), binary, &schemafile) && + flatbuffers::LoadFile(jsonPath.c_str(), binary, &jsonfile); + if (!ok) { + printf("couldn't load files!\n"); + return nullptr; + } + + // parse schema first, so we can use it to parse the data after + const char *include_directories[] = { include_directory.c_str(), "flatbuffer", nullptr }; + ok = parser.Parse(schemafile.c_str(), include_directories) && + parser.Parse(jsonfile.c_str(), include_directories); + assert(ok); + return ok; +} /// @brief Loads a flatbuffers schema file from disk into a flatbuffers parser. /// @@ -169,8 +169,6 @@ bool SaveFlatBufferFile( std::ofstream out(json_file_path); out << jsongen.c_str(); out.close(); - std::cout << "json string :" << jsongen.size() << std::endl; - std::cout << jsongen.c_str() << std::endl; } return ok; } diff --git a/include/grl/kuka/KukaToFlatbuffer.hpp b/include/grl/kuka/KukaToFlatbuffer.hpp index fcc13e0..5d0ac3a 100644 --- a/include/grl/kuka/KukaToFlatbuffer.hpp +++ b/include/grl/kuka/KukaToFlatbuffer.hpp @@ -6,7 +6,7 @@ #define FLATBUFFERS_DEBUG_VERIFICATION_FAILURE #include -#include "flatbuffers/flatbuffers.h" + #include "grl/flatbuffer/JointState_generated.h" #include "grl/flatbuffer/ArmControlState_generated.h" #include "grl/flatbuffer/KUKAiiwa_generated.h" @@ -577,142 +577,106 @@ flatbuffers::Offset toFlatBuffer( } /// KUKAiiwa.fbs -// flatbuffers::Offset toFlatBuffer( -// flatbuffers::FlatBufferBuilder &fbb, -// const std::string &name, -// const std::string &destination, -// const std::string &source, -// const double timestamp, -// const bool setArmControlState, -// flatbuffers::Offset &armControlState, -// const bool setArmConfiguration, -// flatbuffers::Offset &armConfiguration, -// const bool hasMonitorState, // MessageMonitorData monitorData in FRIMessage.pb.h -// flatbuffers::Offset &monitorState, -// const bool hasMonitorConfig, -// flatbuffers::Offset &monitorConfig) -// { -// return grl::flatbuffer::CreateKUKAiiwaState( -// fbb, -// fbb.CreateString(name), -// fbb.CreateString(destination), -// fbb.CreateString(source), -// timestamp, -// setArmControlState, -// armControlState, -// setArmConfiguration, -// armConfiguration, -// hasMonitorState, -// monitorState, -// hasMonitorConfig, -// monitorConfig); -// } +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const std::string &name, + const std::string &destination, + const std::string &source, + const double timestamp, + const bool setArmControlState, + flatbuffers::Offset &armControlState, + const bool setArmConfiguration, + flatbuffers::Offset &armConfiguration, + const bool hasMonitorState, // MessageMonitorData monitorData in FRIMessage.pb.h + flatbuffers::Offset &monitorState, + const bool hasMonitorConfig, + flatbuffers::Offset &monitorConfig) +{ + return grl::flatbuffer::CreateKUKAiiwaState( + fbb, + fbb.CreateString(name), + fbb.CreateString(destination), + fbb.CreateString(source), + timestamp, + setArmControlState, + armControlState, + setArmConfiguration, + armConfiguration, + hasMonitorState, + monitorState, + hasMonitorConfig, + monitorConfig); +} -/// KUKAiiwa.fbs -// flatbuffers::Offset toFlatBuffer( -// flatbuffers::FlatBufferBuilder &fbb, -// const ::FRISessionState &sessionState, -// const ::FRIConnectionQuality &connectionQuality, -// const ::ControlMode &controlMode, // enum -// const ::FRIMonitoringMessage &friMonitoringMessage, -// const grl::TimeEvent &timeEvent) // There are two times (TimeStamp and TimeEvent) here, which one should be kept? both? -// { -// auto _sessionState = toFlatBuffer(sessionState); -// auto _connectionQuality = toFlatBuffer(connectionQuality); -// auto _controlMode = toFlatBuffer(controlMode); -// auto _messageIdentifier = friMonitoringMessage.header.messageIdentifier; -// auto _sequenceCounter = friMonitoringMessage.header.sequenceCounter; -// auto _reflectedSequenceCounter = friMonitoringMessage.header.reflectedSequenceCounter; -// // auto _sec = timeStamp.sec; -// // auto _nanosec = timeStamp.nanosec; -// std::vector data; -// // get measured joint position -// grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_angle_open_chain_state_tag()); -// flatbuffers::Offset> _measuredJointPosition = fbb.CreateVector(data); - -// data.clear(); -// // get measured joint torque -// grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_torque_open_chain_state_tag()); -// flatbuffers::Offset> _measuredTorque = fbb.CreateVector(data); - -// data.clear(); -// // get measured joint torque -// grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_torque_open_chain_command_tag()); -// flatbuffers::Offset> _commandedJointPosition = fbb.CreateVector(data); - -// data.clear(); -// // get commanded joint torque -// grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_torque_open_chain_command_tag()); -// flatbuffers::Offset> _commandedTorque = fbb.CreateVector(data); - -// data.clear(); -// // get measured external torque -// grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_torque_external_open_chain_state_tag()); -// flatbuffers::Offset> _externalTorque = fbb.CreateVector(data); - -// data.clear(); -// // get interpolated joint state -// grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_angle_interpolated_open_chain_state_tag()); -// flatbuffers::Offset> _jointStateInterpolated = fbb.CreateVector(data); - -// flatbuffers::Offset _timeEvent = grl::toFlatBuffer(fbb, timeEvent); - -// auto _overlayType = toFlatBuffer(friMonitoringMessage.ipoData.overlayType); - -// //////////////////////////////////////////////////////////////////////////// -// /*** This block is to verify the specific flatbuffer object ***/ -// flatbuffers::FlatBufferBuilder _fbb; -// flatbuffers::Offset> test_measuredJointPosition = _fbb.CreateVector(data); -// flatbuffers::Offset> test_measuredTorque = _fbb.CreateVector(data); -// flatbuffers::Offset> test_commandedJointPosition = _fbb.CreateVector(data); -// flatbuffers::Offset> test_commandedTorque = _fbb.CreateVector(data); -// flatbuffers::Offset> test_externalTorque = _fbb.CreateVector(data); -// flatbuffers::Offset> test_jointStateInterpolated = _fbb.CreateVector(data); -// flatbuffers::Offset test_timeEvent = grl::toFlatBuffer(_fbb, timeEvent); -// auto _friMessageLog = grl::flatbuffer::CreateFRIMessageLog( -// _fbb, -// _sessionState, -// _connectionQuality, -// _controlMode, -// _messageIdentifier, -// _sequenceCounter, -// _reflectedSequenceCounter, -// // _sec, -// // _nanosec, -// test_measuredJointPosition, -// test_measuredTorque, -// test_commandedJointPosition, -// test_commandedTorque, -// test_externalTorque, -// test_jointStateInterpolated, -// test_timeEvent); -// _fbb.Finish(_friMessageLog); -// auto verifier = flatbuffers::Verifier(_fbb.GetBufferPointer(), _fbb.GetSize()); -// bool success = verifier.VerifyBuffer(); -// std::cout <<" verifier success for grl::flatbuffer::FRIMessageLog: " << success << std::endl; -// /////////////////////////////////////////////////////////////////////////// - - -// return grl::flatbuffer::CreateFRIMessageLog( -// fbb, -// _sessionState, -// _connectionQuality, -// _controlMode, -// _messageIdentifier, -// _sequenceCounter, -// _reflectedSequenceCounter, -// // _sec, -// // _nanosec, -// _measuredJointPosition, -// _measuredTorque, -// _commandedJointPosition, -// _commandedTorque, -// _externalTorque, -// _jointStateInterpolated, -// _timeEvent); -// } +// KUKAiiwa.fbs +flatbuffers::Offset toFlatBuffer( + flatbuffers::FlatBufferBuilder &fbb, + const ::FRISessionState &sessionState, + const ::FRIConnectionQuality &connectionQuality, + const ::ControlMode &controlMode, // enum + const ::FRIMonitoringMessage &friMonitoringMessage, + const grl::TimeEvent &timeEvent) // There are two times (TimeStamp and TimeEvent) here, which one should be kept? both? +{ + auto _sessionState = toFlatBuffer(sessionState); + auto _connectionQuality = toFlatBuffer(connectionQuality); + auto _controlMode = toFlatBuffer(controlMode); + auto _messageIdentifier = friMonitoringMessage.header.messageIdentifier; + auto _sequenceCounter = friMonitoringMessage.header.sequenceCounter; + auto _reflectedSequenceCounter = friMonitoringMessage.header.reflectedSequenceCounter; + std::vector data; + // get measured joint position + grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_angle_open_chain_state_tag()); + flatbuffers::Offset> _measuredJointPosition = fbb.CreateVector(data); + + data.clear(); + // get measured joint torque + grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_torque_open_chain_state_tag()); + flatbuffers::Offset> _measuredTorque = fbb.CreateVector(data); + + data.clear(); + // get measured joint torque + grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_torque_open_chain_command_tag()); + flatbuffers::Offset> _commandedJointPosition = fbb.CreateVector(data); + + data.clear(); + // get commanded joint torque + grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_torque_open_chain_command_tag()); + flatbuffers::Offset> _commandedTorque = fbb.CreateVector(data); + + data.clear(); + // get measured external torque + grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_torque_external_open_chain_state_tag()); + flatbuffers::Offset> _externalTorque = fbb.CreateVector(data); + + data.clear(); + // get interpolated joint state + grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_angle_interpolated_open_chain_state_tag()); + flatbuffers::Offset> _jointStateInterpolated = fbb.CreateVector(data); + + flatbuffers::Offset _timeEvent = grl::toFlatBuffer(fbb, timeEvent); + + auto _overlayType = toFlatBuffer(friMonitoringMessage.ipoData.overlayType); + + return grl::flatbuffer::CreateFRIMessageLog( + fbb, + _sessionState, + _connectionQuality, + _controlMode, + _messageIdentifier, + _sequenceCounter, + _reflectedSequenceCounter, + // _sec, + // _nanosec, + _measuredJointPosition, + _measuredTorque, + _commandedJointPosition, + _commandedTorque, + _externalTorque, + _jointStateInterpolated, + _timeEvent); +} /// KUKAiiwa.fbs flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder &fbb, @@ -727,8 +691,8 @@ flatbuffers::Offset toFlatBuffer( const bool hasMonitorState, const flatbuffers::Offset &monitorState, const bool hasMonitorConfig, - const flatbuffers::Offset &monitorConfig) - //const flatbuffers::Offset &FRIMessage + const flatbuffers::Offset &monitorConfig, + const flatbuffers::Offset &FRIMessage) { return grl::flatbuffer::CreateKUKAiiwaState( @@ -753,14 +717,10 @@ flatbuffers::Offset toFlatBuffer( flatbuffers::FlatBufferBuilder &fbb, const std::vector> &kukaiiwastates) { - auto states = fbb.CreateVector>(kukaiiwastates); - // std::cout<< "states (flatbuffer vector):" << states.size() << std::endl; return grl::flatbuffer::CreateKUKAiiwaStates( fbb, - states); + fbb.CreateVector>(kukaiiwastates)); } -// } // End of arm namespace -// } // End of robot namespace } // End of grl namespace diff --git a/test/kukaiiwaTest.cpp b/test/kukaiiwaTest.cpp index 8fa8a47..9a48a33 100644 --- a/test/kukaiiwaTest.cpp +++ b/test/kukaiiwaTest.cpp @@ -1,6 +1,7 @@ #define BOOST_TEST_DYN_LINK #define BOOST_TEST_MODULE grlTest +#define FLATBUFFERS_DEBUG_VERIFICATION_FAILUR // system includes #include #include @@ -55,8 +56,10 @@ BOOST_AUTO_TEST_SUITE(KukaTest) BOOST_AUTO_TEST_CASE(runRepeatedly) { const std::size_t MegaByte = 1024*1024; - // If we write too large a flatbuffer - const std::size_t single_buffer_limit_bytes = 3*MegaByte; + // The maximum size of a single flatbuffer is 2GB - 1, but actually single_buffer_limit_bytes can't be set beyond 2045 MB, otherwise assert might be launched. + // In 32bits, this evaluates to 2GB - 1, defined in base.h. + // #define FLATBUFFERS_MAX_BUFFER_SIZE ((1ULL << (sizeof(soffset_t) * 8 - 1)) - 1) + const std::size_t single_buffer_limit_bytes = 2045*MegaByte; // Install a signal handler to catch a signal when CONTROL+C std::signal(SIGINT, signal_handler); bool OK = true; @@ -135,26 +138,10 @@ BOOST_AUTO_TEST_CASE(runRepeatedly) jointstate.push_back(jointValues[i]); } /// Make reference to void map_repeatedDouble() in pb_frimessages_callback.c and pb_frimessages_callbacks.h - std::shared_ptr values(std::make_shared()); - // pb_callback_t *values = new pb_callback_t; - int numDOF = 7; - values->funcs.encode = &encode_repeatedDouble; - values->funcs.decode = &decode_repeatedDouble; - tRepeatedDoubleArguments *arg = new tRepeatedDoubleArguments; - // map the local container data to the message data fields - arg->max_size = numDOF; - arg->size = 0; - arg->value = (double*) malloc(numDOF * sizeof(double)); - - for(int i=0; ivalue[i] = jointstate[i]; - } - values->arg = arg; - strjointValues.value = *values; - // strjointValues.value = *makeupJointValues(jointValues); + strjointValues.value = *makeupJointValues(jointValues); grl::robot::arm::KukaState::joint_state kukajointstate(jointstate); - //////////////////////////////////////////////////////////// + //////////////////////////////////////////////////////////// // Double check the initialization of this parameter ////// ////////////////////////////////////////////////////////// grl::robot::arm::KukaState armState; @@ -190,15 +177,15 @@ BOOST_AUTO_TEST_CASE(runRepeatedly) ::Transformation transformation{ "Transformation", 12, {1,2,3,4,5,6,7,8,9,10,11,12}, true, ::TimeStamp{3,4}}; /// Transformation requestedTransformations[5] = { transformation, transformation, transformation, transformation, transformation }; ::MessageEndOf endOfMessageData{true, 32, true, ::Checksum{true, 32}}; - // ::FRIMonitoringMessage friMonitoringMessage { - // messageHeader, - // true, robotInfo, - // true, messageMonitorData, - // true, connectionInfo, - // true, ipoData, - // 5, - // { transformation, transformation, transformation, transformation, transformation }, - // true, endOfMessageData}; + ::FRIMonitoringMessage friMonitoringMessage { + messageHeader, + true, robotInfo, + true, messageMonitorData, + true, connectionInfo, + true, ipoData, + 5, + { transformation, transformation, transformation, transformation, transformation }, + true, endOfMessageData}; grl::TimeEvent time_event_stamp; std::vector> kukaiiwaStateVec; std::size_t builder_size_bytes = 0; @@ -207,12 +194,8 @@ BOOST_AUTO_TEST_CASE(runRepeatedly) while(!signalStatusG && OK && builder_size_bytes < single_buffer_limit_bytes ) { - std::cout <<"Step into loop..." << std::endl; // grl::robot::arm::copy(friMonitoringMessage, armState); flatbuffers::Offset controlState = grl::toFlatBuffer(fbb, basename, sequenceNumber++, duration, armState, armControlMode); - // if(controlState == 0) { - // assert(("controlState is empty?", controlState == 0)); - // } flatbuffers::Offset setCartesianImpedance = grl::toFlatBuffer(fbb, cart_stiffness_, cart_damping_, nullspace_stiffness_, nullspace_damping_, cart_max_path_deviation_, cart_max_ctrl_vel_, cart_max_ctrl_force_, max_control_force_stop_); flatbuffers::Offset setJointImpedance = grl::toFlatBuffer(fbb, joint_stiffness_, joint_damping_); @@ -262,13 +245,13 @@ BOOST_AUTO_TEST_CASE(runRepeatedly) "currentMotionCenter", true); - // flatbuffers::Offset friMessageLog = grl::toFlatBuffer( - // fbb, - // ::FRISessionState::FRISessionState_IDLE, - // ::FRIConnectionQuality::FRIConnectionQuality_POOR, - // controlMode, - // friMonitoringMessage, - // time_event_stamp); + flatbuffers::Offset friMessageLog = grl::toFlatBuffer( + fbb, + ::FRISessionState::FRISessionState_IDLE, + ::FRIConnectionQuality::FRIConnectionQuality_POOR, + controlMode, + friMonitoringMessage, + time_event_stamp); grl::flatbuffer::Wrench cartesianWrench{grl::flatbuffer::Vector3d(1,1,1),grl::flatbuffer::Vector3d(1,1,1),grl::flatbuffer::Vector3d(1,1,1)}; @@ -301,9 +284,8 @@ BOOST_AUTO_TEST_CASE(runRepeatedly) true, controlState, true, kukaiiwaArmConfiguration, true, kukaiiwaMonitorState, - false, monitorConfig - // , friMessageLog - ); + false, monitorConfig, + friMessageLog); kukaiiwaStateVec.push_back(kukaiiwaState); @@ -317,84 +299,32 @@ BOOST_AUTO_TEST_CASE(runRepeatedly) flatbuffers::Offset kukaStates = grl::toFlatBuffer(fbb, kukaiiwaStateVec); - grl::flatbuffer::FinishKUKAiiwaStatesBuffer(fbb, kukaStates); uint8_t *buf = fbb.GetBufferPointer(); std::size_t bufsize = fbb.GetSize(); - auto KUKAiiwaStatesP= grl::flatbuffer::GetKUKAiiwaStates(buf); - auto states = KUKAiiwaStatesP->states(); - assert(states); - // for (flatbuffers::uoffset_t i = 0; i < bufsize; i++) { - // printf("%d ", buf[i]); - // } - flatbuffers::Verifier verifier(buf, bufsize); - std::cout<< "VerifyKUKAiiwaStatesBuffer: " << grl::flatbuffer::VerifyKUKAiiwaStatesBuffer(verifier) << std::endl; - std::cout << " bufsize: " << bufsize << std::endl; + OK = OK && grl::flatbuffer::VerifyKUKAiiwaStatesBuffer(verifier); + //assert(OK && "VerifyKUKAiiwaStatesBuffer"); + + std::cout << "Buffer size: " << bufsize << std::endl; std::string binary_file_path = "Kuka_test_binary.iiwa"; std::string json_file_path = "kuka_test_text.json"; - - OK = flatbuffers::SaveFile(binary_file_path.c_str(), reinterpret_cast(buf), bufsize, true); - // Get the current working directory std::string fbs_filename("KUKAiiwa.fbs"); - std::cout << "fbs exist? " << flatbuffers::FileExists(fbs_filename.c_str()) << std::endl; - flatbuffers::Parser parser; - std::vector includePaths = std::vector(); - - - std::string current_working_dir; - char buff[2048]; - /// Get the current working directory - getcwd(buff, 2048); - current_working_dir = std::string(buff); - std::cout << "The current working dir: " << current_working_dir << std::endl; - includePaths.push_back(current_working_dir); - std::string schemafile; - flatbuffers::LoadFile(fbs_filename.c_str(), false, &schemafile); - // std::cout<<"Schema File: " << std::endl << schemafile << std::endl; - // parse fbs schema, so we can use it to parse the data after - // create a list of char* pointers so we can call Parse - std::vector include_directories; - for(int i = 0; i < includePaths.size(); i++){ - include_directories.push_back(includePaths[i].c_str()); - } - include_directories.push_back(nullptr); - - OK = OK && parser.Parse(schemafile.c_str(), &include_directories[0]); - - // OK = OK && grl::ParseSchemaFile(parser, fbs_filename, includePaths, false); - std::string jsongen; - // now generate text from the flatbuffer binary - - OK = OK && flatbuffers::GenerateText(parser, buf, &jsongen); - std::cout << "json string :" << jsongen.size() << std::endl; - std::cout << jsongen.c_str() << std::endl; - // Write the data get from flatbuffer binary to json file on disk. - std::cout << "buffer :" << charP_size(reinterpret_cast(buf)) << std::endl; - std::ofstream out(json_file_path); - out << jsongen.c_str(); - out.close(); - - // OK = OK && grl::SaveFlatBufferFile( - // buf, - // fbb.GetSize(), - // binary_file_path, - // fbs_filename, - // json_file_path); - std::cout << "Save json file correctly? " << OK << " Buffer size saved to binary file: " << bufsize << std::endl; + OK = OK && grl::SaveFlatBufferFile( + buf, + fbb.GetSize(), + binary_file_path, + fbs_filename, + json_file_path); + assert(OK && "SaveFlatBufferFile"); std::cout << "End of the program" << std::endl; - - } - - - BOOST_AUTO_TEST_SUITE_END() From 02f443a230bebabf7fab906aac39e5315dcdc7b7 Mon Sep 17 00:00:00 2001 From: Chunting Date: Mon, 11 Dec 2017 23:35:52 -0500 Subject: [PATCH 021/102] Auto stash before merge of "RobotToFlatBuffer" and "origin/fusiontrack" --- include/thirdparty/fbs_tk/fbs_tk.hpp | 4 ++++ .../v_repExtAtracsysFusionTrack.cpp | 12 ++++++++++++ 2 files changed, 16 insertions(+) diff --git a/include/thirdparty/fbs_tk/fbs_tk.hpp b/include/thirdparty/fbs_tk/fbs_tk.hpp index e34d32f..40164be 100644 --- a/include/thirdparty/fbs_tk/fbs_tk.hpp +++ b/include/thirdparty/fbs_tk/fbs_tk.hpp @@ -251,6 +251,10 @@ struct Root { } const Buffer & get_data() const { + return data; + } + + const Buffer & get_data_safe() const { assert(valid()); return data; } diff --git a/src/v_repExtAtracsysFusionTrack/v_repExtAtracsysFusionTrack.cpp b/src/v_repExtAtracsysFusionTrack/v_repExtAtracsysFusionTrack.cpp index c471df9..ede4641 100755 --- a/src/v_repExtAtracsysFusionTrack/v_repExtAtracsysFusionTrack.cpp +++ b/src/v_repExtAtracsysFusionTrack/v_repExtAtracsysFusionTrack.cpp @@ -48,6 +48,7 @@ std::shared_ptr fusionTrackPG; std::shared_ptr loggerPG; /// Recording will begin when the simulation starts running, and log files will be saved every time it stops running. bool recordWhileSimulationIsRunningG = false; +bool shouldPrintConnectionSuccess = true; void removeGeometryID(std::string geometryID_lua_param, grl::FusionTrackLogAndTrack::Params ¶ms) @@ -283,6 +284,7 @@ void LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_STOP(SLuaCallBack *p) { loggerPG->info("Ending Atracsys Fusion Track Plugin connection to Optical Tracker\n"); fusionTrackPG.reset(); + shouldPrintConnectionSuccess = true; } ///////////////////////////////////////////////////////////////////////////////////////// @@ -293,6 +295,7 @@ void LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_RESET(SLuaCallBack *p) { fusionTrackPG.reset(); LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_START(p); + shouldPrintConnectionSuccess = true; } ///////////////////////////////////////////////////////////////////////////////////////// @@ -660,6 +663,15 @@ VREP_DLLEXPORT void *v_repMessage(int message, int *auxiliaryData, void *customD frameInWhichToMoveObject, pose); } + + // Let the user know when the connection starts successfully and sees a marker :-) + if(shouldPrintConnectionSuccess && transforms.size() > 0) + { + std::string msg("v_repExtAtracsysFusionTrack plugin connected successfully and detected " + std::to_string(transforms.size()) +" markers!"); + simAddStatusbarMessage(msg.c_str()); + loggerPG->info(msg); + shouldPrintConnectionSuccess = false; + } } else if(fusionTrackPG && !fusionTrackPG->is_active()) { From e20bdd855811bf608d50eaf860e4dc0d5fe74a16 Mon Sep 17 00:00:00 2001 From: Chunting Date: Tue, 12 Dec 2017 00:26:25 -0500 Subject: [PATCH 022/102] updated call to simExtHandEyeCalibStart() to supply vrep string names --- src/lua/robone.lua | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/lua/robone.lua b/src/lua/robone.lua index 7491b10..68b9496 100644 --- a/src/lua/robone.lua +++ b/src/lua/robone.lua @@ -351,7 +351,8 @@ robone.handEyeCalibScript=function() startP,startO=grl.getTransformBetweenHandles(target,targetBase) -- Enable/Disable custom IK - useGrlInverseKinematics=false + useGrlInverseKinematics=true + simExtHandEyeCalibStart('Robotiiwa' , 'RobotMillTipTarget', 'OpticalTrackerBase', 'Fiducial#22') if (grl.isModuleLoaded('GrlInverseKinematics') and useGrlInverseKinematics) then simExtGrlInverseKinematicsStart() From f0301746985997019d6fe0bc166b5293aa893770 Mon Sep 17 00:00:00 2001 From: Chunting Date: Tue, 12 Dec 2017 18:06:48 -0500 Subject: [PATCH 023/102] Expand the capacity of single buffer size by passing _max_tables to Verifier --- example/fusionTrackExample.cpp | 21 +++++-------------- .../grl/sensor/FusionTrackToFlatbuffer.hpp | 5 ++++- test/kukaiiwaTest.cpp | 14 ++++++------- 3 files changed, 16 insertions(+), 24 deletions(-) diff --git a/example/fusionTrackExample.cpp b/example/fusionTrackExample.cpp index 2c8e209..ba7e4b3 100644 --- a/example/fusionTrackExample.cpp +++ b/example/fusionTrackExample.cpp @@ -34,7 +34,7 @@ int main(int argc, char **argv) const std::size_t MegaByte = 1024*1024; // If we write too large a flatbuffer - const std::size_t single_buffer_limit_bytes = 512*MegaByte; + const std::size_t single_buffer_limit_bytes = 1024*MegaByte; // Install a signal handler to catch a signal when CONTROL+C std::signal(SIGINT, signal_handler); // std::raise(SIGINT); @@ -89,9 +89,10 @@ int main(int argc, char **argv) std::string fbs_filename("LogKUKAiiwaFusionTrack.fbs"); // std::string includePath = grl::getpathtofbsfile(fbs_filename); std::string includePath = "/home/chunting/src/robonetracker/build/"; + std::size_t builder_size_bytes = 0; - while(!signalStatusG && test) + while(!signalStatusG && test && builder_size_bytes < single_buffer_limit_bytes) { // loop through all connected devices for(auto serialNumber : serialNumbers) @@ -124,12 +125,9 @@ int main(int argc, char **argv) // also log all other data *except* when there is no new frame available oneKUKAiiwaFusionTrackMessage = grl::toFlatBuffer(fbb, ft, frame, writeParameters); KUKAiiwaFusionTrackMessage_vector.push_back(oneKUKAiiwaFusionTrackMessage); - - // test_oneKUKAiiwaFusionTrackMessage = grl::toFlatBuffer(test_fbb, ft, frame); - // test_KUKAiiwaFusionTrackMessage_vector.push_back(test_oneKUKAiiwaFusionTrackMessage); } - std::size_t builder_size_bytes = fbb.GetSize(); + builder_size_bytes = fbb.GetSize(); if(debug || update_step % print_status_period == 0) { std::size_t newData = builder_size_bytes - previous_size; @@ -201,16 +199,7 @@ int main(int argc, char **argv) json_file_path); - std::cout << "Saved binary_file_path: " << binary_file_path << " json_file_path: " << json_file_path << " saved with success result: " << success << std::endl; - - // flatbuffers::Offset>> test_states = grl::toFlatBuffer(test_fbb, test_KUKAiiwaFusionTrackMessage_vector); - // flatbuffers::Offset test_fbLogKUKAiiwaFusionTrack = grl::toFlatBuffer(test_fbb, test_states); - - // test_fbb.Finish(test_fbLogKUKAiiwaFusionTrack); - // auto test_verifier = flatbuffers::Verifier(test_fbb.GetBufferPointer(), test_fbb.GetSize()); - // bool test_success = test_verifier.VerifyBuffer(); - // std::cout <<" verifier test_success for LogKUKAiiwaFusionTrack: " << test_success << std::endl; - + std::cout << "Saved binary_file_path: " << binary_file_path << " json_file_path: " << json_file_path << " saved with success result: " << success << std::endl; std::cout << "End of the program" << std::endl; return success; } // End of main function diff --git a/include/grl/sensor/FusionTrackToFlatbuffer.hpp b/include/grl/sensor/FusionTrackToFlatbuffer.hpp index bdad1aa..3cf346e 100644 --- a/include/grl/sensor/FusionTrackToFlatbuffer.hpp +++ b/include/grl/sensor/FusionTrackToFlatbuffer.hpp @@ -40,8 +40,11 @@ bool FinishAndVerifyBuffer( // fbb.Finish(oneKUKAiiwaFusionTrackMessage, file_identifier); fbb.Finish(fbLogKUKAiiwaFusionTrack, file_identifier); - auto verifier = flatbuffers::Verifier(fbb.GetBufferPointer(), fbb.GetSize()); + flatbuffers::uoffset_t _max_depth = 64; + flatbuffers::uoffset_t _max_tables = 1000000000; + auto verifier = flatbuffers::Verifier(fbb.GetBufferPointer(), fbb.GetSize(), _max_depth, _max_tables); bool success = grl::flatbuffer::VerifyLogKUKAiiwaFusionTrackBuffer(verifier); + assert(success && "VerifyLogKUKAiiwaFusionTrackBuffer"); return success; } diff --git a/test/kukaiiwaTest.cpp b/test/kukaiiwaTest.cpp index 9a48a33..fa01008 100644 --- a/test/kukaiiwaTest.cpp +++ b/test/kukaiiwaTest.cpp @@ -17,9 +17,8 @@ /// Make reference to void map_repeatedDouble() in pb_frimessages_callback.c and pb_frimessages_callbacks.h std::shared_ptr makeupJointValues(std::vector &jointvalue) { - /// Make reference to void map_repeatedDouble() in pb_frimessages_callback.c and pb_frimessages_callbacks.h + /// Make reference to void map_repeatedDouble() in pb_frimessages_callback.c and pb_frimessages_callbacks.h std::shared_ptr values(std::make_shared()); - // pb_callback_t *values = new pb_callback_t; int numDOF = 7; values->funcs.encode = &encode_repeatedDouble; values->funcs.decode = &decode_repeatedDouble; @@ -56,9 +55,7 @@ BOOST_AUTO_TEST_SUITE(KukaTest) BOOST_AUTO_TEST_CASE(runRepeatedly) { const std::size_t MegaByte = 1024*1024; - // The maximum size of a single flatbuffer is 2GB - 1, but actually single_buffer_limit_bytes can't be set beyond 2045 MB, otherwise assert might be launched. - // In 32bits, this evaluates to 2GB - 1, defined in base.h. - // #define FLATBUFFERS_MAX_BUFFER_SIZE ((1ULL << (sizeof(soffset_t) * 8 - 1)) - 1) + // In 32bits, the maximum size of a single flatbuffer is 2GB - 1 (defined in base.h), but actually single_buffer_limit_bytes can't be set beyond 2045 MB, otherwise assert might be launched. const std::size_t single_buffer_limit_bytes = 2045*MegaByte; // Install a signal handler to catch a signal when CONTROL+C std::signal(SIGINT, signal_handler); @@ -304,9 +301,12 @@ BOOST_AUTO_TEST_CASE(runRepeatedly) uint8_t *buf = fbb.GetBufferPointer(); std::size_t bufsize = fbb.GetSize(); - flatbuffers::Verifier verifier(buf, bufsize); + /// To expand the capacity of a single buffer, _max_tables is set to 10000000 + flatbuffers::uoffset_t _max_depth = 64; + flatbuffers::uoffset_t _max_tables = 10000000; + flatbuffers::Verifier verifier(buf, bufsize, _max_depth, _max_tables); OK = OK && grl::flatbuffer::VerifyKUKAiiwaStatesBuffer(verifier); - //assert(OK && "VerifyKUKAiiwaStatesBuffer"); + assert(OK && "VerifyKUKAiiwaStatesBuffer"); std::cout << "Buffer size: " << bufsize << std::endl; From ad1e078fc2f9ae20f81b02afb174c48ca1569b96 Mon Sep 17 00:00:00 2001 From: Chunting Date: Wed, 3 Jan 2018 14:11:35 -0500 Subject: [PATCH 024/102] Connected to the robot in Unit Test --- include/grl/kuka/Kuka.hpp | 2 +- include/grl/kuka/KukaDriver.hpp | 3 +- include/grl/kuka/KukaFRIdriver.hpp | 13 +- include/grl/sensor/FusionTrackLogAndTrack.hpp | 6 +- src/lua/robone.lua | 2 +- test/KukaFRITest.cpp | 57 +- test/kukaiiwaTest.cpp | 536 ++++++++++-------- 7 files changed, 375 insertions(+), 244 deletions(-) diff --git a/include/grl/kuka/Kuka.hpp b/include/grl/kuka/Kuka.hpp index aa48663..5c55e40 100644 --- a/include/grl/kuka/Kuka.hpp +++ b/include/grl/kuka/Kuka.hpp @@ -236,7 +236,7 @@ class KukaUDP { }; enum ThreadingRunMode { run_manually = 0, run_automatically = 1 }; - + /// a fixed-size collection of heterogeneous values. typedef std::tuple Params; diff --git a/include/grl/kuka/KukaDriver.hpp b/include/grl/kuka/KukaDriver.hpp index af0de4d..62b9e62 100644 --- a/include/grl/kuka/KukaDriver.hpp +++ b/include/grl/kuka/KukaDriver.hpp @@ -321,8 +321,7 @@ namespace grl { namespace robot { namespace arm { armState_.clearCommands(); boost::copy(range, std::back_inserter(armState_.commandedPosition)); boost::copy(range, std::back_inserter(armState_.commandedPosition_goal)); - - std::cout << "set commandedpos:" << armState_.commandedPosition; + // std::cout << "set commandedpos:" << armState_.commandedPosition; } /** diff --git a/include/grl/kuka/KukaFRIdriver.hpp b/include/grl/kuka/KukaFRIdriver.hpp index 2f963ae..99ffcc1 100644 --- a/include/grl/kuka/KukaFRIdriver.hpp +++ b/include/grl/kuka/KukaFRIdriver.hpp @@ -940,16 +940,20 @@ class KukaFRIClientDataDriver /// so that the low level update algorithm can change. /// @todo add getter for number of milliseconds between fri updates (1-5) aka /// sync_period aka send_period aka ms per tick + +/// std::enable_shared_from_this allows an object t that is currently managed by +/// a std::shared_ptr named pt to safely generate additional std::shared_ptr instances pt1, pt2, ... +/// that all share ownership of t with pt. template class KukaFRIdriver : public std::enable_shared_from_this< KukaFRIdriver>, public KukaUDP { public: - using KukaUDP::ParamIndex; + using KukaUDP::ParamIndex; // enum, define some connection parameters, such as ip, port... using KukaUDP::ThreadingRunMode; - using KukaUDP::Params; - using KukaUDP::defaultParams; + using KukaUDP::Params; // std::tuple, contains the information needed to connect to the robot. + using KukaUDP::defaultParams; // A method to assign Params with defaut values. KukaFRIdriver(Params params = defaultParams()) : params_(params) {} @@ -971,6 +975,7 @@ class KukaFRIdriver : public std::enable_shared_from_this< params_ = params; // keep driver threads from exiting immediately after creation, because they // have work to do! + // boost::asio::io_service::work: Constructor notifies the io_service that work is starting. device_driver_workP_.reset( new boost::asio::io_service::work(device_driver_io_service)); @@ -1176,6 +1181,7 @@ class KukaFRIdriver : public std::enable_shared_from_this< << "\n Consecutive Successes: " << m_attemptedCommunicationConsecutiveSuccessCount << "\n"; m_attemptedCommunicationConsecutiveSuccessCount = 0; + /// @todo TODO(ahundt) Add time information from update_state call here for debugging purposes /// @todo TODO(ahundt) should the results of getlatest state even be possible to call /// without receiving real data? should the library change? /// @todo TODO(ahundt) use spdlog library instead of cerr? @@ -1337,6 +1343,7 @@ class KukaFRIdriver : public std::enable_shared_from_this< volatile std::size_t m_attemptedCommunicationConsecutiveSuccessCount = 0; boost::asio::io_service device_driver_io_service; + // The work class is used to inform the io_service when work starts and finishes. std::unique_ptr device_driver_workP_; std::unique_ptr driver_threadP; std::shared_ptr< diff --git a/include/grl/sensor/FusionTrackLogAndTrack.hpp b/include/grl/sensor/FusionTrackLogAndTrack.hpp index ef2276a..e4fba59 100644 --- a/include/grl/sensor/FusionTrackLogAndTrack.hpp +++ b/include/grl/sensor/FusionTrackLogAndTrack.hpp @@ -244,7 +244,7 @@ class FusionTrackLogAndTrack : public std::enable_shared_from_this(save_fbbP->GetBufferPointer()), save_fbbP->GetSize(), write_binary_stream); - /// TODO(ahundt) replace cout with proper spdlog and vrep banner notification + /// TODO(ahFusionTrackLogAndTrackundt) replace cout with proper spdlog and vrep banner notification #ifdef HAVE_spdlog lambdaLoggerP->info("filename: ", filename, " verifier success: ", success); #else // HAVE_spdlog @@ -460,7 +460,7 @@ class FusionTrackLogAndTrack : public std::enable_shared_from_this +volatile std::sig_atomic_t signalStatusG = 0; + +// called when the user presses ctrl+c +void signal_handler(int signal) +{ + signalStatusG = signal; +} using boost::asio::ip::udp; @@ -55,7 +62,23 @@ int main(int argc, char* argv[]) } grl::periodic<> callIfMinPeriodPassed; + // defines how the robot will move when this test is executed HowToMove howToMove = HowToMove::absolute_position_with_relative_rotation;//HowToMove::absolute_position; HowToMove::relative_position; + + // change which part of the driver is being tested. + // It is divided into several levels of functionality + // and the java program you need to execute varies based on + // which selection you make. + // + // Run GRL_Driver on the Kuka teach pendant with: + // + // DriverToUse::kuka_driver_high_level_class + // + // Run FRI_HoldsPosition_command on the KUKA teach pendant with: + // + // DriverToUse::low_level_fri_function, + // DriverToUse::low_level_fri_class, + // DriverToUse driverToUse = DriverToUse::kuka_driver_high_level_class; try @@ -79,12 +102,12 @@ int main(int argc, char* argv[]) remoteport = std::string(argv[4]); } - std::cout << "using: " << argv[0] << " " << localhost << " " << localport << " " << remotehost << " " << remoteport << "\n"; + std::cout << "using: " << argv[0] << " " << localhost << " " << localport << " " << remotehost << " " << remoteport << "\n"; // A single class for an I/O service object. boost::asio::io_service io_service; - std::shared_ptr friData(std::make_shared(7)); - /// std::chrono::time_point startTime; + std::shared_ptr friData(std::make_shared(7)); + /// std::chrono::time_point startTime; cartographer::common::Time startTime; BOOST_VERIFY(friData); @@ -93,7 +116,9 @@ int main(int argc, char* argv[]) double delta_sum = 0; /// consider moving joint angles based on time int joint_to_move = 6; - loggerPG->warn("WARNING: YOU COULD DAMAGE OR DESTROY YOUR KUKA ROBOT {}{}{}{}{}{}", + loggerPG->warn("WARNING: THIS PROGRAM WILL ACTUALLY MOVE YOUR KUKA ROBOT!!!", + "YOU COULD INJURE YOURSELF, THE OTHERS AROUND YOU, ", + "AND DAMAGE OR DESTROY YOUR KUKA ROBOT. TAKE APPROPRIATE PRECAUTIONS. YOU ARE RESPONSIBLE. {}{}{}{}{}{}", "if joint angle delta variable is too large with respect to ", "the time it takes to go around the loop and change it. ", "Current delta (radians/update): ", delta, " Joint to move: ", joint_to_move); @@ -129,7 +154,6 @@ int main(int argc, char* argv[]) remotehost, remoteport/*,4 ms per tick*/, grl::robot::arm::KukaFRIClientDataDriver::run_automatically)); - } std::shared_ptr socketP; @@ -171,13 +195,14 @@ int main(int argc, char* argv[]) kukaDriverP->set(grl::flatbuffer::ArmState::MoveArmJointServo); kukaDriverP->set(goal_position_command_time_duration,grl::time_duration_command_tag()); std::cout << "KUKA COMMAND MODE: " << std::get(params) << "\n"; + /// kukaDriverP->start_recording(); } - - - unsigned int num_missed = 0; - - for (std::size_t i = 0;;++i) { + std::size_t num_missed = 0; + std::size_t maximum_consecutive_missed_updates_before_warning = 100000; + std::size_t maximum_consecutive_missed_updates_before_exit = 100000000; + // run until the user kills the program with ctrl+c + for (std::size_t i = 0; !signalStatusG; ++i) { /// Save the interpolated joint position from the previous update as the base for some motions /// The interpolated position is where the JAVA side is commanding, @@ -238,10 +263,17 @@ int main(int argc, char* argv[]) // but we can't process the new data so try updating again immediately. if(!haveNewData && !recv_bytes_transferred) { - if(driverToUse == DriverToUse::low_level_fri_class) std::this_thread::sleep_for(std::chrono::milliseconds(1)); + if(driverToUse == DriverToUse::low_level_fri_class || + driverToUse == DriverToUse::kuka_driver_high_level_class) + { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } ++num_missed; - if(num_missed>10000) { + if(num_missed > maximum_consecutive_missed_updates_before_warning) { loggerPG->warn("No new data for ", num_missed, " milliseconds."); + continue; + } else if(num_missed > maximum_consecutive_missed_updates_before_exit) { + loggerPG->warn("No new data for ", num_missed, " milliseconds. Max time limit hit, Exiting..."); break; } else { continue; @@ -323,5 +355,6 @@ int main(int argc, char* argv[]) // Release and close all loggers spdlog::drop_all(); + std::cout << "End of the program" << std::endl; return 0; } diff --git a/test/kukaiiwaTest.cpp b/test/kukaiiwaTest.cpp index fa01008..f62aca4 100644 --- a/test/kukaiiwaTest.cpp +++ b/test/kukaiiwaTest.cpp @@ -50,6 +50,26 @@ int charP_size(const char* buf) return Size; } +using boost::asio::ip::udp; + + +enum { max_length = 1024 }; + +enum class HowToMove +{ + remain_stationary, + absolute_position, + relative_position, + absolute_position_with_relative_rotation +}; + +enum class DriverToUse +{ + low_level_fri_function, + low_level_fri_class, + kuka_driver_high_level_class +}; + BOOST_AUTO_TEST_SUITE(KukaTest) BOOST_AUTO_TEST_CASE(runRepeatedly) @@ -62,28 +82,99 @@ BOOST_AUTO_TEST_CASE(runRepeatedly) bool OK = true; bool debug = true; if (debug) std::cout << "starting KukaTest" << std::endl; - // std::shared_ptr fbbP; - flatbuffers::FlatBufferBuilder fbb; - // fbbP = std::make_shared(); - // std::shared_ptr friData(std::make_shared(7)); - // std::shared_ptr> highLevelDriverClassP; + + + std::string localhost("192.170.10.100"); + std::string localport("30200"); + std::string remotehost("192.170.10.2"); + std::string remoteport("30200"); + // A single class for an I/O service object. + boost::asio::io_service io_service; + std::shared_ptr friData(std::make_shared(7)); + cartographer::common::Time startTime; + BOOST_VERIFY(friData); + std::vector ipoJointPos(7,0); + std::vector jointOffset(7,0); // length 7, value 0 + boost::container::static_vector jointStateToCommand(7,0); + grl::robot::arm::KukaState armState; + std::unique_ptr lowLevelStepAlgorithmP; + + // Need to tell the system how long in milliseconds it has to reach the goal + // or it will never move! + std::size_t goal_position_command_time_duration = 4; + lowLevelStepAlgorithmP.reset(new grl::robot::arm::LinearInterpolation()); + + std::shared_ptr> highLevelDriverClassP; + DriverToUse driverToUse = DriverToUse::kuka_driver_high_level_class; + if(driverToUse == DriverToUse::low_level_fri_class) + { + /// @todo TODO(ahundt) BUG: Need way to supply time to reach specified goal for position control and eliminate this allocation internally in the kuka driver. See similar comment in KukaFRIDriver.hpp + /// IDEA: PASS A LOW LEVEL STEP ALGORITHM PARAMS OBJECT ON EACH UPDATE AND ONLY ONE INSTANCE OF THE ALGORITHM OBJECT ITSELF + highLevelDriverClassP = std::make_shared>(io_service, + std::make_tuple("KUKA_LBR_IIWA_14_R820", + localhost, + localport, + remotehost, + remoteport/*,4 ms per tick*/, + grl::robot::arm::KukaFRIClientDataDriver::run_automatically)); + } + std::shared_ptr socketP; + + std::shared_ptr kukaDriverP; double duration = boost::chrono::high_resolution_clock::now().time_since_epoch().count(); - if (debug) std::cout << "duration: " << duration << std::endl; - grl::robot::arm::KukaDriver::Params params = std::make_tuple( - "Robotiiwa" , // RobotName, - "KUKA_LBR_IIWA_14_R820" , // RobotModel (options are KUKA_LBR_IIWA_14_R820, KUKA_LBR_IIWA_7_R800) - "0.0.0.0" , // LocalUDPAddress - "30010" , // LocalUDPPort - "172.31.1.147" , // RemoteUDPAddress - "192.170.10.100" , // LocalHostKukaKoniUDPAddress, - "30200" , // LocalHostKukaKoniUDPPort, - "192.170.10.2" , // RemoteHostKukaKoniUDPAddress, - "30200" , // RemoteHostKukaKoniUDPPort - "JAVA" , // KukaCommandMode (options are FRI, JAVA) - "FRI" // KukaMonitorMode (options are FRI, JAVA) + if(driverToUse == DriverToUse::kuka_driver_high_level_class) + { + grl::robot::arm::KukaDriver::Params params = std::make_tuple( + "Robotiiwa" , // RobotName, + "KUKA_LBR_IIWA_14_R820" , // RobotModel (options are KUKA_LBR_IIWA_14_R820, KUKA_LBR_IIWA_7_R800) + "0.0.0.0" , // LocalUDPAddress + "30010" , // LocalUDPPort + "172.31.1.147" , // RemoteUDPAddress + "192.170.10.100" , // LocalHostKukaKoniUDPAddress, + "30200" , // LocalHostKukaKoniUDPPort, + remotehost , // RemoteHostKukaKoniUDPAddress, + remoteport , // RemoteHostKukaKoniUDPPort + "FRI" , // KukaCommandMode (options are FRI, JAVA) + "FRI" // KukaMonitorMode (options are FRI, JAVA) ); + /// @todo TODO(ahundt) Currently assumes ip address + kukaDriverP=std::make_shared(params); + kukaDriverP->construct(); + // Default to joint servo mode for commanding motion + kukaDriverP->set(grl::flatbuffer::ArmState::MoveArmJointServo); + kukaDriverP->set(goal_position_command_time_duration,grl::time_duration_command_tag()); + std::cout << "KUKA COMMAND MODE: " << std::get(params) << "\n"; + } + + unsigned int num_missed = 0; + + for (std::size_t i = 0; !signalStatusG; ++i) { + + /// perform the update step, receiving and sending data to/from the arm + boost::system::error_code send_ec, recv_ec; + std::size_t send_bytes_transferred = 0, recv_bytes_transferred = 0; + bool haveNewData = false; + grl::TimeEvent time_event_stamp; + if(driverToUse == DriverToUse::kuka_driver_high_level_class) + { + kukaDriverP->set( jointStateToCommand, grl::revolute_joint_angle_open_chain_command_tag()); + kukaDriverP->run_one(); + //kukaDriverP->get(); + } + + /// use the interpolated joint position from the previous update as the base + /// The interpolated position is where the java side is commanding, + /// or the fixed starting position with a hold position command on the java side. + if(i!=0 && friData){ + grl::robot::arm::copy(friData->monitoringMsg,ipoJointPos.begin(),grl::revolute_joint_angle_interpolated_open_chain_state_tag()); + } + + + } + std::shared_ptr fbbP; + fbbP = std::make_shared(); std::string RobotName("Robotiiwa" ); std::string destination("where this message is going (URI)"); std::string source("where this message came from (URI)"); @@ -108,221 +199,222 @@ BOOST_AUTO_TEST_CASE(runRepeatedly) std::vector joint_VelocityRel_(7, 1.0); bool updateMinimumTrajectoryExecutionTime = false; double minimumTrajectoryExecutionTime = 4; - std::size_t goal_position_command_time_duration = 4; - std::cout << "KUKA COMMAND MODE: " << std::get(params) << "\n"; - int64_t sequenceNumber = 0; - //Cartesian Impedance Values - grl::flatbuffer::Vector3d cart_stiffness_trans_ = grl::flatbuffer::Vector3d(500,500,500); - grl::flatbuffer::EulerRotation cart_stifness_rot_ = grl::flatbuffer::EulerRotation(200,200,200,grl::flatbuffer::EulerOrder::xyz); - grl::flatbuffer::Vector3d cart_damping_trans_ = grl::flatbuffer::Vector3d(0.3,0.3,0.3); - grl::flatbuffer::EulerRotation cart_damping_rot_ = grl::flatbuffer::EulerRotation(0.3,0.3,0.3,grl::flatbuffer::EulerOrder::xyz); - grl::flatbuffer::EulerPose cart_stiffness_ = grl::flatbuffer::EulerPose(cart_stiffness_trans_, cart_stifness_rot_); - grl::flatbuffer::EulerPose cart_damping_ = grl::flatbuffer::EulerPose(cart_damping_trans_, cart_damping_rot_); - - grl::flatbuffer::EulerPose cart_max_path_deviation_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(1000,1000,100), grl::flatbuffer::EulerRotation(5.,5.,5., grl::flatbuffer::EulerOrder::xyz)); - grl::flatbuffer::EulerPose cart_max_ctrl_vel_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(1000,1000,1000), grl::flatbuffer::EulerRotation(6.3,6.3,6.3, grl::flatbuffer::EulerOrder::xyz)); - grl::flatbuffer::EulerPose cart_max_ctrl_force_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(200,200,200), grl::flatbuffer::EulerRotation(200.,200.,200., grl::flatbuffer::EulerOrder::xyz)); - double nullspace_stiffness_ = 0.1; - double nullspace_damping_ = 0.1; - - ::MessageHeader messageHeader{1,2,3}; - ::RobotInfo robotInfo{true, 7, true, ::SafetyState::SafetyState_NORMAL_OPERATION}; - ::JointValues strjointValues; - std::vector jointValues(7, 0.1); - boost::container::static_vector jointstate; - for(int i = 0; i<7; i++) { - jointstate.push_back(jointValues[i]); - } - /// Make reference to void map_repeatedDouble() in pb_frimessages_callback.c and pb_frimessages_callbacks.h - strjointValues.value = *makeupJointValues(jointValues); - grl::robot::arm::KukaState::joint_state kukajointstate(jointstate); - - //////////////////////////////////////////////////////////// - // Double check the initialization of this parameter ////// - ////////////////////////////////////////////////////////// - grl::robot::arm::KukaState armState; - armState.position = kukajointstate; - armState.torque = kukajointstate; - armState.externalTorque = kukajointstate; - armState.commandedPosition = kukajointstate; - armState.commandedTorque = kukajointstate; - armState.ipoJointPosition = kukajointstate; - armState.ipoJointPositionOffsets = kukajointstate; - armState.commandedCartesianWrenchFeedForward = kukajointstate; - armState.externalForce = kukajointstate; - armState.sessionState = grl::toFlatBuffer(connectionInfo.sessionState); - armState.connectionQuality = grl::toFlatBuffer(connectionInfo.quality); - armState.safetyState = grl::toFlatBuffer(::SafetyState::SafetyState_NORMAL_OPERATION); - armState.operationMode = grl::toFlatBuffer(OperationMode::OperationMode_TEST_MODE_1); - armState.driveState = grl::toFlatBuffer(::DriveState::DriveState_OFF); - - ::MessageMonitorData messageMonitorData{ - true, strjointValues, - true, strjointValues, - true, strjointValues, - true, strjointValues, - true, strjointValues, - true, ::TimeStamp{1,2} - }; - ::MessageIpoData ipoData{ - true, strjointValues, - true, clientCommandMode, - true, overlayType, - true, 4.0 - }; - ::Transformation transformation{ "Transformation", 12, {1,2,3,4,5,6,7,8,9,10,11,12}, true, ::TimeStamp{3,4}}; - /// Transformation requestedTransformations[5] = { transformation, transformation, transformation, transformation, transformation }; - ::MessageEndOf endOfMessageData{true, 32, true, ::Checksum{true, 32}}; - ::FRIMonitoringMessage friMonitoringMessage { - messageHeader, - true, robotInfo, - true, messageMonitorData, - true, connectionInfo, - true, ipoData, - 5, - { transformation, transformation, transformation, transformation, transformation }, - true, endOfMessageData}; - grl::TimeEvent time_event_stamp; - std::vector> kukaiiwaStateVec; - std::size_t builder_size_bytes = 0; - std::size_t previous_size = 0; - - - while(!signalStatusG && OK && builder_size_bytes < single_buffer_limit_bytes ) - { - // grl::robot::arm::copy(friMonitoringMessage, armState); - flatbuffers::Offset controlState = grl::toFlatBuffer(fbb, basename, sequenceNumber++, duration, armState, armControlMode); - flatbuffers::Offset setCartesianImpedance = grl::toFlatBuffer(fbb, cart_stiffness_, cart_damping_, - nullspace_stiffness_, nullspace_damping_, cart_max_path_deviation_, cart_max_ctrl_vel_, cart_max_ctrl_force_, max_control_force_stop_); - flatbuffers::Offset setJointImpedance = grl::toFlatBuffer(fbb, joint_stiffness_, joint_damping_); - flatbuffers::Offset setSmartServo = grl::toFlatBuffer(fbb, joint_AccelerationRel_, joint_VelocityRel_, updateMinimumTrajectoryExecutionTime, minimumTrajectoryExecutionTime); - flatbuffers::Offset FRIConfig = grl::toFlatBuffer(fbb, overlayType, connectionInfo, false, 3501, false, 3502); - - std::vector> tools; - std::vector> processData; - - for(int i=0; i<7; i++) { - std::string linkname = "Link" + std::to_string(i); - std::string parent = i==0?"Base":("Link" + std::to_string(i-1)); - grl::flatbuffer::Pose pose = grl::flatbuffer::Pose(grl::flatbuffer::Vector3d(i,i,i), grl::flatbuffer::Quaternion(i,i,i,i)); - grl::flatbuffer::Inertia inertia = grl::flatbuffer::Inertia(1, pose, 1, 2, 3, 4, 5, 6); - flatbuffers::Offset linkObject = grl::toFlatBuffer(fbb, linkname, parent, pose, inertia); - tools.push_back(linkObject); - processData.push_back( - grl::toFlatBuffer(fbb, - "dataType"+ std::to_string(i), - "defaultValue"+ std::to_string(i), - "displayName"+ std::to_string(i), - "id"+ std::to_string(i), - "min"+ std::to_string(i), - "max"+ std::to_string(i), - "unit"+ std::to_string(i), - "value"+ std::to_string(i))); + bool logdata = false; + if (logdata == true) { + int64_t sequenceNumber = 0; + //Cartesian Impedance Values + grl::flatbuffer::Vector3d cart_stiffness_trans_ = grl::flatbuffer::Vector3d(500,500,500); + grl::flatbuffer::EulerRotation cart_stifness_rot_ = grl::flatbuffer::EulerRotation(200,200,200,grl::flatbuffer::EulerOrder::xyz); + grl::flatbuffer::Vector3d cart_damping_trans_ = grl::flatbuffer::Vector3d(0.3,0.3,0.3); + grl::flatbuffer::EulerRotation cart_damping_rot_ = grl::flatbuffer::EulerRotation(0.3,0.3,0.3,grl::flatbuffer::EulerOrder::xyz); + grl::flatbuffer::EulerPose cart_stiffness_ = grl::flatbuffer::EulerPose(cart_stiffness_trans_, cart_stifness_rot_); + grl::flatbuffer::EulerPose cart_damping_ = grl::flatbuffer::EulerPose(cart_damping_trans_, cart_damping_rot_); + + grl::flatbuffer::EulerPose cart_max_path_deviation_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(1000,1000,100), grl::flatbuffer::EulerRotation(5.,5.,5., grl::flatbuffer::EulerOrder::xyz)); + grl::flatbuffer::EulerPose cart_max_ctrl_vel_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(1000,1000,1000), grl::flatbuffer::EulerRotation(6.3,6.3,6.3, grl::flatbuffer::EulerOrder::xyz)); + grl::flatbuffer::EulerPose cart_max_ctrl_force_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(200,200,200), grl::flatbuffer::EulerRotation(200.,200.,200., grl::flatbuffer::EulerOrder::xyz)); + double nullspace_stiffness_ = 0.1; + double nullspace_damping_ = 0.1; + + ::MessageHeader messageHeader{1,2,3}; + ::RobotInfo robotInfo{true, 7, true, ::SafetyState::SafetyState_NORMAL_OPERATION}; + ::JointValues strjointValues; + std::vector jointValues(7, 0.1); + boost::container::static_vector jointstate; + for(int i = 0; i<7; i++) { + jointstate.push_back(jointValues[i]); + } + /// Make reference to void map_repeatedDouble() in pb_frimessages_callback.c and pb_frimessages_callbacks.h + strjointValues.value = *makeupJointValues(jointValues); + grl::robot::arm::KukaState::joint_state kukajointstate(jointstate); + + //////////////////////////////////////////////////////////// + // Double check the initialization of this parameter ////// + ////////////////////////////////////////////////////////// + + armState.position = kukajointstate; + armState.torque = kukajointstate; + armState.externalTorque = kukajointstate; + armState.commandedPosition = kukajointstate; + armState.commandedTorque = kukajointstate; + armState.ipoJointPosition = kukajointstate; + armState.ipoJointPositionOffsets = kukajointstate; + armState.commandedCartesianWrenchFeedForward = kukajointstate; + armState.externalForce = kukajointstate; + armState.sessionState = grl::toFlatBuffer(connectionInfo.sessionState); + armState.connectionQuality = grl::toFlatBuffer(connectionInfo.quality); + armState.safetyState = grl::toFlatBuffer(::SafetyState::SafetyState_NORMAL_OPERATION); + armState.operationMode = grl::toFlatBuffer(OperationMode::OperationMode_TEST_MODE_1); + armState.driveState = grl::toFlatBuffer(::DriveState::DriveState_OFF); + + ::MessageMonitorData messageMonitorData{ + true, strjointValues, + true, strjointValues, + true, strjointValues, + true, strjointValues, + true, strjointValues, + true, ::TimeStamp{1,2} + }; + ::MessageIpoData ipoData{ + true, strjointValues, + true, clientCommandMode, + true, overlayType, + true, 4.0 + }; + ::Transformation transformation{ "Transformation", 12, {1,2,3,4,5,6,7,8,9,10,11,12}, true, ::TimeStamp{3,4}}; + /// Transformation requestedTransformations[5] = { transformation, transformation, transformation, transformation, transformation }; + ::MessageEndOf endOfMessageData{true, 32, true, ::Checksum{true, 32}}; + ::FRIMonitoringMessage friMonitoringMessage { + messageHeader, + true, robotInfo, + true, messageMonitorData, + true, connectionInfo, + true, ipoData, + 5, + { transformation, transformation, transformation, transformation, transformation }, + true, endOfMessageData}; + grl::TimeEvent time_event_stamp; + std::vector> kukaiiwaStateVec; + std::size_t builder_size_bytes = 0; + std::size_t previous_size = 0; + + + while(!signalStatusG && OK && builder_size_bytes < single_buffer_limit_bytes ) + { + // grl::robot::arm::copy(friMonitoringMessage, armState); + flatbuffers::Offset controlState = grl::toFlatBuffer(*fbbP, basename, sequenceNumber++, duration, armState, armControlMode); + flatbuffers::Offset setCartesianImpedance = grl::toFlatBuffer(*fbbP, cart_stiffness_, cart_damping_, + nullspace_stiffness_, nullspace_damping_, cart_max_path_deviation_, cart_max_ctrl_vel_, cart_max_ctrl_force_, max_control_force_stop_); + flatbuffers::Offset setJointImpedance = grl::toFlatBuffer(*fbbP, joint_stiffness_, joint_damping_); + flatbuffers::Offset setSmartServo = grl::toFlatBuffer(*fbbP, joint_AccelerationRel_, joint_VelocityRel_, updateMinimumTrajectoryExecutionTime, minimumTrajectoryExecutionTime); + flatbuffers::Offset FRIConfig = grl::toFlatBuffer(*fbbP, overlayType, connectionInfo, false, 3501, false, 3502); + + std::vector> tools; + std::vector> processData; + + for(int i=0; i<7; i++) { + std::string linkname = "Link" + std::to_string(i); + std::string parent = i==0?"Base":("Link" + std::to_string(i-1)); + grl::flatbuffer::Pose pose = grl::flatbuffer::Pose(grl::flatbuffer::Vector3d(i,i,i), grl::flatbuffer::Quaternion(i,i,i,i)); + grl::flatbuffer::Inertia inertia = grl::flatbuffer::Inertia(1, pose, 1, 2, 3, 4, 5, 6); + flatbuffers::Offset linkObject = grl::toFlatBuffer(*fbbP, linkname, parent, pose, inertia); + tools.push_back(linkObject); + processData.push_back( + grl::toFlatBuffer(*fbbP, + "dataType"+ std::to_string(i), + "defaultValue"+ std::to_string(i), + "displayName"+ std::to_string(i), + "id"+ std::to_string(i), + "min"+ std::to_string(i), + "max"+ std::to_string(i), + "unit"+ std::to_string(i), + "value"+ std::to_string(i))); + } + + + // copy the state data into a more accessible object + /// TODO(ahundt) switch from this copy to a non-deprecated call + + flatbuffers::Offset kukaiiwaArmConfiguration = grl::toFlatBuffer( + *fbbP, + RobotName, + commandInterface, + monitorInterface, + clientCommandMode, + overlayType, + controlMode, + setCartesianImpedance, + setJointImpedance, + setSmartServo, + FRIConfig, + tools, + processData, + "currentMotionCenter", + true); + + flatbuffers::Offset friMessageLog = grl::toFlatBuffer( + *fbbP, + ::FRISessionState::FRISessionState_IDLE, + ::FRIConnectionQuality::FRIConnectionQuality_POOR, + controlMode, + friMonitoringMessage, + time_event_stamp); + grl::flatbuffer::Wrench cartesianWrench{grl::flatbuffer::Vector3d(1,1,1),grl::flatbuffer::Vector3d(1,1,1),grl::flatbuffer::Vector3d(1,1,1)}; + + + flatbuffers::Offset jointStatetab = grl::toFlatBuffer(*fbbP, jointValues, jointValues, jointValues, jointValues); + grl::flatbuffer::Pose cartesianFlangePose = grl::flatbuffer::Pose(grl::flatbuffer::Vector3d(1,1,1), grl::flatbuffer::Quaternion(2,3,4,5)); + flatbuffers::Offset kukaiiwaMonitorState = grl::toFlatBuffer( + *fbbP, + jointStatetab, // flatbuffers::Offset &measuredState, + cartesianFlangePose, + jointStatetab, // flatbuffers::Offset &jointStateReal, + jointStatetab, // flatbuffers::Offset &jointStateInterpolated, + jointStatetab, // flatbuffers::Offset &externalState, + ::FRISessionState::FRISessionState_IDLE, + ::OperationMode::OperationMode_TEST_MODE_1, + cartesianWrench); + std::vector torqueSensorLimits(7,0.5); + flatbuffers::Offset monitorConfig = grl::toFlatBuffer( + *fbbP, + "hardvareVersion", + torqueSensorLimits, + true, + false, + processData); + flatbuffers::Offset kukaiiwaState = grl::toFlatBuffer( + *fbbP, + RobotName, + destination, + source, + duration, + true, controlState, + true, kukaiiwaArmConfiguration, + true, kukaiiwaMonitorState, + false, monitorConfig, + friMessageLog); + + kukaiiwaStateVec.push_back(kukaiiwaState); + + builder_size_bytes = fbbP->GetSize(); + std::size_t newData = builder_size_bytes - previous_size; + previous_size = builder_size_bytes; + // std::cout<< "single message data size (bytes): " << newData << " Buffer size: " << builder_size_bytes/MegaByte <<" MB" << std::endl; } + std::cout<< "kukaiiwaStateVec:" << kukaiiwaStateVec.size() << std::endl; - // copy the state data into a more accessible object - /// TODO(ahundt) switch from this copy to a non-deprecated call - - flatbuffers::Offset kukaiiwaArmConfiguration = grl::toFlatBuffer( - fbb, - RobotName, - commandInterface, - monitorInterface, - clientCommandMode, - overlayType, - controlMode, - setCartesianImpedance, - setJointImpedance, - setSmartServo, - FRIConfig, - tools, - processData, - "currentMotionCenter", - true); - - flatbuffers::Offset friMessageLog = grl::toFlatBuffer( - fbb, - ::FRISessionState::FRISessionState_IDLE, - ::FRIConnectionQuality::FRIConnectionQuality_POOR, - controlMode, - friMonitoringMessage, - time_event_stamp); - grl::flatbuffer::Wrench cartesianWrench{grl::flatbuffer::Vector3d(1,1,1),grl::flatbuffer::Vector3d(1,1,1),grl::flatbuffer::Vector3d(1,1,1)}; - - - flatbuffers::Offset jointStatetab = grl::toFlatBuffer(fbb, jointValues, jointValues, jointValues, jointValues); - grl::flatbuffer::Pose cartesianFlangePose = grl::flatbuffer::Pose(grl::flatbuffer::Vector3d(1,1,1), grl::flatbuffer::Quaternion(2,3,4,5)); - flatbuffers::Offset kukaiiwaMonitorState = grl::toFlatBuffer( - fbb, - jointStatetab, // flatbuffers::Offset &measuredState, - cartesianFlangePose, - jointStatetab, // flatbuffers::Offset &jointStateReal, - jointStatetab, // flatbuffers::Offset &jointStateInterpolated, - jointStatetab, // flatbuffers::Offset &externalState, - ::FRISessionState::FRISessionState_IDLE, - ::OperationMode::OperationMode_TEST_MODE_1, - cartesianWrench); - std::vector torqueSensorLimits(7,0.5); - flatbuffers::Offset monitorConfig = grl::toFlatBuffer( - fbb, - "hardvareVersion", - torqueSensorLimits, - true, - false, - processData); - flatbuffers::Offset kukaiiwaState = grl::toFlatBuffer( - fbb, - RobotName, - destination, - source, - duration, - true, controlState, - true, kukaiiwaArmConfiguration, - true, kukaiiwaMonitorState, - false, monitorConfig, - friMessageLog); - - kukaiiwaStateVec.push_back(kukaiiwaState); - - builder_size_bytes = fbb.GetSize(); - std::size_t newData = builder_size_bytes - previous_size; - previous_size = builder_size_bytes; - std::cout<< "single message data size (bytes): " << newData << " Buffer size: " << builder_size_bytes/MegaByte <<" MB" << std::endl; - } - - std::cout<< "kukaiiwaStateVec:" << kukaiiwaStateVec.size() << std::endl; - - flatbuffers::Offset kukaStates = grl::toFlatBuffer(fbb, kukaiiwaStateVec); + flatbuffers::Offset kukaStates = grl::toFlatBuffer(*fbbP, kukaiiwaStateVec); - grl::flatbuffer::FinishKUKAiiwaStatesBuffer(fbb, kukaStates); + grl::flatbuffer::FinishKUKAiiwaStatesBuffer(*fbbP, kukaStates); - uint8_t *buf = fbb.GetBufferPointer(); - std::size_t bufsize = fbb.GetSize(); + uint8_t *buf = fbbP->GetBufferPointer(); + std::size_t bufsize = fbbP->GetSize(); - /// To expand the capacity of a single buffer, _max_tables is set to 10000000 - flatbuffers::uoffset_t _max_depth = 64; - flatbuffers::uoffset_t _max_tables = 10000000; - flatbuffers::Verifier verifier(buf, bufsize, _max_depth, _max_tables); - OK = OK && grl::flatbuffer::VerifyKUKAiiwaStatesBuffer(verifier); - assert(OK && "VerifyKUKAiiwaStatesBuffer"); + /// To expand the capacity of a single buffer, _max_tables is set to 10000000 + flatbuffers::uoffset_t _max_depth = 64; + flatbuffers::uoffset_t _max_tables = 10000000; + flatbuffers::Verifier verifier(buf, bufsize, _max_depth, _max_tables); + OK = OK && grl::flatbuffer::VerifyKUKAiiwaStatesBuffer(verifier); + assert(OK && "VerifyKUKAiiwaStatesBuffer"); - std::cout << "Buffer size: " << bufsize << std::endl; + std::cout << "Buffer size: " << bufsize << std::endl; - std::string binary_file_path = "Kuka_test_binary.iiwa"; - std::string json_file_path = "kuka_test_text.json"; + std::string binary_file_path = "Kuka_test_binary.iiwa"; + std::string json_file_path = "kuka_test_text.json"; - // Get the current working directory - std::string fbs_filename("KUKAiiwa.fbs"); + // Get the current working directory + std::string fbs_filename("KUKAiiwa.fbs"); - OK = OK && grl::SaveFlatBufferFile( - buf, - fbb.GetSize(), - binary_file_path, - fbs_filename, - json_file_path); - assert(OK && "SaveFlatBufferFile"); + OK = OK && grl::SaveFlatBufferFile( + buf, + fbbP->GetSize(), + binary_file_path, + fbs_filename, + json_file_path); + assert(OK && "SaveFlatBufferFile"); + } std::cout << "End of the program" << std::endl; } From 65d484f233b1b7768717718c65e4e9c600f8cdde Mon Sep 17 00:00:00 2001 From: Chunting Date: Thu, 4 Jan 2018 22:12:14 -0500 Subject: [PATCH 025/102] [NOT FINISHED] Log data in the KukaFRIdriver.hpp --- include/grl/kuka/Kuka.hpp | 3 +- include/grl/kuka/KukaFRIdriver.hpp | 156 +++++++++++++++++++++++++- include/grl/kuka/KukaToFlatbuffer.hpp | 4 +- 3 files changed, 157 insertions(+), 6 deletions(-) diff --git a/include/grl/kuka/Kuka.hpp b/include/grl/kuka/Kuka.hpp index 5c55e40..a80fbba 100644 --- a/include/grl/kuka/Kuka.hpp +++ b/include/grl/kuka/Kuka.hpp @@ -83,8 +83,7 @@ struct KukaState { // which needed to be reimplemented due to licensing restrictions // in the corresponding C++ code flatbuffer::ESessionState sessionState; // KUKA::FRI::ESessionState - flatbuffer::EConnectionQuality - connectionQuality; // KUKA::FRI::EConnectionQuality + flatbuffer::EConnectionQuality connectionQuality; // KUKA::FRI::EConnectionQuality flatbuffer::ESafetyState safetyState; // KUKA::FRI::ESafetyState flatbuffer::EOperationMode operationMode; // KUKA::FRI::EOperationMode flatbuffer::EDriveState driveState; // KUKA::FRI::EDriveState diff --git a/include/grl/kuka/KukaFRIdriver.hpp b/include/grl/kuka/KukaFRIdriver.hpp index 99ffcc1..ced5b89 100644 --- a/include/grl/kuka/KukaFRIdriver.hpp +++ b/include/grl/kuka/KukaFRIdriver.hpp @@ -3,6 +3,10 @@ #ifndef _KUKA_FRI_DRIVER #define _KUKA_FRI_DRIVER +// To call flatbuffer and toflatbuffer functions + +#include "grl/kuka/KukaToFlatbuffer.hpp" + #include #include #include @@ -34,6 +38,9 @@ #include #include + + + struct KukaState; namespace grl { @@ -81,6 +88,8 @@ struct LinearInterpolation { TimeDurationToDestMS }; + // A variable-size array container with fixed capacity. + // A static_vector is a sequence that supports random access to elements, constant time insertion and removal of elements at the end, and linear time insertion and removal of elements at the beginning or in the middle. typedef std::tuple,std::size_t> Params; // extremely conservative default timeframe to reach destination plus no goal position @@ -584,6 +593,8 @@ class KukaFRIClientDataDriver if (!isConnectionEstablished_ || !std::get(latestStateForUser_)) { // no new data, so immediately return results accordingly + // Constructs a tuple object whose elements are references to the arguments in args std::tie (Types&... args) , in the same order. + // This allows a set of objects to act as a tuple, which is especially useful to unpack tuple objects. std::tie(step_alg_params, friData, receive_ec, receive_bytes_transferred, send_ec, send_bytes_transferred, timeEvent) = make_LatestState(step_alg_params,friData); return !haveNewData; @@ -616,6 +627,12 @@ class KukaFRIClientDataDriver // return the latest state to the caller std::tie(step_alg_params,friData, receive_ec, receive_bytes_transferred, send_ec, send_bytes_transferred, timeEvent) = std::move(latestStateForUser_); + // TODO(chunting) consider adding to log buffer here, data is in latestStateForUser_, alternate might be latestClientData + + + + + haveNewData = true; } else if (std::get( validFriDataLatestState)) { @@ -870,6 +887,7 @@ class KukaFRIClientDataDriver return make_valid_LatestState(emptyLowLevelAlgParams,friData); } + Params params_; /// @todo replace with unique_ptr @@ -897,7 +915,7 @@ class KukaFRIClientDataDriver boost::mutex ptrMutex_; typename LowLevelStepAlgorithmType::Params step_alg_params_; -}; +}; /// End of KukaFRIClientDataDriver /// @brief Primary Kuka FRI driver, only talks over realtime network FRI KONI /// ethernet port @@ -1170,6 +1188,7 @@ class KukaFRIdriver : public std::enable_shared_from_this< // } // std::cout << '\n'; + // TODO(chunting) add data to log here, when full write to disk on a separate thread like FusionTrackLogAndTrack } else { m_attemptedCommunicationConsecutiveFailureCount++; std::cerr << "No new FRI data available, is an FRI application running " @@ -1326,6 +1345,139 @@ class KukaFRIdriver : public std::enable_shared_from_this< state = armState; } + + + bool startRecordingDataToFlatBuffer(flatbuffers::FlatBufferBuilder &fbb, std::shared_ptr &friData) + { + FRIMonitoringMessage monitoringMsg = friData->monitoringMsg; + + std::string RobotName("Robotiiwa" ); + std::string destination("where this message is going (URI)"); + std::string source("where this message came from (URI)"); + std::string basename = RobotName; //std::get<0>(params); + bool setArmConfiguration_ = true; // set the arm config first time + bool max_control_force_stop_ = false; + + grl::flatbuffer::ArmState armControlMode = grl::flatbuffer::ArmState::StartArm; + grl::flatbuffer::KUKAiiwaInterface commandInterface = grl::flatbuffer::KUKAiiwaInterface::SmartServo;// KUKAiiwaInterface::SmartServo; + grl::flatbuffer::KUKAiiwaInterface monitorInterface = grl::flatbuffer::KUKAiiwaInterface::FRI; + + + ::ConnectionInfo connectionInfo = monitoringMsg.connectionInfo; + ::RobotInfo robotInfo = monitoringMsg.robotInfo; + ::MessageIpoData ipoData = monitoringMsg.ipoData; + ::ControlMode controlMode = robotInfo.controlMode; + ::ClientCommandMode clientCommandMode = ipoData.clientCommandMode; + ::OverlayType overlayType = ipoData.overlayType; + ::FRISessionState friSessionState = connectionInfo.sessionState; + ::FRIConnectionQuality friConnectionQuality = connectionInfo.quality; + + ::MessageHeader messageHeader = monitoringMsg.header; + + ::MessageMonitorData messageMonitorData = monitoringMsg.monitorData; + + ::Transformation *transformation = new ::Transformation[5]; + + for (int i=0; i<5; i++) { + transformation[i] = monitoringMsg.requestedTransformations[i]; + } + ::MessageEndOf endOfMessageData = monitoringMsg.endOfMessageData; + std::size_t builder_size_bytes = 0; + std::size_t previous_size = 0; + + auto bns = fbb.CreateString(basename); + double duration = boost::chrono::high_resolution_clock::now().time_since_epoch().count(); + + flatbuffers::Offset controlState; + + int64_t sequenceNumber = 0; + + const std::size_t MegaByte = 1024*1024; + // In 32bits, the maximum size of a single flatbuffer is 2GB - 1 (defined in base.h), but actually single_buffer_limit_bytes can't be set beyond 2045 MB, otherwise assert might be launched. + const std::size_t single_buffer_limit_bytes = 2045*MegaByte; + + // @TODO(Chunting) Put all these parameters in a configuration file. + std::vector joint_stiffness_(7, 0.2); + std::vector joint_damping_(7, 0.3); + std::vector joint_AccelerationRel_(7, 0.5); + std::vector joint_VelocityRel_(7, 1.0); + bool updateMinimumTrajectoryExecutionTime = false; + double minimumTrajectoryExecutionTime = 4; + + + //Cartesian Impedance Values + grl::flatbuffer::Vector3d cart_stiffness_trans_ = grl::flatbuffer::Vector3d(500,500,500); + grl::flatbuffer::EulerRotation cart_stifness_rot_ = grl::flatbuffer::EulerRotation(200,200,200,grl::flatbuffer::EulerOrder::xyz); + grl::flatbuffer::Vector3d cart_damping_trans_ = grl::flatbuffer::Vector3d(0.3,0.3,0.3); + grl::flatbuffer::EulerRotation cart_damping_rot_ = grl::flatbuffer::EulerRotation(0.3,0.3,0.3,grl::flatbuffer::EulerOrder::xyz); + grl::flatbuffer::EulerPose cart_stiffness_ = grl::flatbuffer::EulerPose(cart_stiffness_trans_, cart_stifness_rot_); + grl::flatbuffer::EulerPose cart_damping_ = grl::flatbuffer::EulerPose(cart_damping_trans_, cart_damping_rot_); + grl::flatbuffer::EulerPose cart_max_path_deviation_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(1000,1000,100), grl::flatbuffer::EulerRotation(5.,5.,5., grl::flatbuffer::EulerOrder::xyz)); + grl::flatbuffer::EulerPose cart_max_ctrl_vel_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(1000,1000,1000), grl::flatbuffer::EulerRotation(6.3,6.3,6.3, grl::flatbuffer::EulerOrder::xyz)); + grl::flatbuffer::EulerPose cart_max_ctrl_force_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(200,200,200), grl::flatbuffer::EulerRotation(200.,200.,200., grl::flatbuffer::EulerOrder::xyz)); + double nullspace_stiffness_ = 0.1; + double nullspace_damping_ = 0.1; + bool updatePortOnRemote = false; + int16_t portOnRemote = 3501; + bool updatePortOnController = false; + int16_t portOnController = 3502; + + while(builder_size_bytes < single_buffer_limit_bytes ) + { + copy(monitoringMsg, armState); + + armState.sessionState = grl::toFlatBuffer(friSessionState); + armState.connectionQuality = grl::toFlatBuffer(friConnectionQuality); + armState.safetyState = grl::toFlatBuffer(robotInfo.safetyState); + armState.operationMode = grl::toFlatBuffer(robotInfo.operationMode); + // // // This parameter is never used. + armState.driveState = grl::toFlatBuffer(::DriveState::DriveState_OFF); + flatbuffers::Offset controlState = grl::toFlatBuffer(fbb, basename, sequenceNumber++, duration, armState, armControlMode); + flatbuffers::Offset setCartesianImpedance = grl::toFlatBuffer(fbb, cart_stiffness_, cart_damping_, + nullspace_stiffness_, nullspace_damping_, cart_max_path_deviation_, cart_max_ctrl_vel_, cart_max_ctrl_force_, max_control_force_stop_); + flatbuffers::Offset setJointImpedance = grl::toFlatBuffer(fbb, joint_stiffness_, joint_damping_); + // flatbuffers::Offset setSmartServo = grl::toFlatBuffer(*fbbP, joint_AccelerationRel_, joint_VelocityRel_, updateMinimumTrajectoryExecutionTime, minimumTrajectoryExecutionTime); + flatbuffers::Offset FRIConfig = grl::toFlatBuffer(fbb, overlayType, connectionInfo, updatePortOnRemote, portOnRemote, updatePortOnController, portOnController); + std::vector> tools; + std::vector> processData; + // @TODO(Chunting) Initialize Pose with 0, we ought to calculate it later with ::Transformation; + // Also assign the configuration parameters with random values, we can figure them out later. + for(int i=0; i<7; i++) { + std::string linkname = "Link" + std::to_string(i); + std::string parent = i==0?"Base":("Link" + std::to_string(i-1)); + + grl::flatbuffer::Pose pose = grl::flatbuffer::Pose(grl::flatbuffer::Vector3d(0,0,0), grl::flatbuffer::Quaternion(0,0,0,0)); + grl::flatbuffer::Inertia inertia = grl::flatbuffer::Inertia(1, pose, 1, 2, 3, 4, 5, 6); + flatbuffers::Offset linkObject = grl::toFlatBuffer(fbb, linkname, parent, pose, inertia); + tools.push_back(linkObject); + processData.push_back( + grl::toFlatBuffer(fbb, + "dataType"+ std::to_string(i), + "defaultValue"+ std::to_string(i), + "displayName"+ std::to_string(i), + "id"+ std::to_string(i), + "min"+ std::to_string(i), + "max"+ std::to_string(i), + "unit"+ std::to_string(i), + "value"+ std::to_string(i))); + } + + } + + + + return true; + } + + + + + + + + + + // The total number of times the FRI interface has successfully received a UDP // packet // from the robot since this class was initialized. @@ -1356,7 +1508,7 @@ class KukaFRIdriver : public std::enable_shared_from_this< Params params_; std::shared_ptr friData_; -}; +}; /// End of KukaFRIdriver.hpp /// @brief nonmember wrapper function to help integrate KukaFRIdriver objects /// with generic programming interface diff --git a/include/grl/kuka/KukaToFlatbuffer.hpp b/include/grl/kuka/KukaToFlatbuffer.hpp index 5d0ac3a..e2a8eeb 100644 --- a/include/grl/kuka/KukaToFlatbuffer.hpp +++ b/include/grl/kuka/KukaToFlatbuffer.hpp @@ -13,7 +13,7 @@ #include "grl/flatbuffer/LinkObject_generated.h" #include "grl/flatbuffer/Euler_generated.h" #include "grl/flatbuffer/HelperToFlatbuffer.hpp" -#include "KukaFRIdriver.hpp" +// #include "KukaFRIdriver.hpp" #include "Kuka.hpp" #include "KukaFRIalgorithm.hpp" #include @@ -367,7 +367,7 @@ flatbuffers::Offset toFlatBuffer( grl::flatbuffer::ArmState &armControlMode) { switch (armControlMode) { - std::cout<< "C++ KukaJAVAdriver: sending armposition command: " << armState.commandedPosition_goal << std::endl; + // std::cout<< "C++ KukaJAVAdriver: sending armposition command: " << armState.commandedPosition_goal << std::endl; case grl::flatbuffer::ArmState::MoveArmJointServo: { auto armPositionBuffer = fbb.CreateVector(armState.commandedPosition_goal.data(), armState.commandedPosition_goal.size()); auto commandedTorque = fbb.CreateVector(armState.commandedTorque.data(), armState.commandedTorque.size()); From d9d552d058d06738a69db9312546569a61e38024 Mon Sep 17 00:00:00 2001 From: Chunting Date: Fri, 5 Jan 2018 18:38:42 -0500 Subject: [PATCH 026/102] KukaFRIdriver.hpp log data to flatbuffer --- include/grl/kuka/Kuka.hpp | 3 +- include/grl/kuka/KukaFRIdriver.hpp | 181 +++++++++++++++++++++++------ src/ros/CMakeLists.txt | 5 +- test/CMakeLists.txt | 4 +- 4 files changed, 153 insertions(+), 40 deletions(-) diff --git a/include/grl/kuka/Kuka.hpp b/include/grl/kuka/Kuka.hpp index a80fbba..61e4f3c 100644 --- a/include/grl/kuka/Kuka.hpp +++ b/include/grl/kuka/Kuka.hpp @@ -48,8 +48,7 @@ namespace arm { /// @TODO(ahundt) add support for a vector of Transformation data @see FRIMessages.pb.h, /// but store as quaternon + vector, not rotation matrix struct KukaState { - typedef boost::container::static_vector - joint_state; + typedef boost::container::static_vector joint_state; // cartesian state entries consist of a vector x,y,z and a quaternion [x, y, z, w] typedef boost::container::static_vector cartesian_state; /// Class std::chrono::high_resolution_clock represents the clock with the smallest tick period provided by the implementation. diff --git a/include/grl/kuka/KukaFRIdriver.hpp b/include/grl/kuka/KukaFRIdriver.hpp index ced5b89..b82758c 100644 --- a/include/grl/kuka/KukaFRIdriver.hpp +++ b/include/grl/kuka/KukaFRIdriver.hpp @@ -3,9 +3,7 @@ #ifndef _KUKA_FRI_DRIVER #define _KUKA_FRI_DRIVER -// To call flatbuffer and toflatbuffer functions -#include "grl/kuka/KukaToFlatbuffer.hpp" #include #include @@ -23,7 +21,7 @@ //#ifdef BOOST_NO_CXX11_ATOMIC_SMART_PTR #include //#endif - +#include "grl/flatbuffer/flatbuffer.hpp" // friClientData is found in the kuka connectivity FRI cpp zip file #include "grl/kuka/Kuka.hpp" #include "grl/kuka/KukaFRI.hpp" @@ -33,14 +31,14 @@ // used for time synchronization #include "grl/TimeEvent.hpp" +// To call flatbuffer and toflatbuffer functions + +#include "grl/kuka/KukaToFlatbuffer.hpp" /// @todo TODO(ahundt) REMOVE SPDLOG FROM LOW LEVEL CODE #include #include - - - struct KukaState; namespace grl { @@ -834,8 +832,7 @@ class KukaFRIClientDataDriver std::shared_ptr, boost::system::error_code, std::size_t, boost::system::error_code, std::size_t, - grl::TimeEvent> - LatestState; + grl::TimeEvent> LatestState; /// Creates a default LatestState Object static LatestState @@ -1349,7 +1346,7 @@ class KukaFRIdriver : public std::enable_shared_from_this< bool startRecordingDataToFlatBuffer(flatbuffers::FlatBufferBuilder &fbb, std::shared_ptr &friData) { - FRIMonitoringMessage monitoringMsg = friData->monitoringMsg; + std::string RobotName("Robotiiwa" ); std::string destination("where this message is going (URI)"); @@ -1357,21 +1354,33 @@ class KukaFRIdriver : public std::enable_shared_from_this< std::string basename = RobotName; //std::get<0>(params); bool setArmConfiguration_ = true; // set the arm config first time bool max_control_force_stop_ = false; - + // Resolve the data in FRIMonitoringMessage + // Don't match the FRIMessage.pb.h grl::flatbuffer::ArmState armControlMode = grl::flatbuffer::ArmState::StartArm; grl::flatbuffer::KUKAiiwaInterface commandInterface = grl::flatbuffer::KUKAiiwaInterface::SmartServo;// KUKAiiwaInterface::SmartServo; grl::flatbuffer::KUKAiiwaInterface monitorInterface = grl::flatbuffer::KUKAiiwaInterface::FRI; + FRIMonitoringMessage monitoringMsg = friData->monitoringMsg; + // RobotInfo + // how to use pb_callback_t driveState in RobotInfo? + ::RobotInfo robotInfo = monitoringMsg.robotInfo; + int NUM_DOF = robotInfo.has_numberOfJoints?robotInfo.numberOfJoints:7; + ::ControlMode controlMode = robotInfo.controlMode; + ::SafetyState safetyState = robotInfo.safetyState; + ::OperationMode operationMode = robotInfo.operationMode; + // ConnectionInfo + // how to use uint32_t receiveMultiplier ::ConnectionInfo connectionInfo = monitoringMsg.connectionInfo; - ::RobotInfo robotInfo = monitoringMsg.robotInfo; + ::FRISessionState friSessionState = connectionInfo.sessionState; + ::FRIConnectionQuality friConnectionQuality = connectionInfo.quality; + // MessageIpoData + // JointValues jointPosition; double trackingPerformance; ::MessageIpoData ipoData = monitoringMsg.ipoData; - ::ControlMode controlMode = robotInfo.controlMode; ::ClientCommandMode clientCommandMode = ipoData.clientCommandMode; ::OverlayType overlayType = ipoData.overlayType; - ::FRISessionState friSessionState = connectionInfo.sessionState; - ::FRIConnectionQuality friConnectionQuality = connectionInfo.quality; + // No MessageHeader in flatbuffer objects ::MessageHeader messageHeader = monitoringMsg.header; ::MessageMonitorData messageMonitorData = monitoringMsg.monitorData; @@ -1381,21 +1390,17 @@ class KukaFRIdriver : public std::enable_shared_from_this< for (int i=0; i<5; i++) { transformation[i] = monitoringMsg.requestedTransformations[i]; } + // MessageEndOf exists in FRIMessage.pb.h, but it's never used in the FlatBuffer objects ::MessageEndOf endOfMessageData = monitoringMsg.endOfMessageData; + std::size_t builder_size_bytes = 0; std::size_t previous_size = 0; - - auto bns = fbb.CreateString(basename); - double duration = boost::chrono::high_resolution_clock::now().time_since_epoch().count(); - - flatbuffers::Offset controlState; - - int64_t sequenceNumber = 0; - const std::size_t MegaByte = 1024*1024; // In 32bits, the maximum size of a single flatbuffer is 2GB - 1 (defined in base.h), but actually single_buffer_limit_bytes can't be set beyond 2045 MB, otherwise assert might be launched. const std::size_t single_buffer_limit_bytes = 2045*MegaByte; - + auto bns = fbb.CreateString(basename); + double duration = boost::chrono::high_resolution_clock::now().time_since_epoch().count(); + flatbuffers::Offset controlState; // @TODO(Chunting) Put all these parameters in a configuration file. std::vector joint_stiffness_(7, 0.2); std::vector joint_damping_(7, 0.3); @@ -1421,22 +1426,27 @@ class KukaFRIdriver : public std::enable_shared_from_this< int16_t portOnRemote = 3501; bool updatePortOnController = false; int16_t portOnController = 3502; - - while(builder_size_bytes < single_buffer_limit_bytes ) + bool OK = true; + std::vector> kukaiiwaStateVec; + int64_t sequenceNumber = 0; + while(OK && builder_size_bytes < single_buffer_limit_bytes ) { copy(monitoringMsg, armState); armState.sessionState = grl::toFlatBuffer(friSessionState); armState.connectionQuality = grl::toFlatBuffer(friConnectionQuality); - armState.safetyState = grl::toFlatBuffer(robotInfo.safetyState); - armState.operationMode = grl::toFlatBuffer(robotInfo.operationMode); - // // // This parameter is never used. + armState.safetyState = grl::toFlatBuffer(safetyState); + armState.operationMode = grl::toFlatBuffer(operationMode); + // This parameter is never used. armState.driveState = grl::toFlatBuffer(::DriveState::DriveState_OFF); flatbuffers::Offset controlState = grl::toFlatBuffer(fbb, basename, sequenceNumber++, duration, armState, armControlMode); flatbuffers::Offset setCartesianImpedance = grl::toFlatBuffer(fbb, cart_stiffness_, cart_damping_, nullspace_stiffness_, nullspace_damping_, cart_max_path_deviation_, cart_max_ctrl_vel_, cart_max_ctrl_force_, max_control_force_stop_); flatbuffers::Offset setJointImpedance = grl::toFlatBuffer(fbb, joint_stiffness_, joint_damping_); - // flatbuffers::Offset setSmartServo = grl::toFlatBuffer(*fbbP, joint_AccelerationRel_, joint_VelocityRel_, updateMinimumTrajectoryExecutionTime, minimumTrajectoryExecutionTime); + // normalized joint accelerations/velocities from 0 to 1 relative to system capabilities + // how to get the acceleration of the robot? There is no acceleration information in KukaState (armState). + flatbuffers::Offset setSmartServo = grl::toFlatBuffer(fbb, joint_AccelerationRel_, joint_VelocityRel_, updateMinimumTrajectoryExecutionTime, minimumTrajectoryExecutionTime); + // FRI configuration parameters flatbuffers::Offset FRIConfig = grl::toFlatBuffer(fbb, overlayType, connectionInfo, updatePortOnRemote, portOnRemote, updatePortOnController, portOnController); std::vector> tools; std::vector> processData; @@ -1445,7 +1455,7 @@ class KukaFRIdriver : public std::enable_shared_from_this< for(int i=0; i<7; i++) { std::string linkname = "Link" + std::to_string(i); std::string parent = i==0?"Base":("Link" + std::to_string(i-1)); - + // Compute pose later with transformation matrix grl::flatbuffer::Pose pose = grl::flatbuffer::Pose(grl::flatbuffer::Vector3d(0,0,0), grl::flatbuffer::Quaternion(0,0,0,0)); grl::flatbuffer::Inertia inertia = grl::flatbuffer::Inertia(1, pose, 1, 2, 3, 4, 5, 6); flatbuffers::Offset linkObject = grl::toFlatBuffer(fbb, linkname, parent, pose, inertia); @@ -1461,9 +1471,114 @@ class KukaFRIdriver : public std::enable_shared_from_this< "unit"+ std::to_string(i), "value"+ std::to_string(i))); } - + // Set the configuration of the Kuka iiwa + flatbuffers::Offset kukaiiwaArmConfiguration = grl::toFlatBuffer( + fbb, + RobotName, + commandInterface, + monitorInterface, + clientCommandMode, + overlayType, + controlMode, + setCartesianImpedance, + setJointImpedance, + setSmartServo, + FRIConfig, + tools, + processData, + "currentMotionCenter", + true); + grl::TimeEvent time_event_stamp; + flatbuffers::Offset friMessageLog = grl::toFlatBuffer( + fbb, + friSessionState, + friConnectionQuality, + controlMode, + monitoringMsg, + time_event_stamp); + // getWrench() is availble in KukaJAVAdriver, so maybe it's better to log data in KukaDriver where user can access both KukaFRIDriver and KukaJAVADriver? + grl::flatbuffer::Wrench cartesianWrench{grl::flatbuffer::Vector3d(1,1,1),grl::flatbuffer::Vector3d(1,1,1),grl::flatbuffer::Vector3d(1,1,1)}; + + // In armState there is neither joint velocity nor acceleration + std::vector position(7,0); + std::vector velocity(7,0); + std::vector acceleration(7,0); + std::vector torque(7,0); + for(int i = 0; i<7; i++) { + position.push_back(armState.position[i]); + torque.push_back(armState.torque[i]); + } + flatbuffers::Offset jointStatetab = grl::toFlatBuffer(fbb, position, velocity, acceleration, torque); + // Calculate the data later. + // Cartesian pose of the flange relative to the base of the arm + grl::flatbuffer::Pose cartesianFlangePose = grl::flatbuffer::Pose(grl::flatbuffer::Vector3d(1,1,1), grl::flatbuffer::Quaternion(2,3,4,5)); + flatbuffers::Offset kukaiiwaMonitorState = grl::toFlatBuffer( + fbb, + jointStatetab, // flatbuffers::Offset &measuredState, + cartesianFlangePose, + jointStatetab, // flatbuffers::Offset &jointStateReal, + jointStatetab, // flatbuffers::Offset &jointStateInterpolated, + jointStatetab, // flatbuffers::Offset &externalState, + friSessionState, + robotInfo.operationMode, + cartesianWrench); + + // Set it up in the configuration file + std::vector torqueSensorLimits(7,0.5); + std::string hardwareVersion( "hardvareVersion"); + bool isReadyToMove = true; + bool isMastered = true; + flatbuffers::Offset monitorConfig = grl::toFlatBuffer( + fbb, + hardwareVersion, + torqueSensorLimits, + isReadyToMove, + isMastered, + processData); + flatbuffers::Offset kukaiiwaState = grl::toFlatBuffer( + fbb, + RobotName, + destination, + source, + duration, + true, controlState, + true, kukaiiwaArmConfiguration, + true, kukaiiwaMonitorState, + false, monitorConfig, + friMessageLog); + + kukaiiwaStateVec.push_back(kukaiiwaState); } + std::cout<< "kukaiiwaStateVec:" << kukaiiwaStateVec.size() << std::endl; + + flatbuffers::Offset kukaStates = grl::toFlatBuffer(fbb, kukaiiwaStateVec); + grl::flatbuffer::FinishKUKAiiwaStatesBuffer(fbb, kukaStates); + uint8_t *buf = fbb.GetBufferPointer(); + std::size_t bufsize = fbb.GetSize(); + /// To expand the capacity of a single buffer, _max_tables is set to 10000000 + flatbuffers::uoffset_t _max_depth = 64; + flatbuffers::uoffset_t _max_tables = 10000000; + flatbuffers::Verifier verifier(buf, bufsize, _max_depth, _max_tables); + OK = OK && grl::flatbuffer::VerifyKUKAiiwaStatesBuffer(verifier); + assert(OK && "VerifyKUKAiiwaStatesBuffer"); + std::cout << "Buffer size: " << bufsize << std::endl; + std::string binary_file_path = "Kuka_test_binary.iiwa"; + std::string json_file_path = "kuka_test_text.json"; + // Get the current working directory + std::string fbs_filename("KUKAiiwa.fbs"); + // OK = OK && grl::SaveFlatBufferFile( + // buf, + // fbb.GetSize(), + // binary_file_path, + // fbs_filename, + // json_file_path); + assert(OK && "SaveFlatBufferFile"); + + std::cout << "End of the program" << std::endl; + + + return true; @@ -1498,9 +1613,7 @@ class KukaFRIdriver : public std::enable_shared_from_this< // The work class is used to inform the io_service when work starts and finishes. std::unique_ptr device_driver_workP_; std::unique_ptr driver_threadP; - std::shared_ptr< - grl::robot::arm::KukaFRIClientDataDriver> - kukaFRIClientDataDriverP_; + std::shared_ptr> kukaFRIClientDataDriverP_; private: KukaState armState; diff --git a/src/ros/CMakeLists.txt b/src/ros/CMakeLists.txt index af03ea3..97af8cd 100644 --- a/src/ros/CMakeLists.txt +++ b/src/ros/CMakeLists.txt @@ -41,9 +41,9 @@ if(ROS_FOUND AND TARGET KukaFRIClient AND Boost_FOUND AND Boost_CHRONO_FOUND) - + basis_include_directories(${FRI-Client-SDK_Cpp_PROJECT_INCLUDE_DIRS} ${FRI-Client-SDK_Cpp_INCLUDE_DIRS} ${ROS_INCLUDE_DIR}) - basis_add_executable(grl_kuka_ros_driver grl_kuka_ros_driver.cpp) + basis_add_executable(grl_kuka_ros_driver grl_kuka_ros_driver.cpp ) basis_add_dependencies(grl_kuka_ros_driver grlflatbuffers ${FRI-Client-SDK_Cpp_LIBRARIES}) basis_target_link_libraries(grl_kuka_ros_driver ${Boost_LIBRARIES} @@ -52,6 +52,7 @@ if(ROS_FOUND AND TARGET KukaFRIClient AND Boost_FOUND AND Boost_CHRONO_FOUND) ${CMAKE_THREAD_LIBS_INIT} ${Nanopb_LIBRARIES} ${ROS_LIBRARIES} + ${FLATBUFFERS_STATIC_LIB} KukaFRIClient ) target_compile_definitions(grl_kuka_ros_driver PUBLIC $) diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index d71a5d7..340680e 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -82,7 +82,7 @@ if(TARGET KukaFRIClient OR FRI_Client_SDK_Cpp_FOUND) ${FLATBUFFERS_INCLUDE_DIRS} ${FUSIONTRACK_INCLUDE_DIRS}) basis_add_executable(KukaFRITest.cpp)# ${GRL_FLATBUFFERS_OUTPUTS}) - basis_target_link_libraries(KukaFRITest ${Boost_LIBRARIES} ${Boost_REGEX_LIBRARY} ${FRI_Client_SDK_Cpp_LIBRARIES} ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} KukaFRIClient ) + basis_target_link_libraries(KukaFRITest ${Boost_LIBRARIES} ${Boost_REGEX_LIBRARY} ${FRI_Client_SDK_Cpp_LIBRARIES} ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} ${FLATBUFFERS_STATIC_LIB} KukaFRIClient ) if(UNIX AND NOT APPLE) set(LINUX_ONLY_LIBS ${LIBDL_LIBRARIES}) @@ -90,7 +90,7 @@ if(TARGET KukaFRIClient OR FRI_Client_SDK_Cpp_FOUND) basis_add_test(KukaLBRiiwaVrepPluginTest.cpp) basis_target_link_libraries(KukaLBRiiwaVrepPluginTest ${Boost_LIBRARIES} ${Boost_REGEX_LIBRARY} ${Boost_UNIT_TEST_FRAMEWORK_LIBRARY} ${FRI_Client_SDK_Cpp_LIBRARIES} ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} - v_repLib KukaFRIClient ${LINUX_ONLY_LIBS} ) + ${FLATBUFFERS_STATIC_LIB} v_repLib KukaFRIClient ${LINUX_ONLY_LIBS} ) basis_add_executable(kukaiiwaTest.cpp)# ${GRL_FLATBUFFERS_OUTPUTS}) basis_target_link_libraries(kukaiiwaTest ${Boost_LIBRARIES} ${Boost_REGEX_LIBRARY} ${FRI_Client_SDK_Cpp_LIBRARIES} ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} KukaFRIClient ${FLATBUFFERS_STATIC_LIB}) From 8d4f31358a53701b21673e8fe941abbe55ecb2c5 Mon Sep 17 00:00:00 2001 From: Chunting Date: Wed, 10 Jan 2018 18:58:04 -0500 Subject: [PATCH 027/102] Log data from robot, fail to excute. --- include/grl/kuka/KukaFRIClientDataDriver.hpp | 924 ++++++++ include/grl/kuka/KukaFRIdriver.hpp | 2005 +++++------------- include/grl/kuka/KukaToFlatbuffer.hpp | 28 + 3 files changed, 1540 insertions(+), 1417 deletions(-) create mode 100644 include/grl/kuka/KukaFRIClientDataDriver.hpp diff --git a/include/grl/kuka/KukaFRIClientDataDriver.hpp b/include/grl/kuka/KukaFRIClientDataDriver.hpp new file mode 100644 index 0000000..8449454 --- /dev/null +++ b/include/grl/kuka/KukaFRIClientDataDriver.hpp @@ -0,0 +1,924 @@ +/// KukaFRIDriver.hpp handles communication with the Kuka over FRI. +/// If you are new to this code base you are most likely looking for KukaDriver.hpp +#ifndef _KUKA_FRI_CLIENT_DATA_DRIVER +#define _KUKA_FRI_CLIENT_DATA_DRIVER + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +//#ifdef BOOST_NO_CXX11_ATOMIC_SMART_PTR +#include +//#endif +#include "grl/flatbuffer/flatbuffer.hpp" +// friClientData is found in the kuka connectivity FRI cpp zip file +#include "grl/kuka/Kuka.hpp" +#include "grl/kuka/KukaFRI.hpp" +#include "grl/exception.hpp" +#include "grl/vector_ostream.hpp" +#include "grl/kuka/KukaFRIalgorithm.hpp" +// used for time synchronization +#include "grl/TimeEvent.hpp" +#include "grl/time.hpp" + +// To call flatbuffer and toflatbuffer functions + +#include "grl/kuka/KukaToFlatbuffer.hpp" + +/// @todo TODO(ahundt) REMOVE SPDLOG FROM LOW LEVEL CODE +#include +#include + +struct KukaState; + +namespace grl { +namespace robot { +namespace arm { + +/// @brief internal function to decode KUKA FRI message buffer (using nanopb +/// decoder) for the KUKA FRI +/// +/// @note encode needs to be updated for each additional supported command type +/// and when updating to newer FRI versions +void decode(KUKA::FRI::ClientData &friData, std::size_t msg_size) { + // The decoder was given a pointer to the monitoringMessage at initialization + if (!friData.decoder.decode(friData.receiveBuffer, msg_size)) { + BOOST_THROW_EXCEPTION(std::runtime_error("Error decoding received FRI data, the message may be missing or corrupted. This error is most likely due to the application running on the KUKA robot's side of the connection going down or disabling FRI, so check the robot and the JAVA side of the system.")); + } + + // check message type + if (friData.expectedMonitorMsgID != + friData.monitoringMsg.header.messageIdentifier) { + BOOST_THROW_EXCEPTION(std::invalid_argument( + std::string("KukaFRI.hpp: Problem reading buffer, id code: ") + + boost::lexical_cast( + static_cast(friData.monitoringMsg.header.messageIdentifier)) + + std::string(" does not match expected id code: ") + + boost::lexical_cast( + static_cast(friData.expectedMonitorMsgID)) + + std::string("\n"))); + return; + } + + friData.lastState = + grl::robot::arm::get(friData.monitoringMsg, KUKA::FRI::ESessionState()); +} + +/// @brief Default LowLevelStepAlgorithmType +/// This algorithm is designed to be changed out +/// @todo Generalize this class using C++ techinques "tag dispatching" and "type +/// traits". See boost.geometry access and coorinate_type classes for examples. +/// Also perhaps make this the outer class which accepts drivers at the template param? +struct LinearInterpolation { + + enum ParamIndex { + JointAngleDest, + TimeDurationToDestMS + }; + + // A variable-size array container with fixed capacity. + // A static_vector is a sequence that supports random access to elements, constant time insertion and removal of elements at the end, and linear time insertion and removal of elements at the beginning or in the middle. + typedef std::tuple,std::size_t> Params; + + // extremely conservative default timeframe to reach destination plus no goal position + static const Params defaultParams() { + boost::container::static_vector nopos; + return std::make_tuple(nopos,10000); + } + /// Default constructor + /// @todo verify this doesn't corrupt the state of the system + LinearInterpolation() { }; + + // no action by default + template + void lowLevelTimestep(ArmDataType &, CommandModeType &) { + // need to tag dispatch here + BOOST_VERIFY(false); // not yet supported + } + + template + void lowLevelTimestep(ArmData &friData, + revolute_joint_angle_open_chain_command_tag) { + + // no updates if no goal has been set + if(goal_position.size() == 0) return; + // switch (friData_->monitoringMsg.robotInfo.controlMode) { + // case ControlMode_POSITION_CONTROLMODE: + // case ControlMode_JOINT_IMPEDANCE_CONTROLMODE: + + KukaState::joint_state ipoJointPos; + KukaState::joint_state currentJointPos; + KukaState::joint_state currentMinusIPOJointPos; + KukaState::joint_state goalPlusIPOJointPos; + KukaState::joint_state diffToGoal; + KukaState::joint_state amountToMove; + KukaState::joint_state commandToSend; + KukaState::joint_state commandToSendPlusIPOJointPos; + + double ripoJointPos[7]; + double rcurrentJointPos[7]; + double rcurrentMinusIPOJointPos[7]; + double rgoalPlusIPOJointPos[7]; + double rcommandedGoal[7]; + double rdiffToGoal[7]; + double ramountToMove[7]; + double rcommandToSend[7]; + double rvelocity_limits[7]; + double rcommandToSendPlusIPOJointPos[7]; + + // the current "holdposition" joint angles + /// @todo maybe this should be the + /// revolute_joint_angle_interpolated_open_chain_state_tag()? @see + /// kukaFRIalgorithm.hpp + grl::robot::arm::copy(friData.monitoringMsg, + std::back_inserter(currentJointPos), + revolute_joint_angle_open_chain_state_tag()); + grl::robot::arm::copy(friData.monitoringMsg, + std::back_inserter(ipoJointPos), + revolute_joint_angle_interpolated_open_chain_state_tag()); + + // copy value for debugging + boost::copy(ipoJointPos, &ripoJointPos[0]); + boost::copy(currentJointPos, &rcurrentJointPos[0]); + boost::copy(goal_position, &rcommandedGoal[0]); + + boost::transform(currentJointPos, ipoJointPos, + std::back_inserter(currentMinusIPOJointPos), std::minus()); + boost::transform(goal_position, ipoJointPos, + std::back_inserter(goalPlusIPOJointPos), std::plus()); + + + boost::copy(currentMinusIPOJointPos, &rcurrentMinusIPOJointPos[0]); + boost::copy(goal_position, &rcommandedGoal[0]); + boost::copy(goalPlusIPOJointPos, &rgoalPlusIPOJointPos[0]); + + // only move if there is time left to reach the goal + if(goal_position_command_time_duration_remaining > 0) + { + // single timestep in ms + int thisTimeStepMS(grl::robot::arm::get(friData.monitoringMsg, grl::time_step_tag())); + double thisTimeStepS = (static_cast(thisTimeStepMS) / 1000); + //double secondsPerTick = std::chrono::duration_cast(thisTimeStep).count(); + + // the fraction of the distance to the goal that should be traversed this + // tick + double fractionOfDistanceToTraverse = + static_cast(thisTimeStepMS) / + static_cast(goal_position_command_time_duration_remaining); + + // makes viewing in a debugger easier + boost::copy(goal_position, &rcommandedGoal[0]); + // get the angular distance to the goal + // use current time and time to destination to interpolate (scale) goal + // joint position + boost::transform(goal_position, currentJointPos, + std::back_inserter(diffToGoal), + [&](double commanded_angle, double current_angle) { + return (commanded_angle - current_angle) * + fractionOfDistanceToTraverse; + }); + boost::copy(diffToGoal, &rdiffToGoal[0]); + + goal_position_command_time_duration_remaining -= thisTimeStepMS; + + /// @todo correctly pass velocity limits from outside, use "copy" fuction in + /// Kuka.hpp, correctly account for differing robot models. This *should* + /// be in KukaFRIdriver at the end of this file. + + // R820 velocity limits + // A1 - 85 °/s == 1.483529864195 rad/s + // A2 - 85 °/s == 1.483529864195 rad/s + // A3 - 100 °/s == 1.745329251994 rad/s + // A4 - 75 °/s == 1.308996938996 rad/s + // A5 - 130 °/s == 2.268928027593 rad/s + // A6 - 135 °/s == 2.356194490192 rad/s + // A1 - 135 °/s == 2.356194490192 rad/s + KukaState::joint_state velocity_limits; + velocity_limits.push_back(1.483529864195*thisTimeStepS); + velocity_limits.push_back(1.483529864195*thisTimeStepS); + velocity_limits.push_back(1.745329251994*thisTimeStepS); + velocity_limits.push_back(1.308996938996*thisTimeStepS); + velocity_limits.push_back(2.268928027593*thisTimeStepS); + velocity_limits.push_back(2.356194490192*thisTimeStepS); + velocity_limits.push_back(2.356194490192*thisTimeStepS); + + boost::copy(velocity_limits, &rvelocity_limits[0]); + // clamp the commanded velocities to below the system limits + // use std::min to ensure commanded change in position remains under the + // maximum possible velocity for a single timestep + boost::transform( + diffToGoal, velocity_limits, std::back_inserter(amountToMove), + [&](double diff, double maxvel) { + return boost::math::copysign(std::min(std::abs(diff), maxvel), diff); + }); + + boost::copy(amountToMove, &ramountToMove[0]); + + + // add the current joint position to the amount to move to get the actual + // position command to send + boost::transform(currentJointPos, amountToMove, + std::back_inserter(commandToSend), std::plus()); + + boost::copy(currentMinusIPOJointPos, &rcurrentMinusIPOJointPos[0]); + boost::copy(commandToSend, &rcommandToSend[0]); + + + boost::transform(commandToSend, ipoJointPos, + std::back_inserter(commandToSendPlusIPOJointPos), std::plus()); + + + boost::copy(commandToSendPlusIPOJointPos, &rcommandToSendPlusIPOJointPos[0]); + // send the command +// grl::robot::arm::set(friData.commandMsg, commandToSend, +// grl::revolute_joint_angle_open_chain_command_tag()); + // send the command + grl::robot::arm::set(friData.commandMsg, commandToSend, + grl::revolute_joint_angle_open_chain_command_tag()); + } + // break; + } + + void setGoal(const Params& params ) { + /// @todo TODO(ahundt) support param tag structs for additional control modes + goal_position_command_time_duration_remaining = std::get(params); + goal_position = std::get(params); + + } + + /// @todo look in FRI_Client_SDK_Cpp.zip to see if position must be set for + /// joint torques. Ref files: LBRTorqueSineOverlayClient.cpp, + /// LBRTorqueSineOverlayClient.h, friLBRCommand.cpp, friLBRCommand.h + template + void lowLevelTimestep(ArmData &friData, + revolute_joint_torque_open_chain_command_tag) { + + //not yet supported + BOOST_VERIFY(false); + // case ControlMode_JOINT_IMPEDANCE_CONTROLMODE: +// grl::robot::arm::set(friData.commandMsg, armState.commandedTorque, +// grl::revolute_joint_torque_open_chain_command_tag()); + + /// @note encode() needs to be updated for each additional supported command + /// type + // break; + } + + /// @todo look in FRI_Client_SDK_Cpp.zip to see if position must be set for + /// cartesian wrench. Ref files: LBRWrenchSineOverlayClient.cpp, + /// LBRWrenchSineOverlayClient.h, friLBRCommand.cpp, friLBRCommand.h + template + void lowLevelTimestep(ArmData &friData, cartesian_wrench_command_tag) { + + //not yet supported + BOOST_VERIFY(false); + // case ControlMode_CARTESIAN_IMPEDANCE_CONTROLMODE: + // not yet supported +// grl::robot::arm::set(friData.commandMsg, +// armState.commandedCartesianWrenchFeedForward, +// grl::cartesian_wrench_command_tag()); + // break; + } + + /// @todo make this accessible via a nonmember function + bool hasCommandData() { + /// @todo check if duration remaining should be greater than zero or greater + /// than the last tick size + return goal_position_command_time_duration_remaining > 0; + } + // template + // void operator()(ArmData& clientData, + // revolute_joint_angle_open_chain_command_tag){ + // default: + // break; + // } +private: + // the armstate at initialization of this object + KukaState::joint_state velocity_limits; + KukaState::joint_state goal_position; + double goal_position_command_time_duration_remaining; // milliseconds +}; + +/// @brief encode data in the class KUKA::FRI::ClientData into the send buffer +/// for the KUKA FRI. +/// this preps the information for transport over the network +/// +/// @note encode needs to be updated for each additional supported command type +/// and when updating to newer FRI versions +template +std::size_t encode(LowLevelStepAlgorithmType &step_alg, + KUKA::FRI::ClientData &friData, + boost::system::error_code &ec) { + // reset send counter + friData.lastSendCounter = 0; + + // set sequence counters + friData.commandMsg.header.sequenceCounter = friData.sequenceCounter++; + friData.commandMsg.header.reflectedSequenceCounter = + friData.monitoringMsg.header.sequenceCounter; + + KUKA::FRI::ESessionState sessionState = + grl::robot::arm::get(friData.monitoringMsg, KUKA::FRI::ESessionState()); + + if ((step_alg.hasCommandData() && + (sessionState == KUKA::FRI::COMMANDING_WAIT || + sessionState == KUKA::FRI::COMMANDING_ACTIVE))) { + KUKA::FRI::EClientCommandMode commandMode = grl::robot::arm::get( + friData.monitoringMsg, KUKA::FRI::EClientCommandMode()); + switch (commandMode) { + case ClientCommandMode_POSITION: + step_alg.lowLevelTimestep(friData, revolute_joint_angle_open_chain_command_tag()); + break; + case ClientCommandMode_WRENCH: + step_alg.lowLevelTimestep(friData, cartesian_wrench_command_tag()); + break; + case ClientCommandMode_TORQUE: + step_alg.lowLevelTimestep(friData, revolute_joint_torque_open_chain_command_tag()); + break; + default: + // this is unhandled at the moment... + BOOST_VERIFY(false); + // BOOST_THROW_EXCEPTION_CURRENT_FUNCTION; + // ClientCommandMode_NO_COMMAND_MODE, anything else that is added in the + // future and unimplemented + /// @todo do nothing if in an unsupported command mode? Or do the same as + /// the next else if step? + break; + } + + } else if (!(friData.commandMsg.has_commandData && + step_alg.hasCommandData() && + (sessionState == KUKA::FRI::COMMANDING_WAIT || + sessionState == KUKA::FRI::COMMANDING_ACTIVE))) { + // copy current measured joint position to commanded position only if we + // *don't* have new command data + + /// @todo should this be different if it is in torque mode? + /// @todo allow copying of data directly between commandmsg and + /// monitoringMsg + std::vector msg; + copy(friData.monitoringMsg, std::back_inserter(msg), + revolute_joint_angle_open_chain_command_tag()); + // copy the previously recorded command over + set(friData.commandMsg, msg, + grl::revolute_joint_angle_open_chain_command_tag()); + } + + int buffersize = KUKA::FRI::FRI_COMMAND_MSG_MAX_SIZE; + if (!friData.encoder.encode(friData.sendBuffer, buffersize)) { + // @todo figure out PB_GET_ERROR, integrate with error_code type supported + // by boost + ec = boost::system::errc::make_error_code(boost::system::errc::bad_message); + return 0; + } + + return buffersize; +} + +/// @brief Actually talk over the network to receive an update and send out a +/// new KUKA FRI command +/// +/// Receives an update, performs the necessary checks, then sends a message if +/// appropriate. +/// +/// @pre socket must already have the endpoint resolved and "connected". While +/// udp is technically stateless the asio socket supports the connection api +/// components for convenience. +template +void update_state(boost::asio::ip::udp::socket &socket, + LowLevelStepAlgorithmType &step_alg, + KUKA::FRI::ClientData &friData, + boost::system::error_code &receive_ec, + std::size_t &receive_bytes_transferred, + boost::system::error_code &send_ec, + std::size_t &send_bytes_transferred, + grl::TimeEvent& timeEvent, + boost::asio::ip::udp::endpoint sender_endpoint = boost::asio::ip::udp::endpoint()) { + + static const int message_flags = 0; + // get a local clock timestamp, then the latest frame from the device, then another timestamp + timeEvent.local_request_time = cartographer::common::UniversalTimeScaleClock::now(); + receive_bytes_transferred = socket.receive_from( + boost::asio::buffer(friData.receiveBuffer, + KUKA::FRI::FRI_MONITOR_MSG_MAX_SIZE), + sender_endpoint, message_flags, receive_ec); + timeEvent.local_receive_time = cartographer::common::UniversalTimeScaleClock::now(); + decode(friData, receive_bytes_transferred); + + friData.lastSendCounter++; + // Check whether to send a response + if (friData.lastSendCounter >= + friData.monitoringMsg.connectionInfo.receiveMultiplier) { + send_bytes_transferred = encode(step_alg, friData, send_ec); + if (send_ec) + return; + socket.send(boost::asio::buffer(friData.sendBuffer, send_bytes_transferred), + message_flags, send_ec); + } +} + + +/// @brief don't use this +/// @deprecated this is an old implemenation that will be removed in the future +void copy(const FRIMonitoringMessage &monitoringMsg, KukaState &state) { + state.clear(); + copy(monitoringMsg, std::back_inserter(state.position), + revolute_joint_angle_open_chain_state_tag()); + copy(monitoringMsg, std::back_inserter(state.torque), + revolute_joint_torque_open_chain_state_tag()); + copy(monitoringMsg, std::back_inserter(state.commandedPosition), + revolute_joint_angle_open_chain_command_tag()); + copy(monitoringMsg, std::back_inserter(state.commandedTorque), + revolute_joint_torque_open_chain_command_tag()); + copy(monitoringMsg, std::back_inserter(state.ipoJointPosition), + revolute_joint_angle_interpolated_open_chain_state_tag()); + state.sessionState = static_cast( + get(monitoringMsg, KUKA::FRI::ESessionState())); + state.connectionQuality = static_cast( + get(monitoringMsg, KUKA::FRI::EConnectionQuality())); + state.safetyState = static_cast( + get(monitoringMsg, KUKA::FRI::ESafetyState())); + state.operationMode = static_cast( + get(monitoringMsg, KUKA::FRI::EOperationMode())); + state.driveState = static_cast( + get(monitoringMsg, KUKA::FRI::EDriveState())); + + /// @todo fill out missing state update steps +} + +/// @brief Simple low level driver to communicate over the Kuka iiwa FRI +/// interface using KUKA::FRI::ClientData status objects +/// +/// @note If you aren't sure, see KukaDriver in KukaDriver.hpp. +/// +/// @note If you want to change how the lowest level high rate updates are +/// performed see KukaFRIdriver +/// +/// One important aspect of this design is the is_running_automatically flag. If +/// you are unsure, +/// the suggested default is run_automatically (true/enabled). When it is +/// enabled, +/// the driver will create a thread internally and run the event loop +/// (io_service) itself. +/// If run manually, you are expected to call io_service.run() on the io_service +/// you provide, +/// or on the run() member function. When running manually you are also expected +/// to call +/// async_getLatestState(handler) frequently enought that the 5ms response +/// requirement of the KUKA +/// FRI interface is met. +template +class KukaFRIClientDataDriver + : public std::enable_shared_from_this< + KukaFRIClientDataDriver>, + public KukaUDP { + +public: + using KukaUDP::ParamIndex; + using KukaUDP::ThreadingRunMode; + using KukaUDP::Params; + using KukaUDP::defaultParams; + + KukaFRIClientDataDriver(boost::asio::io_service &ios, + Params params = defaultParams()) + : params_(params), m_shouldStop(false), isConnectionEstablished_(false), + io_service_(ios) + //,socketP_(std::move(connect(params, io_service_,sender_endpoint_))) ///< + //@todo this breaks the assumption that the object can be constructed without + // hardware issues being a porblem + { + construct(params); + } + + KukaFRIClientDataDriver(Params params = defaultParams()) + : params_(params), m_shouldStop(false), isConnectionEstablished_(false), + optional_internal_io_service_P(new boost::asio::io_service), + io_service_(*optional_internal_io_service_P) + //,socketP_(std::move(connect(params, io_service_,sender_endpoint_))) ///< + //@todo this breaks the assumption that the object can be constructed without + // hardware issues being a porblem + { + construct(params); + } + + /// Call this to initialize the object after the constructor has been called + void construct(Params params = defaultParams()) { + try { + + /////////// + // initialize all of the states + latestStateForUser_ = make_valid_LatestState(); + spareStates_.emplace_back(std::move(make_valid_LatestState())); + spareStates_.emplace_back(std::move(make_valid_LatestState())); + + // start up the driver thread since the io_service_ is internal only + if (std::get(params)) { + driver_threadP_.reset(new std::thread([&] { update(); })); + } + + } catch (boost::exception &e) { + add_details_to_connection_error(e, params); + throw; + } + } + + /// @brief blocking call to communicate with the robot continuously + /// @pre construct() should be called before run() + void run() { update(); } + + /// @brief Updates the passed friData shared pointer to a pointer to newly + /// updated data, plus any errors that occurred. + /// + /// We recommend you supply a valid shared_ptr to friData, even if all command + /// elements are set to false. + /// The friData pointer you pass in can contain a command to send to the arm. + /// To update with new state and optional input state, you give up lifetime + /// control of the input, + /// and assume liftime control of the output. + /// + /// This function is designed for single threaded use to quickly receive and + /// send "non-blocking" updates to the robot. + /// It is not thread safe cannot be called simultaneously from multiple + /// threads. + /// + /// + /// @note { An error code is set if update_state is called with no new data + /// available. + /// In this special case, all error codes and bytes_transferred are 0, + /// because + /// there was no new data available for the user. + /// } + /// + /// @warning Do not pound this call continuously in a very tight loop, because + /// then the driver won't be able to acquire the lock and send updates to the + /// robot. + /// + /// @param[in,out] friData supply a new command, receive a new update of the + /// robot state. Pointer is null if no new data is available. + /// + /// @pre If friData!=nullptr it is assumed valid for use and this class will + /// take control of the object. + /// + /// @return isError = false if you have new data, true when there is either an + /// error or no new data + bool update_state(std::shared_ptr &step_alg_params, + std::shared_ptr &friData, + boost::system::error_code &receive_ec, + std::size_t &receive_bytes_transferred, + boost::system::error_code &send_ec, + std::size_t &send_bytes_transferred, + grl::TimeEvent& timeEvent) { + + if (exceptionPtr) { + /// @note this exception most likely came from the update() call running + /// the kuka driver + std::rethrow_exception(exceptionPtr); + } + + bool haveNewData = false; + + if (!isConnectionEstablished_ || + !std::get(latestStateForUser_)) { + // no new data, so immediately return results accordingly + // Constructs a tuple object whose elements are references to the arguments in args std::tie (Types&... args) , in the same order. + // This allows a set of objects to act as a tuple, which is especially useful to unpack tuple objects. + std::tie(step_alg_params, friData, receive_ec, receive_bytes_transferred, send_ec, + send_bytes_transferred, timeEvent) = make_LatestState(step_alg_params,friData); + return !haveNewData; + } + + // ensure we have valid data for future updates + // need to copy this over because friData will be set as an output value + // later + // and allocate/initialize data if null + auto validFriDataLatestState = make_valid_LatestState(step_alg_params,friData); + + // get the latest state from the driver thread + { + boost::lock_guard lock(ptrMutex_); + + // get the update if one is available + // the user has provided new data to send to the device + if (std::get(validFriDataLatestState) + ->commandMsg.has_commandData) { + std::swap(validFriDataLatestState, newCommandForDriver_); + } + // newCommandForDriver_ is finalized + + if (spareStates_.size() < spareStates_.capacity() && + std::get(validFriDataLatestState)) { + spareStates_.emplace_back(std::move(validFriDataLatestState)); + } + + if (std::get(latestStateForUser_)) { + // return the latest state to the caller + std::tie(step_alg_params,friData, receive_ec, receive_bytes_transferred, send_ec, + send_bytes_transferred, timeEvent) = std::move(latestStateForUser_); + // TODO(chunting) consider adding to log buffer here, data is in latestStateForUser_, alternate might be latestClientData + + + + + + haveNewData = true; + } else if (std::get( + validFriDataLatestState)) { + // all storage is full, return the spare data to the user + std::tie(step_alg_params, friData, receive_ec, receive_bytes_transferred, send_ec, + send_bytes_transferred, timeEvent) = validFriDataLatestState; + } + } + + // let the user know if we aren't in the best possible state + return !haveNewData || receive_bytes_transferred == 0 || receive_ec || + send_ec; + } + + void destruct() { + m_shouldStop = true; + if (driver_threadP_) { + driver_threadP_->join(); + } + } + + ~KukaFRIClientDataDriver() { destruct(); } + + /// Is everything ok? + /// @return true if the kuka fri connection is actively running without any + /// issues + /// @todo consider expanding to support real error codes + bool is_active() { return !exceptionPtr && isConnectionEstablished_; } + +private: + /// Reads data off of the real kuka fri device in a separate thread + /// @todo consider switching to single producer single consumer queue to avoid + /// locking overhead, but keep latency in mind + /// https://github.com/facebook/folly/blob/master/folly/docs/ProducerConsumerQueue.md + void update() { + try { + + LowLevelStepAlgorithmType step_alg; + /// nextState is the object currently being loaded with data off the + /// network + /// the driver thread should access this exclusively in update() + LatestState nextState = make_valid_LatestState(); + LatestState latestCommandBackup = make_valid_LatestState(); + + boost::asio::ip::udp::endpoint sender_endpoint; + boost::asio::ip::udp::socket socket( + connect(params_, io_service_, sender_endpoint)); + KukaState kukastate; ///< @todo TODO(ahundt) remove this line when new + /// api works completely since old one is deprecated + + ///////////// + // run the primary update loop in a separate thread + while (!m_shouldStop) { + /// @todo maybe there is a more convienient way to set this that is + /// easier for users? perhaps initializeClientDataForiiwa()? + + // nextState and latestCommandBackup should never be null + BOOST_VERIFY(std::get(nextState)); + BOOST_VERIFY(std::get(latestCommandBackup)); + + // set the flag that must always be there + std::get(nextState) + ->expectedMonitorMsgID = KUKA::LBRState::LBRMONITORMESSAGEID; + + auto lowLevelAlgorithmParamP = std::get(nextState); + + // if there is a valid low level algorithm param command set the new goal + if(lowLevelAlgorithmParamP) step_alg.setGoal(*lowLevelAlgorithmParamP); + + // actually talk over the network to receive an update and send out a + // new command + grl::robot::arm::update_state( + socket, step_alg, + *std::get(nextState), + std::get(nextState), + std::get(nextState), + std::get(nextState), + std::get(nextState), + std::get(nextState)); + + /// @todo use atomics to eliminate the global mutex lock for this object + // lock the mutex to communicate with the user thread + // if it cannot lock, simply send the previous message + // again + if (ptrMutex_.try_lock()) { + + ////////////////////////////////////////////// + // right now this is the state of everything: + ////////////////////////////////////////////// + // + // Always Valid: + // + // nextState: valid, contains the latest update + // latestCommandBackup: should always be valid (though hasCommand + // might be false) + // + // Either Valid or Null: + // latestStateForUser_ : null if the user took data out, valid + // otherwise + // newCommandForDriver_: null if there is no new command data from + // the user, vaild otherwise + + // 1) set the outgoing latest state for the user to pick up + // latestStateForUser_ is finalized + std::swap(latestStateForUser_, nextState); + + // 2) get a new incoming command if available and set incoming command + // variable to null + if (std::get(newCommandForDriver_)) { + // 3) back up the new incoming command + // latestCommandBackup is finalized, newCommandForDriver_ needs to + // be cleared out + std::swap(latestCommandBackup, newCommandForDriver_); + + // nextState may be null + if (!std::get(nextState)) { + nextState = std::move(newCommandForDriver_); + } else if (!(spareStates_.size() == spareStates_.capacity())) { + spareStates_.emplace_back(std::move(newCommandForDriver_)); + } else { + std::get(newCommandForDriver_) + .reset(); + } + } + + // finalized: latestStateForUser_, latestCommandBackup, + // newCommandForDriver_ is definitely null + // issues to be resolved: + // nextState: may be null right now, and it should be valid + // newCommandForDriver_: needs to be cleared with 100% certainty + BOOST_VERIFY(spareStates_.size() > 0); + + if (!std::get(nextState) && + spareStates_.size()) { + // move the last element out and shorten the vector + nextState = std::move(*(spareStates_.end() - 1)); + spareStates_.pop_back(); + } + + BOOST_VERIFY(std::get(nextState)); + + KUKA::FRI::ClientData &nextClientData = + *std::get(nextState); + KUKA::FRI::ClientData &latestClientData = + *std::get(latestStateForUser_); + + // copy essential data from latestStateForUser_ to nextState + nextClientData.lastState = latestClientData.lastState; + nextClientData.sequenceCounter = latestClientData.sequenceCounter; + nextClientData.lastSendCounter = latestClientData.lastSendCounter; + nextClientData.expectedMonitorMsgID = + latestClientData.expectedMonitorMsgID; + + // copy command from latestCommandBackup to nextState aka + // nextClientData + KUKA::FRI::ClientData &latestCommandBackupClientData = + *std::get(latestCommandBackup); + set(nextClientData.commandMsg, + latestCommandBackupClientData.commandMsg); + + // if there are no error codes and we have received data, + // then we can consider the connection established! + /// @todo perhaps data should always send too? + if (!std::get(nextState) && + !std::get(nextState) && + std::get(nextState)) { + isConnectionEstablished_ = true; + } + + ptrMutex_.unlock(); + } + } + + } catch (...) { + // transport the exception to the main thread in a safe manner + exceptionPtr = std::current_exception(); + m_shouldStop = true; + isConnectionEstablished_ = false; + } + + isConnectionEstablished_ = false; + } + + enum LatestStateIndex { + latest_low_level_algorithm_params, + latest_receive_monitor_state, + latest_receive_ec, + latest_receive_bytes_transferred, + latest_send_ec, + latest_send_bytes_transferred, + latest_time_event_data + }; + + /// this is the object that stores all data for the latest device state + /// including the KUKA defined ClientData object, and a grl defined TimeEvent + /// which stores the time data needed for synchronization. + typedef std::tuple, + std::shared_ptr, + boost::system::error_code, std::size_t, + boost::system::error_code, std::size_t, + grl::TimeEvent> LatestState; + + /// Creates a default LatestState Object + static LatestState + make_LatestState(std::shared_ptr lowLevelAlgorithmParams, + std::shared_ptr &clientData) { + return std::make_tuple(lowLevelAlgorithmParams, + clientData, + boost::system::error_code(), std::size_t(), + boost::system::error_code(), std::size_t(), + grl::TimeEvent()); + } + + /// creates a shared_ptr to KUKA::FRI::ClientData with all command message + /// status explicitly set to false + /// @post std::shared_ptr will be non-null + static std::shared_ptr make_shared_valid_ClientData( + std::shared_ptr &friData) { + if (friData.get() == nullptr) { + friData = std::make_shared(KUKA::LBRState::NUM_DOF); + // there is no commandMessage data on a new object + friData->resetCommandMessage(); + } + + return friData; + } + + static std::shared_ptr make_shared_valid_ClientData() { + std::shared_ptr friData; + return make_shared_valid_ClientData(friData); + } + + /// Initialize valid shared ptr to LatestState object with a valid allocated + /// friData. Note that lowLevelAlgorithmParams will remain null! + static LatestState + make_valid_LatestState( + std::shared_ptr lowLevelAlgorithmParams, + std::shared_ptr &friData + ) { + if (!friData) { + friData = make_shared_valid_ClientData(); + } + + return make_LatestState(lowLevelAlgorithmParams,friData); + } + + static LatestState make_valid_LatestState() { + std::shared_ptr emptyLowLevelAlgParams; + std::shared_ptr friData; + return make_valid_LatestState(emptyLowLevelAlgParams,friData); + } + + + Params params_; + + /// @todo replace with unique_ptr + /// the latest state we have available to give to the user + LatestState latestStateForUser_; + LatestState newCommandForDriver_; + + /// should always be valid, never null + boost::container::static_vector spareStates_; + + std::atomic m_shouldStop; + std::exception_ptr exceptionPtr; + std::atomic isConnectionEstablished_; + + /// may be null, allows the user to choose if they want to provide an + /// io_service + std::unique_ptr optional_internal_io_service_P; + + // other things to do somewhere: + // - get list of control points + // - get the control point in the arm base coordinate system + // - load up a configuration file with ip address to send to, etc. + boost::asio::io_service &io_service_; + std::unique_ptr driver_threadP_; + boost::mutex ptrMutex_; + + typename LowLevelStepAlgorithmType::Params step_alg_params_; +}; /// End of KukaFRIClientDataDriver + +} +} +} // namespace grl::robot::arm + +#endif diff --git a/include/grl/kuka/KukaFRIdriver.hpp b/include/grl/kuka/KukaFRIdriver.hpp index b82758c..c6fcafb 100644 --- a/include/grl/kuka/KukaFRIdriver.hpp +++ b/include/grl/kuka/KukaFRIdriver.hpp @@ -3,41 +3,7 @@ #ifndef _KUKA_FRI_DRIVER #define _KUKA_FRI_DRIVER - - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -//#ifdef BOOST_NO_CXX11_ATOMIC_SMART_PTR -#include -//#endif -#include "grl/flatbuffer/flatbuffer.hpp" -// friClientData is found in the kuka connectivity FRI cpp zip file -#include "grl/kuka/Kuka.hpp" -#include "grl/kuka/KukaFRI.hpp" -#include "grl/exception.hpp" -#include "grl/vector_ostream.hpp" -#include "grl/kuka/KukaFRIalgorithm.hpp" -// used for time synchronization -#include "grl/TimeEvent.hpp" - -// To call flatbuffer and toflatbuffer functions - -#include "grl/kuka/KukaToFlatbuffer.hpp" - -/// @todo TODO(ahundt) REMOVE SPDLOG FROM LOW LEVEL CODE -#include -#include +#include "grl/kuka/KukaFRIClientDataDriver.hpp" struct KukaState; @@ -45,875 +11,6 @@ namespace grl { namespace robot { namespace arm { -/// @brief internal function to decode KUKA FRI message buffer (using nanopb -/// decoder) for the KUKA FRI -/// -/// @note encode needs to be updated for each additional supported command type -/// and when updating to newer FRI versions -void decode(KUKA::FRI::ClientData &friData, std::size_t msg_size) { - // The decoder was given a pointer to the monitoringMessage at initialization - if (!friData.decoder.decode(friData.receiveBuffer, msg_size)) { - BOOST_THROW_EXCEPTION(std::runtime_error("Error decoding received FRI data, the message may be missing or corrupted. This error is most likely due to the application running on the KUKA robot's side of the connection going down or disabling FRI, so check the robot and the JAVA side of the system.")); - } - - // check message type - if (friData.expectedMonitorMsgID != - friData.monitoringMsg.header.messageIdentifier) { - BOOST_THROW_EXCEPTION(std::invalid_argument( - std::string("KukaFRI.hpp: Problem reading buffer, id code: ") + - boost::lexical_cast( - static_cast(friData.monitoringMsg.header.messageIdentifier)) + - std::string(" does not match expected id code: ") + - boost::lexical_cast( - static_cast(friData.expectedMonitorMsgID)) + - std::string("\n"))); - return; - } - - friData.lastState = - grl::robot::arm::get(friData.monitoringMsg, KUKA::FRI::ESessionState()); -} - -/// @brief Default LowLevelStepAlgorithmType -/// This algorithm is designed to be changed out -/// @todo Generalize this class using C++ techinques "tag dispatching" and "type -/// traits". See boost.geometry access and coorinate_type classes for examples. -/// Also perhaps make this the outer class which accepts drivers at the template param? -struct LinearInterpolation { - - enum ParamIndex { - JointAngleDest, - TimeDurationToDestMS - }; - - // A variable-size array container with fixed capacity. - // A static_vector is a sequence that supports random access to elements, constant time insertion and removal of elements at the end, and linear time insertion and removal of elements at the beginning or in the middle. - typedef std::tuple,std::size_t> Params; - - // extremely conservative default timeframe to reach destination plus no goal position - static const Params defaultParams() { - boost::container::static_vector nopos; - return std::make_tuple(nopos,10000); - } - /// Default constructor - /// @todo verify this doesn't corrupt the state of the system - LinearInterpolation() { - }; - - // no action by default - template - void lowLevelTimestep(ArmDataType &, CommandModeType &) { - // need to tag dispatch here - BOOST_VERIFY(false); // not yet supported - } - - template - void lowLevelTimestep(ArmData &friData, - revolute_joint_angle_open_chain_command_tag) { - - // no updates if no goal has been set - if(goal_position.size() == 0) return; - // switch (friData_->monitoringMsg.robotInfo.controlMode) { - // case ControlMode_POSITION_CONTROLMODE: - // case ControlMode_JOINT_IMPEDANCE_CONTROLMODE: - - KukaState::joint_state ipoJointPos; - KukaState::joint_state currentJointPos; - KukaState::joint_state currentMinusIPOJointPos; - KukaState::joint_state goalPlusIPOJointPos; - KukaState::joint_state diffToGoal; - KukaState::joint_state amountToMove; - KukaState::joint_state commandToSend; - KukaState::joint_state commandToSendPlusIPOJointPos; - - double ripoJointPos[7]; - double rcurrentJointPos[7]; - double rcurrentMinusIPOJointPos[7]; - double rgoalPlusIPOJointPos[7]; - double rcommandedGoal[7]; - double rdiffToGoal[7]; - double ramountToMove[7]; - double rcommandToSend[7]; - double rvelocity_limits[7]; - double rcommandToSendPlusIPOJointPos[7]; - - // the current "holdposition" joint angles - /// @todo maybe this should be the - /// revolute_joint_angle_interpolated_open_chain_state_tag()? @see - /// kukaFRIalgorithm.hpp - grl::robot::arm::copy(friData.monitoringMsg, - std::back_inserter(currentJointPos), - revolute_joint_angle_open_chain_state_tag()); - grl::robot::arm::copy(friData.monitoringMsg, - std::back_inserter(ipoJointPos), - revolute_joint_angle_interpolated_open_chain_state_tag()); - - // copy value for debugging - boost::copy(ipoJointPos, &ripoJointPos[0]); - boost::copy(currentJointPos, &rcurrentJointPos[0]); - boost::copy(goal_position, &rcommandedGoal[0]); - - boost::transform(currentJointPos, ipoJointPos, - std::back_inserter(currentMinusIPOJointPos), std::minus()); - boost::transform(goal_position, ipoJointPos, - std::back_inserter(goalPlusIPOJointPos), std::plus()); - - - boost::copy(currentMinusIPOJointPos, &rcurrentMinusIPOJointPos[0]); - boost::copy(goal_position, &rcommandedGoal[0]); - boost::copy(goalPlusIPOJointPos, &rgoalPlusIPOJointPos[0]); - - // only move if there is time left to reach the goal - if(goal_position_command_time_duration_remaining > 0) - { - // single timestep in ms - int thisTimeStepMS(grl::robot::arm::get(friData.monitoringMsg, grl::time_step_tag())); - double thisTimeStepS = (static_cast(thisTimeStepMS) / 1000); - //double secondsPerTick = std::chrono::duration_cast(thisTimeStep).count(); - - // the fraction of the distance to the goal that should be traversed this - // tick - double fractionOfDistanceToTraverse = - static_cast(thisTimeStepMS) / - static_cast(goal_position_command_time_duration_remaining); - - // makes viewing in a debugger easier - boost::copy(goal_position, &rcommandedGoal[0]); - // get the angular distance to the goal - // use current time and time to destination to interpolate (scale) goal - // joint position - boost::transform(goal_position, currentJointPos, - std::back_inserter(diffToGoal), - [&](double commanded_angle, double current_angle) { - return (commanded_angle - current_angle) * - fractionOfDistanceToTraverse; - }); - boost::copy(diffToGoal, &rdiffToGoal[0]); - - goal_position_command_time_duration_remaining -= thisTimeStepMS; - - /// @todo correctly pass velocity limits from outside, use "copy" fuction in - /// Kuka.hpp, correctly account for differing robot models. This *should* - /// be in KukaFRIdriver at the end of this file. - - // R820 velocity limits - // A1 - 85 °/s == 1.483529864195 rad/s - // A2 - 85 °/s == 1.483529864195 rad/s - // A3 - 100 °/s == 1.745329251994 rad/s - // A4 - 75 °/s == 1.308996938996 rad/s - // A5 - 130 °/s == 2.268928027593 rad/s - // A6 - 135 °/s == 2.356194490192 rad/s - // A1 - 135 °/s == 2.356194490192 rad/s - KukaState::joint_state velocity_limits; - velocity_limits.push_back(1.483529864195*thisTimeStepS); - velocity_limits.push_back(1.483529864195*thisTimeStepS); - velocity_limits.push_back(1.745329251994*thisTimeStepS); - velocity_limits.push_back(1.308996938996*thisTimeStepS); - velocity_limits.push_back(2.268928027593*thisTimeStepS); - velocity_limits.push_back(2.356194490192*thisTimeStepS); - velocity_limits.push_back(2.356194490192*thisTimeStepS); - - boost::copy(velocity_limits, &rvelocity_limits[0]); - // clamp the commanded velocities to below the system limits - // use std::min to ensure commanded change in position remains under the - // maximum possible velocity for a single timestep - boost::transform( - diffToGoal, velocity_limits, std::back_inserter(amountToMove), - [&](double diff, double maxvel) { - return boost::math::copysign(std::min(std::abs(diff), maxvel), diff); - }); - - boost::copy(amountToMove, &ramountToMove[0]); - - - // add the current joint position to the amount to move to get the actual - // position command to send - boost::transform(currentJointPos, amountToMove, - std::back_inserter(commandToSend), std::plus()); - - boost::copy(currentMinusIPOJointPos, &rcurrentMinusIPOJointPos[0]); - boost::copy(commandToSend, &rcommandToSend[0]); - - - boost::transform(commandToSend, ipoJointPos, - std::back_inserter(commandToSendPlusIPOJointPos), std::plus()); - - - boost::copy(commandToSendPlusIPOJointPos, &rcommandToSendPlusIPOJointPos[0]); - // send the command -// grl::robot::arm::set(friData.commandMsg, commandToSend, -// grl::revolute_joint_angle_open_chain_command_tag()); - // send the command - grl::robot::arm::set(friData.commandMsg, commandToSend, - grl::revolute_joint_angle_open_chain_command_tag()); - } - // break; - } - - void setGoal(const Params& params ) { - /// @todo TODO(ahundt) support param tag structs for additional control modes - goal_position_command_time_duration_remaining = std::get(params); - goal_position = std::get(params); - - } - - /// @todo look in FRI_Client_SDK_Cpp.zip to see if position must be set for - /// joint torques. Ref files: LBRTorqueSineOverlayClient.cpp, - /// LBRTorqueSineOverlayClient.h, friLBRCommand.cpp, friLBRCommand.h - template - void lowLevelTimestep(ArmData &friData, - revolute_joint_torque_open_chain_command_tag) { - - //not yet supported - BOOST_VERIFY(false); - // case ControlMode_JOINT_IMPEDANCE_CONTROLMODE: -// grl::robot::arm::set(friData.commandMsg, armState.commandedTorque, -// grl::revolute_joint_torque_open_chain_command_tag()); - - /// @note encode() needs to be updated for each additional supported command - /// type - // break; - } - - /// @todo look in FRI_Client_SDK_Cpp.zip to see if position must be set for - /// cartesian wrench. Ref files: LBRWrenchSineOverlayClient.cpp, - /// LBRWrenchSineOverlayClient.h, friLBRCommand.cpp, friLBRCommand.h - template - void lowLevelTimestep(ArmData &friData, cartesian_wrench_command_tag) { - - //not yet supported - BOOST_VERIFY(false); - // case ControlMode_CARTESIAN_IMPEDANCE_CONTROLMODE: - // not yet supported -// grl::robot::arm::set(friData.commandMsg, -// armState.commandedCartesianWrenchFeedForward, -// grl::cartesian_wrench_command_tag()); - // break; - } - - /// @todo make this accessible via a nonmember function - bool hasCommandData() { - /// @todo check if duration remaining should be greater than zero or greater - /// than the last tick size - return goal_position_command_time_duration_remaining > 0; - } - // template - // void operator()(ArmData& clientData, - // revolute_joint_angle_open_chain_command_tag){ - // default: - // break; - // } -private: - // the armstate at initialization of this object - KukaState::joint_state velocity_limits; - KukaState::joint_state goal_position; - double goal_position_command_time_duration_remaining; // milliseconds -}; - -/// @brief encode data in the class KUKA::FRI::ClientData into the send buffer -/// for the KUKA FRI. -/// this preps the information for transport over the network -/// -/// @note encode needs to be updated for each additional supported command type -/// and when updating to newer FRI versions -template -std::size_t encode(LowLevelStepAlgorithmType &step_alg, - KUKA::FRI::ClientData &friData, - boost::system::error_code &ec) { - // reset send counter - friData.lastSendCounter = 0; - - // set sequence counters - friData.commandMsg.header.sequenceCounter = friData.sequenceCounter++; - friData.commandMsg.header.reflectedSequenceCounter = - friData.monitoringMsg.header.sequenceCounter; - - KUKA::FRI::ESessionState sessionState = - grl::robot::arm::get(friData.monitoringMsg, KUKA::FRI::ESessionState()); - - if ((step_alg.hasCommandData() && - (sessionState == KUKA::FRI::COMMANDING_WAIT || - sessionState == KUKA::FRI::COMMANDING_ACTIVE))) { - KUKA::FRI::EClientCommandMode commandMode = grl::robot::arm::get( - friData.monitoringMsg, KUKA::FRI::EClientCommandMode()); - switch (commandMode) { - case ClientCommandMode_POSITION: - step_alg.lowLevelTimestep(friData, revolute_joint_angle_open_chain_command_tag()); - break; - case ClientCommandMode_WRENCH: - step_alg.lowLevelTimestep(friData, cartesian_wrench_command_tag()); - break; - case ClientCommandMode_TORQUE: - step_alg.lowLevelTimestep(friData, revolute_joint_torque_open_chain_command_tag()); - break; - default: - // this is unhandled at the moment... - BOOST_VERIFY(false); - // BOOST_THROW_EXCEPTION_CURRENT_FUNCTION; - // ClientCommandMode_NO_COMMAND_MODE, anything else that is added in the - // future and unimplemented - /// @todo do nothing if in an unsupported command mode? Or do the same as - /// the next else if step? - break; - } - - } else if (!(friData.commandMsg.has_commandData && - step_alg.hasCommandData() && - (sessionState == KUKA::FRI::COMMANDING_WAIT || - sessionState == KUKA::FRI::COMMANDING_ACTIVE))) { - // copy current measured joint position to commanded position only if we - // *don't* have new command data - - /// @todo should this be different if it is in torque mode? - /// @todo allow copying of data directly between commandmsg and - /// monitoringMsg - std::vector msg; - copy(friData.monitoringMsg, std::back_inserter(msg), - revolute_joint_angle_open_chain_command_tag()); - // copy the previously recorded command over - set(friData.commandMsg, msg, - grl::revolute_joint_angle_open_chain_command_tag()); - } - - int buffersize = KUKA::FRI::FRI_COMMAND_MSG_MAX_SIZE; - if (!friData.encoder.encode(friData.sendBuffer, buffersize)) { - // @todo figure out PB_GET_ERROR, integrate with error_code type supported - // by boost - ec = boost::system::errc::make_error_code(boost::system::errc::bad_message); - return 0; - } - - return buffersize; -} - -/// @brief Actually talk over the network to receive an update and send out a -/// new KUKA FRI command -/// -/// Receives an update, performs the necessary checks, then sends a message if -/// appropriate. -/// -/// @pre socket must already have the endpoint resolved and "connected". While -/// udp is technically stateless the asio socket supports the connection api -/// components for convenience. -template -void update_state(boost::asio::ip::udp::socket &socket, - LowLevelStepAlgorithmType &step_alg, - KUKA::FRI::ClientData &friData, - boost::system::error_code &receive_ec, - std::size_t &receive_bytes_transferred, - boost::system::error_code &send_ec, - std::size_t &send_bytes_transferred, - grl::TimeEvent& timeEvent, - boost::asio::ip::udp::endpoint sender_endpoint = boost::asio::ip::udp::endpoint()) { - - static const int message_flags = 0; - // get a local clock timestamp, then the latest frame from the device, then another timestamp - timeEvent.local_request_time = cartographer::common::UniversalTimeScaleClock::now(); - receive_bytes_transferred = socket.receive_from( - boost::asio::buffer(friData.receiveBuffer, - KUKA::FRI::FRI_MONITOR_MSG_MAX_SIZE), - sender_endpoint, message_flags, receive_ec); - timeEvent.local_receive_time = cartographer::common::UniversalTimeScaleClock::now(); - decode(friData, receive_bytes_transferred); - - friData.lastSendCounter++; - // Check whether to send a response - if (friData.lastSendCounter >= - friData.monitoringMsg.connectionInfo.receiveMultiplier) { - send_bytes_transferred = encode(step_alg, friData, send_ec); - if (send_ec) - return; - socket.send(boost::asio::buffer(friData.sendBuffer, send_bytes_transferred), - message_flags, send_ec); - } -} - - -/// @brief don't use this -/// @deprecated this is an old implemenation that will be removed in the future -void copy(const FRIMonitoringMessage &monitoringMsg, KukaState &state) { - state.clear(); - copy(monitoringMsg, std::back_inserter(state.position), - revolute_joint_angle_open_chain_state_tag()); - copy(monitoringMsg, std::back_inserter(state.torque), - revolute_joint_torque_open_chain_state_tag()); - copy(monitoringMsg, std::back_inserter(state.commandedPosition), - revolute_joint_angle_open_chain_command_tag()); - copy(monitoringMsg, std::back_inserter(state.commandedTorque), - revolute_joint_torque_open_chain_command_tag()); - copy(monitoringMsg, std::back_inserter(state.ipoJointPosition), - revolute_joint_angle_interpolated_open_chain_state_tag()); - state.sessionState = static_cast( - get(monitoringMsg, KUKA::FRI::ESessionState())); - state.connectionQuality = static_cast( - get(monitoringMsg, KUKA::FRI::EConnectionQuality())); - state.safetyState = static_cast( - get(monitoringMsg, KUKA::FRI::ESafetyState())); - state.operationMode = static_cast( - get(monitoringMsg, KUKA::FRI::EOperationMode())); - state.driveState = static_cast( - get(monitoringMsg, KUKA::FRI::EDriveState())); - - /// @todo fill out missing state update steps -} - -/// @brief Simple low level driver to communicate over the Kuka iiwa FRI -/// interface using KUKA::FRI::ClientData status objects -/// -/// @note If you aren't sure, see KukaDriver in KukaDriver.hpp. -/// -/// @note If you want to change how the lowest level high rate updates are -/// performed see KukaFRIdriver -/// -/// One important aspect of this design is the is_running_automatically flag. If -/// you are unsure, -/// the suggested default is run_automatically (true/enabled). When it is -/// enabled, -/// the driver will create a thread internally and run the event loop -/// (io_service) itself. -/// If run manually, you are expected to call io_service.run() on the io_service -/// you provide, -/// or on the run() member function. When running manually you are also expected -/// to call -/// async_getLatestState(handler) frequently enought that the 5ms response -/// requirement of the KUKA -/// FRI interface is met. -template -class KukaFRIClientDataDriver - : public std::enable_shared_from_this< - KukaFRIClientDataDriver>, - public KukaUDP { - -public: - using KukaUDP::ParamIndex; - using KukaUDP::ThreadingRunMode; - using KukaUDP::Params; - using KukaUDP::defaultParams; - - KukaFRIClientDataDriver(boost::asio::io_service &ios, - Params params = defaultParams()) - : params_(params), m_shouldStop(false), isConnectionEstablished_(false), - io_service_(ios) - //,socketP_(std::move(connect(params, io_service_,sender_endpoint_))) ///< - //@todo this breaks the assumption that the object can be constructed without - // hardware issues being a porblem - { - construct(params); - } - - KukaFRIClientDataDriver(Params params = defaultParams()) - : params_(params), m_shouldStop(false), isConnectionEstablished_(false), - optional_internal_io_service_P(new boost::asio::io_service), - io_service_(*optional_internal_io_service_P) - //,socketP_(std::move(connect(params, io_service_,sender_endpoint_))) ///< - //@todo this breaks the assumption that the object can be constructed without - // hardware issues being a porblem - { - construct(params); - } - - /// Call this to initialize the object after the constructor has been called - void construct(Params params = defaultParams()) { - try { - - /////////// - // initialize all of the states - latestStateForUser_ = make_valid_LatestState(); - spareStates_.emplace_back(std::move(make_valid_LatestState())); - spareStates_.emplace_back(std::move(make_valid_LatestState())); - - // start up the driver thread since the io_service_ is internal only - if (std::get(params)) { - driver_threadP_.reset(new std::thread([&] { update(); })); - } - - } catch (boost::exception &e) { - add_details_to_connection_error(e, params); - throw; - } - } - - /// @brief blocking call to communicate with the robot continuously - /// @pre construct() should be called before run() - void run() { update(); } - - /// @brief Updates the passed friData shared pointer to a pointer to newly - /// updated data, plus any errors that occurred. - /// - /// We recommend you supply a valid shared_ptr to friData, even if all command - /// elements are set to false. - /// The friData pointer you pass in can contain a command to send to the arm. - /// To update with new state and optional input state, you give up lifetime - /// control of the input, - /// and assume liftime control of the output. - /// - /// This function is designed for single threaded use to quickly receive and - /// send "non-blocking" updates to the robot. - /// It is not thread safe cannot be called simultaneously from multiple - /// threads. - /// - /// - /// @note { An error code is set if update_state is called with no new data - /// available. - /// In this special case, all error codes and bytes_transferred are 0, - /// because - /// there was no new data available for the user. - /// } - /// - /// @warning Do not pound this call continuously in a very tight loop, because - /// then the driver won't be able to acquire the lock and send updates to the - /// robot. - /// - /// @param[in,out] friData supply a new command, receive a new update of the - /// robot state. Pointer is null if no new data is available. - /// - /// @pre If friData!=nullptr it is assumed valid for use and this class will - /// take control of the object. - /// - /// @return isError = false if you have new data, true when there is either an - /// error or no new data - bool update_state(std::shared_ptr &step_alg_params, - std::shared_ptr &friData, - boost::system::error_code &receive_ec, - std::size_t &receive_bytes_transferred, - boost::system::error_code &send_ec, - std::size_t &send_bytes_transferred, - grl::TimeEvent& timeEvent) { - - if (exceptionPtr) { - /// @note this exception most likely came from the update() call running - /// the kuka driver - std::rethrow_exception(exceptionPtr); - } - - bool haveNewData = false; - - if (!isConnectionEstablished_ || - !std::get(latestStateForUser_)) { - // no new data, so immediately return results accordingly - // Constructs a tuple object whose elements are references to the arguments in args std::tie (Types&... args) , in the same order. - // This allows a set of objects to act as a tuple, which is especially useful to unpack tuple objects. - std::tie(step_alg_params, friData, receive_ec, receive_bytes_transferred, send_ec, - send_bytes_transferred, timeEvent) = make_LatestState(step_alg_params,friData); - return !haveNewData; - } - - // ensure we have valid data for future updates - // need to copy this over because friData will be set as an output value - // later - // and allocate/initialize data if null - auto validFriDataLatestState = make_valid_LatestState(step_alg_params,friData); - - // get the latest state from the driver thread - { - boost::lock_guard lock(ptrMutex_); - - // get the update if one is available - // the user has provided new data to send to the device - if (std::get(validFriDataLatestState) - ->commandMsg.has_commandData) { - std::swap(validFriDataLatestState, newCommandForDriver_); - } - // newCommandForDriver_ is finalized - - if (spareStates_.size() < spareStates_.capacity() && - std::get(validFriDataLatestState)) { - spareStates_.emplace_back(std::move(validFriDataLatestState)); - } - - if (std::get(latestStateForUser_)) { - // return the latest state to the caller - std::tie(step_alg_params,friData, receive_ec, receive_bytes_transferred, send_ec, - send_bytes_transferred, timeEvent) = std::move(latestStateForUser_); - // TODO(chunting) consider adding to log buffer here, data is in latestStateForUser_, alternate might be latestClientData - - - - - - haveNewData = true; - } else if (std::get( - validFriDataLatestState)) { - // all storage is full, return the spare data to the user - std::tie(step_alg_params, friData, receive_ec, receive_bytes_transferred, send_ec, - send_bytes_transferred, timeEvent) = validFriDataLatestState; - } - } - - // let the user know if we aren't in the best possible state - return !haveNewData || receive_bytes_transferred == 0 || receive_ec || - send_ec; - } - - void destruct() { - m_shouldStop = true; - if (driver_threadP_) { - driver_threadP_->join(); - } - } - - ~KukaFRIClientDataDriver() { destruct(); } - - /// Is everything ok? - /// @return true if the kuka fri connection is actively running without any - /// issues - /// @todo consider expanding to support real error codes - bool is_active() { return !exceptionPtr && isConnectionEstablished_; } - -private: - /// Reads data off of the real kuka fri device in a separate thread - /// @todo consider switching to single producer single consumer queue to avoid - /// locking overhead, but keep latency in mind - /// https://github.com/facebook/folly/blob/master/folly/docs/ProducerConsumerQueue.md - void update() { - try { - - LowLevelStepAlgorithmType step_alg; - /// nextState is the object currently being loaded with data off the - /// network - /// the driver thread should access this exclusively in update() - LatestState nextState = make_valid_LatestState(); - LatestState latestCommandBackup = make_valid_LatestState(); - - boost::asio::ip::udp::endpoint sender_endpoint; - boost::asio::ip::udp::socket socket( - connect(params_, io_service_, sender_endpoint)); - KukaState kukastate; ///< @todo TODO(ahundt) remove this line when new - /// api works completely since old one is deprecated - - ///////////// - // run the primary update loop in a separate thread - while (!m_shouldStop) { - /// @todo maybe there is a more convienient way to set this that is - /// easier for users? perhaps initializeClientDataForiiwa()? - - // nextState and latestCommandBackup should never be null - BOOST_VERIFY(std::get(nextState)); - BOOST_VERIFY( - std::get(latestCommandBackup)); - - // set the flag that must always be there - std::get(nextState) - ->expectedMonitorMsgID = KUKA::LBRState::LBRMONITORMESSAGEID; - - auto lowLevelAlgorithmParamP = std::get(nextState); - - // if there is a valid low level algorithm param command set the new goal - if(lowLevelAlgorithmParamP) step_alg.setGoal(*lowLevelAlgorithmParamP); - - // actually talk over the network to receive an update and send out a - // new command - grl::robot::arm::update_state( - socket, step_alg, - *std::get(nextState), - std::get(nextState), - std::get(nextState), - std::get(nextState), - std::get(nextState), - std::get(nextState)); - - /// @todo use atomics to eliminate the global mutex lock for this object - // lock the mutex to communicate with the user thread - // if it cannot lock, simply send the previous message - // again - if (ptrMutex_.try_lock()) { - - ////////////////////////////////////////////// - // right now this is the state of everything: - ////////////////////////////////////////////// - // - // Always Valid: - // - // nextState: valid, contains the latest update - // latestCommandBackup: should always be valid (though hasCommand - // might be false) - // - // Either Valid or Null: - // latestStateForUser_ : null if the user took data out, valid - // otherwise - // newCommandForDriver_: null if there is no new command data from - // the user, vaild otherwise - - // 1) set the outgoing latest state for the user to pick up - // latestStateForUser_ is finalized - std::swap(latestStateForUser_, nextState); - - // 2) get a new incoming command if available and set incoming command - // variable to null - if (std::get(newCommandForDriver_)) { - // 3) back up the new incoming command - // latestCommandBackup is finalized, newCommandForDriver_ needs to - // be cleared out - std::swap(latestCommandBackup, newCommandForDriver_); - - // nextState may be null - if (!std::get(nextState)) { - nextState = std::move(newCommandForDriver_); - } else if (!(spareStates_.size() == spareStates_.capacity())) { - spareStates_.emplace_back(std::move(newCommandForDriver_)); - } else { - std::get(newCommandForDriver_) - .reset(); - } - } - - // finalized: latestStateForUser_, latestCommandBackup, - // newCommandForDriver_ is definitely null - // issues to be resolved: - // nextState: may be null right now, and it should be valid - // newCommandForDriver_: needs to be cleared with 100% certainty - BOOST_VERIFY(spareStates_.size() > 0); - - if (!std::get(nextState) && - spareStates_.size()) { - // move the last element out and shorten the vector - nextState = std::move(*(spareStates_.end() - 1)); - spareStates_.pop_back(); - } - - BOOST_VERIFY(std::get(nextState)); - - KUKA::FRI::ClientData &nextClientData = - *std::get(nextState); - KUKA::FRI::ClientData &latestClientData = - *std::get(latestStateForUser_); - - // copy essential data from latestStateForUser_ to nextState - nextClientData.lastState = latestClientData.lastState; - nextClientData.sequenceCounter = latestClientData.sequenceCounter; - nextClientData.lastSendCounter = latestClientData.lastSendCounter; - nextClientData.expectedMonitorMsgID = - latestClientData.expectedMonitorMsgID; - - // copy command from latestCommandBackup to nextState aka - // nextClientData - KUKA::FRI::ClientData &latestCommandBackupClientData = - *std::get(latestCommandBackup); - set(nextClientData.commandMsg, - latestCommandBackupClientData.commandMsg); - - // if there are no error codes and we have received data, - // then we can consider the connection established! - /// @todo perhaps data should always send too? - if (!std::get(nextState) && - !std::get(nextState) && - std::get(nextState)) { - isConnectionEstablished_ = true; - } - - ptrMutex_.unlock(); - } - } - - } catch (...) { - // transport the exception to the main thread in a safe manner - exceptionPtr = std::current_exception(); - m_shouldStop = true; - isConnectionEstablished_ = false; - } - - isConnectionEstablished_ = false; - } - - enum LatestStateIndex { - latest_low_level_algorithm_params, - latest_receive_monitor_state, - latest_receive_ec, - latest_receive_bytes_transferred, - latest_send_ec, - latest_send_bytes_transferred, - latest_time_event_data - }; - - /// this is the object that stores all data for the latest device state - /// including the KUKA defined ClientData object, and a grl defined TimeEvent - /// which stores the time data needed for synchronization. - typedef std::tuple, - std::shared_ptr, - boost::system::error_code, std::size_t, - boost::system::error_code, std::size_t, - grl::TimeEvent> LatestState; - - /// Creates a default LatestState Object - static LatestState - make_LatestState(std::shared_ptr lowLevelAlgorithmParams, - std::shared_ptr &clientData) { - return std::make_tuple(lowLevelAlgorithmParams, - clientData, - boost::system::error_code(), std::size_t(), - boost::system::error_code(), std::size_t(), - grl::TimeEvent()); - } - - /// creates a shared_ptr to KUKA::FRI::ClientData with all command message - /// status explicitly set to false - /// @post std::shared_ptr will be non-null - static std::shared_ptr make_shared_valid_ClientData( - std::shared_ptr &friData) { - if (friData.get() == nullptr) { - friData = std::make_shared(KUKA::LBRState::NUM_DOF); - // there is no commandMessage data on a new object - friData->resetCommandMessage(); - } - - return friData; - } - - static std::shared_ptr make_shared_valid_ClientData() { - std::shared_ptr friData; - return make_shared_valid_ClientData(friData); - } - - /// Initialize valid shared ptr to LatestState object with a valid allocated - /// friData. Note that lowLevelAlgorithmParams will remain null! - static LatestState - make_valid_LatestState( - std::shared_ptr lowLevelAlgorithmParams, - std::shared_ptr &friData - ) { - if (!friData) { - friData = make_shared_valid_ClientData(); - } - - return make_LatestState(lowLevelAlgorithmParams,friData); - } - - static LatestState make_valid_LatestState() { - std::shared_ptr emptyLowLevelAlgParams; - std::shared_ptr friData; - return make_valid_LatestState(emptyLowLevelAlgParams,friData); - } - - - Params params_; - - /// @todo replace with unique_ptr - /// the latest state we have available to give to the user - LatestState latestStateForUser_; - LatestState newCommandForDriver_; - - /// should always be valid, never null - boost::container::static_vector spareStates_; - - std::atomic m_shouldStop; - std::exception_ptr exceptionPtr; - std::atomic isConnectionEstablished_; - - /// may be null, allows the user to choose if they want to provide an - /// io_service - std::unique_ptr optional_internal_io_service_P; - - // other things to do somewhere: - // - get list of control points - // - get the control point in the arm base coordinate system - // - load up a configuration file with ip address to send to, etc. - boost::asio::io_service &io_service_; - std::unique_ptr driver_threadP_; - boost::mutex ptrMutex_; - - typename LowLevelStepAlgorithmType::Params step_alg_params_; -}; /// End of KukaFRIClientDataDriver - /// @brief Primary Kuka FRI driver, only talks over realtime network FRI KONI /// ethernet port /// @@ -965,489 +62,498 @@ class KukaFRIdriver : public std::enable_shared_from_this< public KukaUDP { public: - using KukaUDP::ParamIndex; // enum, define some connection parameters, such as ip, port... - using KukaUDP::ThreadingRunMode; - using KukaUDP::Params; // std::tuple, contains the information needed to connect to the robot. - using KukaUDP::defaultParams; // A method to assign Params with defaut values. - - KukaFRIdriver(Params params = defaultParams()) : params_(params) {} - - // KukaFRIdriver(boost::asio::io_service& - // device_driver_io_service__,Params params = defaultParams()) - // : - // device_driver_io_service(device_driver_io_service__), - // params_(params) - // {} - - void construct() { construct(params_); } - - /// @todo create a function that calls simGetObjectHandle and throws an - /// exception when it fails - /// @warning getting the ik group is optional, so it does not throw an - /// exception - void construct(Params params) { - - params_ = params; - // keep driver threads from exiting immediately after creation, because they - // have work to do! - // boost::asio::io_service::work: Constructor notifies the io_service that work is starting. - device_driver_workP_.reset( - new boost::asio::io_service::work(device_driver_io_service)); - - kukaFRIClientDataDriverP_.reset( - new grl::robot::arm::KukaFRIClientDataDriver( - device_driver_io_service, - std::make_tuple(std::string(std::get(params)), - std::string(std::get(params)), - std::string(std::get(params)), - std::string(std::get(params)), - std::string(std::get(params)), - grl::robot::arm::KukaFRIClientDataDriver< - LowLevelStepAlgorithmType>::run_automatically)) - - ); - } + using KukaUDP::ParamIndex; // enum, define some connection parameters, such as ip, port... + using KukaUDP::ThreadingRunMode; + using KukaUDP::Params; // std::tuple, contains the information needed to connect to the robot. + using KukaUDP::defaultParams; // A method to assign Params with defaut values. + + KukaFRIdriver(Params params = defaultParams()) : params_(params), m_shouldStop(false) {} + + // KukaFRIdriver(boost::asio::io_service& + // device_driver_io_service__,Params params = defaultParams()) + // : + // device_driver_io_service(device_driver_io_service__), + // params_(params) + // {} + + void construct() { construct(params_); } + + /// @todo create a function that calls simGetObjectHandle and throws an + /// exception when it fails + /// @warning getting the ik group is optional, so it does not throw an + /// exception + void construct(Params params) { + + params_ = params; + // keep driver threads from exiting immediately after creation, because they + // have work to do! + // boost::asio::io_service::work: Constructor notifies the io_service that work is starting. + device_driver_workP_.reset( + new boost::asio::io_service::work(device_driver_io_service)); + + kukaFRIClientDataDriverP_.reset( + new grl::robot::arm::KukaFRIClientDataDriver( + device_driver_io_service, + std::make_tuple(std::string(std::get(params)), + std::string(std::get(params)), + std::string(std::get(params)), + std::string(std::get(params)), + std::string(std::get(params)), + grl::robot::arm::KukaFRIClientDataDriver< + LowLevelStepAlgorithmType>::run_automatically)) + + ); + m_driverThread.reset(new std::thread(&KukaFRIdriver::update, this)); + } - const Params &getParams() { return params_; } + const Params &getParams() { return params_; } - ~KukaFRIdriver() { - device_driver_workP_.reset(); + ~KukaFRIdriver() { + device_driver_workP_.reset(); - if (driver_threadP) { - device_driver_io_service.stop(); - driver_threadP->join(); + if (driver_threadP) { + device_driver_io_service.stop(); + driver_threadP->join(); + } } - } - /// gets the number of seconds in one message exchange "tick" aka "cycle", - /// "time step" of the robot arm's low level controller - double getSecondsPerTick() { - return std::chrono::duration_cast( - std::chrono::milliseconds(grl::robot::arm::get( - friData_->monitoringMsg, grl::time_step_tag()))) - .count(); - } + /// gets the number of seconds in one message exchange "tick" aka "cycle", + /// "time step" of the robot arm's low level controller + double getSecondsPerTick() { + return std::chrono::duration_cast( + std::chrono::milliseconds(grl::robot::arm::get( + friData_->monitoringMsg, grl::time_step_tag()))).count(); + } - /// @todo make this configurable for different specific robots. Currently set - /// for kuka lbr iiwa 14kg R820 - KukaState::joint_state getMaxVel() { - KukaState::joint_state maxVel; - /// get max velocity constraint parameter for this robot model - copy(std::get(params_), std::back_inserter(maxVel), - grl::revolute_joint_velocity_open_chain_state_constraint_tag()); - - // scale velocity down to a single timestep. In other words multiply each - // velocity by the number of seconds in a tick, likely 0.001-0.005 - boost::transform( - maxVel, maxVel.begin(), - std::bind2nd(std::multiplies(), - getSecondsPerTick())); - - return maxVel; - } + /// @todo make this configurable for different specific robots. Currently set + /// for kuka lbr iiwa 14kg R820 + KukaState::joint_state getMaxVel() { + KukaState::joint_state maxVel; + /// get max velocity constraint parameter for this robot model + copy(std::get(params_), std::back_inserter(maxVel), + grl::revolute_joint_velocity_open_chain_state_constraint_tag()); - /** - * spin once, this is what you call each time you synchronize the client with - * the robot over UDP - * it is expected to be called at least once per send_period_millisec, which - * is the time between - * each FRI udp packet. - * - */ - bool run_one() { - grl::TimeEvent time_event_stamp; - // note: this one sends *and* receives the joint data! - BOOST_VERIFY(kukaFRIClientDataDriverP_.get() != nullptr); - /// @todo use runtime calculation of NUM_JOINTS instead of constant - if (!friData_) { - friData_ = std::make_shared(KUKA::LBRState::NUM_DOF); - } + // scale velocity down to a single timestep. In other words multiply each + // velocity by the number of seconds in a tick, likely 0.001-0.005 + boost::transform( + maxVel, maxVel.begin(), + std::bind2nd(std::multiplies(), + getSecondsPerTick())); - bool haveNewData = false; + return maxVel; + } - static const std::size_t minimumConsecutiveSuccessesBeforeSendingCommands = 100; + /** + * spin once, this is what you call each time you synchronize the client with + * the robot over UDP + * it is expected to be called at least once per send_period_millisec, which + * is the time between + * each FRI udp packet. + * + */ + bool run_one() { + grl::TimeEvent time_event_stamp; + // note: this one sends *and* receives the joint data! + BOOST_VERIFY(kukaFRIClientDataDriverP_.get() != nullptr); + /// @todo use runtime calculation of NUM_JOINTS instead of constant + if (!friData_) { + friData_ = std::make_shared(KUKA::LBRState::NUM_DOF); + } - std::shared_ptr lowLevelStepAlgorithmCommandParamsP; + bool haveNewData = false; + + static const std::size_t minimumConsecutiveSuccessesBeforeSendingCommands = 100; + + std::shared_ptr lowLevelStepAlgorithmCommandParamsP; + + /// @todo probably only need to set this once + armState.velocity_limits.clear(); + armState.velocity_limits = getMaxVel(); + + // This is the key point where the arm's motion goal command is updated and + // sent to the robot + // Set the FRI to the simulated joint positions + if (this->m_haveReceivedRealDataCount > minimumConsecutiveSuccessesBeforeSendingCommands) { + /// @todo TODO(ahundt) Need to eliminate this allocation + boost::lock_guard lock(jt_mutex); + + boost::container::static_vector jointStateToCommand; + boost::copy(armState.commandedPosition,std::back_inserter(jointStateToCommand)); + // pass time to reach specified goal for position control + lowLevelStepAlgorithmCommandParamsP = std::make_shared(std::make_tuple(jointStateToCommand,armState.goal_position_command_time_duration)); + /// @todo construct new low level command object and pass to + /// KukaFRIClientDataDriver + /// this is where we used to setup a new FRI command + + // std::cout << "commandToSend: " << commandToSend << "\n" << + // "currentJointPos: " << currentJointPos << "\n" << "amountToMove: " << + // amountToMove << "\n" << "maxVel: " << maxvel << "\n"; + } else { + /// @todo TODO(ahundt) BUG: Need way to pass time to reach specified goal for position control and eliminate this allocation + lowLevelStepAlgorithmCommandParamsP.reset(new typename LowLevelStepAlgorithmType::Params()); + } - /// @todo probably only need to set this once - armState.velocity_limits.clear(); - armState.velocity_limits = getMaxVel(); + BOOST_VERIFY(lowLevelStepAlgorithmCommandParamsP != nullptr); + boost::system::error_code send_ec, recv_ec; + std::size_t send_bytes, recv_bytes; + // sync with device over network + haveNewData = !kukaFRIClientDataDriverP_->update_state( + lowLevelStepAlgorithmCommandParamsP, friData_, recv_ec, recv_bytes, send_ec, + send_bytes, time_event_stamp); + m_attemptedCommunicationCount++; + + if (haveNewData) { + boost::lock_guard lock(jt_mutex); + // if there were problems sending commands, start by sending the current + // position + // if(this->m_haveReceivedRealDataCount > + // minimumConsecutiveSuccessesBeforeSendingCommands-1) + // { + // boost::lock_guard lock(jt_mutex); + // // initialize arm commands to current arm position + // armState.clearCommands(); + //// armState.commandedPosition.clear(); + //// armState.commandedTorque.clear(); + //// grl::robot::arm::copy(friData_->monitoringMsg, + /// std::back_inserter(armState.commandedPosition), + /// grl::revolute_joint_angle_open_chain_command_tag()); + //// grl::robot::arm::copy(friData_->monitoringMsg, + /// std::back_inserter(armState.commandedTorque) , + /// grl::revolute_joint_torque_open_chain_command_tag()); + // } + + m_attemptedCommunicationConsecutiveSuccessCount++; + this->m_attemptedCommunicationConsecutiveFailureCount = 0; + this->m_haveReceivedRealDataCount++; + + // We have the real kuka state read from the device now + // update real joint angle data + armState.position.clear(); + grl::robot::arm::copy(friData_->monitoringMsg, + std::back_inserter(armState.position), + grl::revolute_joint_angle_open_chain_state_tag()); + + armState.torque.clear(); + grl::robot::arm::copy(friData_->monitoringMsg, + std::back_inserter(armState.torque), + grl::revolute_joint_torque_open_chain_state_tag()); + + armState.externalTorque.clear(); + grl::robot::arm::copy( + friData_->monitoringMsg, std::back_inserter(armState.externalTorque), + grl::revolute_joint_torque_external_open_chain_state_tag()); + + // only supported for kuka sunrise OS 1.9 + #ifdef KUKA_SUNRISE_1_9 + armState.externalForce.clear(); + grl::robot::arm::copy(friData_->monitoringMsg, + std::back_inserter(armState.externalForce), + grl::cartesian_external_force_tag()); + #endif // KUKA_SUNRISE_1_9 + armState.ipoJointPosition.clear(); + grl::robot::arm::copy( + friData_->monitoringMsg, + std::back_inserter(armState.ipoJointPosition), + grl::revolute_joint_angle_interpolated_open_chain_state_tag()); + + armState.sendPeriod = std::chrono::milliseconds( + grl::robot::arm::get(friData_->monitoringMsg, grl::time_step_tag())); + + armState.time_event_stamp = time_event_stamp; + + std::cout << "Measured Torque: "; + std::cout << std::setw(6); + for (float t:armState.torque) { + std::cout << t << " "; + } + std::cout << '\n'; + std::cout << "External Torque: "; + std::cout << std::setw(6); + for (float t:armState.externalTorque) { + std::cout << t << " "; + } + std::cout << '\n'; + std::cout << "External Force: "; + for (float t:armState.externalForce) { + std::cout << t << " "; + } + std::cout << '\n'; + + + // TODO(chunting) add data to log here, when full write to disk on a separate thread like FusionTrackLogAndTrack + save_oneKUKAiiwaStateBuffer(); + // m_driverThread.reset(new std::thread(&KukaFRIdriver::save_recording)); + } else { + m_attemptedCommunicationConsecutiveFailureCount++; + std::cerr << "No new FRI data available, is an FRI application running " + "on the Kuka arm? \n Total sucessful transfers: " + << this->m_haveReceivedRealDataCount + << "\n Total attempts: " << m_attemptedCommunicationCount + << "\n Consecutive Failures: " + << m_attemptedCommunicationConsecutiveFailureCount + << "\n Consecutive Successes: " + << m_attemptedCommunicationConsecutiveSuccessCount << "\n"; + m_attemptedCommunicationConsecutiveSuccessCount = 0; + /// @todo TODO(ahundt) Add time information from update_state call here for debugging purposes + /// @todo TODO(ahundt) should the results of getlatest state even be possible to call + /// without receiving real data? should the library change? + /// @todo TODO(ahundt) use spdlog library instead of cerr? + } + return haveNewData; + } - // This is the key point where the arm's motion goal command is updated and - // sent to the robot - // Set the FRI to the simulated joint positions - if (this->m_haveReceivedRealDataCount > - minimumConsecutiveSuccessesBeforeSendingCommands) { - /// @todo TODO(ahundt) Need to eliminate this allocation + /** + * \brief Set the joint positions for the current interpolation step. + * + * This method is only effective when the robot is in a commanding state + * and the set time point for reaching the destination is in the future. + * This function sets the goal time point for a motion to the epoch, aka "time + * 0" (which is in the past) for safety. + * + * + * For position based motion to work, you must set both the position you want + * and the time you want it to get there. + * This is required because the robot can move extremely fast, so accidentally + * setting the velocity to the max is + * very dangerous. If the time point is in the past, the robot will not move. + * If the time point is too near in the future + * to make it, the robot will move at the max speed towards that position. + * + * @see KukaFRIdriver::set(TimePoint && duration_to_goal_command, time_duration_command_tag) to set + * the destination time point in the future so the position motion can start. + * + * @param state Object which stores the current state of the robot, including + * the command to send next + * @param range Array with the new joint positions (in radians) + * @param tag identifier object indicating that revolute joint angle commands + * should be modified + */ + template + void set(Range &&range, grl::revolute_joint_angle_open_chain_command_tag) { boost::lock_guard lock(jt_mutex); + armState.clearCommands(); + boost::copy(range, std::back_inserter(armState.commandedPosition)); + boost::copy(range, std::back_inserter(armState.commandedPosition_goal)); + } - boost::container::static_vector jointStateToCommand; - boost::copy(armState.commandedPosition,std::back_inserter(jointStateToCommand)); - // pass time to reach specified goal for position control - lowLevelStepAlgorithmCommandParamsP = std::make_shared(std::make_tuple(jointStateToCommand,armState.goal_position_command_time_duration)); - /// @todo construct new low level command object and pass to - /// KukaFRIClientDataDriver - /// this is where we used to setup a new FRI command - - // std::cout << "commandToSend: " << commandToSend << "\n" << - // "currentJointPos: " << currentJointPos << "\n" << "amountToMove: " << - // amountToMove << "\n" << "maxVel: " << maxvel << "\n"; - } else { - /// @todo TODO(ahundt) BUG: Need way to pass time to reach specified goal for position control and eliminate this allocation - lowLevelStepAlgorithmCommandParamsP.reset(new typename LowLevelStepAlgorithmType::Params()); + /** + * @brief Set the time duration expected between new position commands in ms + * + * The driver will likely be updated every so often, such as every 25ms, and + * the lowest level of the + * driver may update even more frequently, such as every 1ms. By providing as + * accurate an + * estimate between high level updates the low level driver can smooth out the + * motion through + * interpolation (the default), or another algorithm. See + * LowLevelStepAlgorithmType template parameter + * in the KukaFRIdriver class if you want to change out the low level + * algorithm. + * + * @see KukaFRIdriver::get(time_duration_command_tag) + * + * @param duration_to_goal_command std::chrono time format representing the + * time duration between updates + * + */ + void set(double duration_to_goal_command, time_duration_command_tag) { + boost::lock_guard lock(jt_mutex); + armState.goal_position_command_time_duration = duration_to_goal_command; } - BOOST_VERIFY(lowLevelStepAlgorithmCommandParamsP != nullptr); - boost::system::error_code send_ec, recv_ec; - std::size_t send_bytes, recv_bytes; - // sync with device over network - haveNewData = !kukaFRIClientDataDriverP_->update_state( - lowLevelStepAlgorithmCommandParamsP, friData_, recv_ec, recv_bytes, send_ec, - send_bytes, time_event_stamp); - m_attemptedCommunicationCount++; + /** + * @brief Get the timestamp of the most recent armState + * + * + * + * @see KukaFRIdriver::set(Range&& range, + * grl::revolute_joint_angle_open_chain_command_tag) + * + */ + // KukaState::time_point_type get(time_point_tag) { + // boost::lock_guard lock(jt_mutex); + // return armState.timestamp; + // } + + cartographer::common::Time get(time_point_tag) { + boost::lock_guard lock(jt_mutex); + return armState.time_event_stamp.device_time; + } - if (haveNewData) { + /** + * \brief Set the applied joint torques for the current interpolation step. + * + * This method is only effective when the client is in a commanding state. + * The ControlMode of the robot has to be joint impedance control mode. The + * Client Command Mode has to be torque. + * + * @param state Object which stores the current state of the robot, including + * the command to send next + * @param torques Array with the applied torque values (in Nm) + * @param tag identifier object indicating that the torqe value command should + * be modified + */ + template + void set(Range &&range, grl::revolute_joint_torque_open_chain_command_tag) { boost::lock_guard lock(jt_mutex); - // if there were problems sending commands, start by sending the current - // position - // if(this->m_haveReceivedRealDataCount > - // minimumConsecutiveSuccessesBeforeSendingCommands-1) - // { - // boost::lock_guard lock(jt_mutex); - // // initialize arm commands to current arm position - // armState.clearCommands(); - //// armState.commandedPosition.clear(); - //// armState.commandedTorque.clear(); - //// grl::robot::arm::copy(friData_->monitoringMsg, - /// std::back_inserter(armState.commandedPosition), - /// grl::revolute_joint_angle_open_chain_command_tag()); - //// grl::robot::arm::copy(friData_->monitoringMsg, - /// std::back_inserter(armState.commandedTorque) , - /// grl::revolute_joint_torque_open_chain_command_tag()); - // } - - m_attemptedCommunicationConsecutiveSuccessCount++; - this->m_attemptedCommunicationConsecutiveFailureCount = 0; - this->m_haveReceivedRealDataCount++; - - // We have the real kuka state read from the device now - // update real joint angle data - armState.position.clear(); - grl::robot::arm::copy(friData_->monitoringMsg, - std::back_inserter(armState.position), - grl::revolute_joint_angle_open_chain_state_tag()); - - armState.torque.clear(); - grl::robot::arm::copy(friData_->monitoringMsg, - std::back_inserter(armState.torque), - grl::revolute_joint_torque_open_chain_state_tag()); - - armState.externalTorque.clear(); - grl::robot::arm::copy( - friData_->monitoringMsg, std::back_inserter(armState.externalTorque), - grl::revolute_joint_torque_external_open_chain_state_tag()); - -// only supported for kuka sunrise OS 1.9 -#ifdef KUKA_SUNRISE_1_9 - armState.externalForce.clear(); - grl::robot::arm::copy(friData_->monitoringMsg, - std::back_inserter(armState.externalForce), - grl::cartesian_external_force_tag()); -#endif // KUKA_SUNRISE_1_9 - armState.ipoJointPosition.clear(); - grl::robot::arm::copy( - friData_->monitoringMsg, - std::back_inserter(armState.ipoJointPosition), - grl::revolute_joint_angle_interpolated_open_chain_state_tag()); - - armState.sendPeriod = std::chrono::milliseconds( - grl::robot::arm::get(friData_->monitoringMsg, grl::time_step_tag())); - - armState.time_event_stamp = time_event_stamp; - - // std::cout << "Measured Torque: "; - // std::cout << std::setw(6); - // for (float t:armState.torque) { - // std::cout << t << " "; - // } - // std::cout << '\n'; - // - // std::cout << "External Torque: "; - // std::cout << std::setw(6); - // for (float t:armState.externalTorque) { - // std::cout << t << " "; - // } - // std::cout << '\n'; - // - // std::cout << "External Force: "; - // for (float t:armState.externalForce) { - // std::cout << t << " "; - // } - // std::cout << '\n'; - - // TODO(chunting) add data to log here, when full write to disk on a separate thread like FusionTrackLogAndTrack - } else { - m_attemptedCommunicationConsecutiveFailureCount++; - std::cerr << "No new FRI data available, is an FRI application running " - "on the Kuka arm? \n Total sucessful transfers: " - << this->m_haveReceivedRealDataCount - << "\n Total attempts: " << m_attemptedCommunicationCount - << "\n Consecutive Failures: " - << m_attemptedCommunicationConsecutiveFailureCount - << "\n Consecutive Successes: " - << m_attemptedCommunicationConsecutiveSuccessCount << "\n"; - m_attemptedCommunicationConsecutiveSuccessCount = 0; - /// @todo TODO(ahundt) Add time information from update_state call here for debugging purposes - /// @todo TODO(ahundt) should the results of getlatest state even be possible to call - /// without receiving real data? should the library change? - /// @todo TODO(ahundt) use spdlog library instead of cerr? + armState.clearCommands(); + boost::copy(range, armState.commandedTorque); } - return haveNewData; - } + /** + * \brief Set the applied wrench vector of the current interpolation step. + * + * The wrench vector consists of: + * [F_x, F_y, F_z, tau_A, tau_B, tau_C] + * + * F ... forces (in N) applied along the Cartesian axes of the + * currently used motion center. + * tau ... torques (in Nm) applied along the orientation angles + * (Euler angles A, B, C) of the currently used motion center. + * + * This method is only effective when the client is in a commanding state. + * The ControlMode of the robot has to be Cartesian impedance control mode. + * The + * Client Command Mode has to be wrench. + * + * @param state object storing the command data that will be sent to the + * physical device + * @param range wrench Applied Cartesian wrench vector, in x, y, z, roll, + * pitch, yaw force measurments. + * @param tag identifier object indicating that the wrench value command + * should be modified + * + * @todo perhaps support some specific more useful data layouts + */ + template + void set(Range &&range, grl::cartesian_wrench_command_tag) { + boost::lock_guard lock(jt_mutex); + armState.clearCommands(); + std::copy(range, armState.commandedCartesianWrenchFeedForward); + } - /** - * \brief Set the joint positions for the current interpolation step. - * - * This method is only effective when the robot is in a commanding state - * and the set time point for reaching the destination is in the future. - * This function sets the goal time point for a motion to the epoch, aka "time - * 0" (which is in the past) for safety. - * - * - * For position based motion to work, you must set both the position you want - * and the time you want it to get there. - * This is required because the robot can move extremely fast, so accidentally - * setting the velocity to the max is - * very dangerous. If the time point is in the past, the robot will not move. - * If the time point is too near in the future - * to make it, the robot will move at the max speed towards that position. - * - * @see KukaFRIdriver::set(TimePoint && duration_to_goal_command, time_duration_command_tag) to set - * the destination time point in the future so the position motion can start. - * - * @param state Object which stores the current state of the robot, including - * the command to send next - * @param range Array with the new joint positions (in radians) - * @param tag identifier object indicating that revolute joint angle commands - * should be modified - */ - template - void set(Range &&range, grl::revolute_joint_angle_open_chain_command_tag) { - boost::lock_guard lock(jt_mutex); - armState.clearCommands(); - boost::copy(range, std::back_inserter(armState.commandedPosition)); - boost::copy(range, std::back_inserter(armState.commandedPosition_goal)); - } + /// @todo should this exist, is it a good design? is it written correctly? + void get(KukaState &state) { + boost::lock_guard lock(jt_mutex); + state = armState; + } - /** - * @brief Set the time duration expected between new position commands in ms - * - * The driver will likely be updated every so often, such as every 25ms, and - * the lowest level of the - * driver may update even more frequently, such as every 1ms. By providing as - * accurate an - * estimate between high level updates the low level driver can smooth out the - * motion through - * interpolation (the default), or another algorithm. See - * LowLevelStepAlgorithmType template parameter - * in the KukaFRIdriver class if you want to change out the low level - * algorithm. - * - * @see KukaFRIdriver::get(time_duration_command_tag) - * - * @param duration_to_goal_command std::chrono time format representing the - * time duration between updates - * - */ - void set(double duration_to_goal_command, time_duration_command_tag) { - boost::lock_guard lock(jt_mutex); - armState.goal_position_command_time_duration = duration_to_goal_command; - } + /// start recording the fusiontrack frame data in memory + /// return true on success, false on failure + bool start_recording() + { - /** - * @brief Get the timestamp of the most recent armState - * - * - * - * @see KukaFRIdriver::set(Range&& range, - * grl::revolute_joint_angle_open_chain_command_tag) - * - */ - // KukaState::time_point_type get(time_point_tag) { - // boost::lock_guard lock(jt_mutex); - // return armState.timestamp; - // } - - cartographer::common::Time get(time_point_tag) { - boost::lock_guard lock(jt_mutex); - return armState.time_event_stamp.device_time; + m_isRecording = true; + return m_isRecording; + } + /// stop recording the fusiontrack frame data in memory + /// return true on success, false on failure + bool stop_recording() + { + m_isRecording = false; + return !m_isRecording; } - /** - * \brief Set the applied joint torques for the current interpolation step. - * - * This method is only effective when the client is in a commanding state. - * The ControlMode of the robot has to be joint impedance control mode. The - * Client Command Mode has to be torque. - * - * @param state Object which stores the current state of the robot, including - * the command to send next - * @param torques Array with the applied torque values (in Nm) - * @param tag identifier object indicating that the torqe value command should - * be modified - */ - template - void set(Range &&range, grl::revolute_joint_torque_open_chain_command_tag) { - boost::lock_guard lock(jt_mutex); - armState.clearCommands(); - boost::copy(range, armState.commandedTorque); - } - /** - * \brief Set the applied wrench vector of the current interpolation step. - * - * The wrench vector consists of: - * [F_x, F_y, F_z, tau_A, tau_B, tau_C] - * - * F ... forces (in N) applied along the Cartesian axes of the - * currently used motion center. - * tau ... torques (in Nm) applied along the orientation angles - * (Euler angles A, B, C) of the currently used motion center. - * - * This method is only effective when the client is in a commanding state. - * The ControlMode of the robot has to be Cartesian impedance control mode. - * The - * Client Command Mode has to be wrench. - * - * @param state object storing the command data that will be sent to the - * physical device - * @param range wrench Applied Cartesian wrench vector, in x, y, z, roll, - * pitch, yaw force measurments. - * @param tag identifier object indicating that the wrench value command - * should be modified - * - * @todo perhaps support some specific more useful data layouts - */ - template - void set(Range &&range, grl::cartesian_wrench_command_tag) { - boost::lock_guard lock(jt_mutex); - armState.clearCommands(); - std::copy(range, armState.commandedCartesianWrenchFeedForward); - } - /// @todo should this exist, is it a good design? is it written correctly? - void get(KukaState &state) { - boost::lock_guard lock(jt_mutex); - state = armState; - } + bool save_oneKUKAiiwaStateBuffer() + { + //boost::lock_guard lock(jt_mutex); + std::string RobotName("Robotiiwa" ); + std::string destination("where this message is going (URI)"); + std::string source("where this message came from (URI)"); + std::string basename = RobotName; //std::get<0>(params); + bool setArmConfiguration_ = true; // set the arm config first time + bool max_control_force_stop_ = false; + // @TODO(Chunting) Put all these parameters in a configuration file. + std::vector joint_stiffness_(7, 0.2); + std::vector joint_damping_(7, 0.3); + std::vector joint_AccelerationRel_(7, 0.5); + std::vector joint_VelocityRel_(7, 1.0); + bool updateMinimumTrajectoryExecutionTime = false; + double minimumTrajectoryExecutionTime = 4; + + + //Cartesian Impedance Values + grl::flatbuffer::Vector3d cart_stiffness_trans_ = grl::flatbuffer::Vector3d(500,500,500); + grl::flatbuffer::EulerRotation cart_stifness_rot_ = grl::flatbuffer::EulerRotation(200,200,200,grl::flatbuffer::EulerOrder::xyz); + grl::flatbuffer::Vector3d cart_damping_trans_ = grl::flatbuffer::Vector3d(0.3,0.3,0.3); + grl::flatbuffer::EulerRotation cart_damping_rot_ = grl::flatbuffer::EulerRotation(0.3,0.3,0.3,grl::flatbuffer::EulerOrder::xyz); + grl::flatbuffer::EulerPose cart_stiffness_ = grl::flatbuffer::EulerPose(cart_stiffness_trans_, cart_stifness_rot_); + grl::flatbuffer::EulerPose cart_damping_ = grl::flatbuffer::EulerPose(cart_damping_trans_, cart_damping_rot_); + grl::flatbuffer::EulerPose cart_max_path_deviation_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(1000,1000,100), grl::flatbuffer::EulerRotation(5.,5.,5., grl::flatbuffer::EulerOrder::xyz)); + grl::flatbuffer::EulerPose cart_max_ctrl_vel_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(1000,1000,1000), grl::flatbuffer::EulerRotation(6.3,6.3,6.3, grl::flatbuffer::EulerOrder::xyz)); + grl::flatbuffer::EulerPose cart_max_ctrl_force_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(200,200,200), grl::flatbuffer::EulerRotation(200.,200.,200., grl::flatbuffer::EulerOrder::xyz)); + double nullspace_stiffness_ = 0.1; + double nullspace_damping_ = 0.1; + bool updatePortOnRemote = false; + int16_t portOnRemote = 3501; + bool updatePortOnController = false; + int16_t portOnController = 3502; + // Resolve the data in FRIMonitoringMessage + // Don't match the FRIMessage.pb.h + grl::flatbuffer::ArmState armControlMode = grl::flatbuffer::ArmState::StartArm; + grl::flatbuffer::KUKAiiwaInterface commandInterface = grl::flatbuffer::KUKAiiwaInterface::SmartServo;// KUKAiiwaInterface::SmartServo; + grl::flatbuffer::KUKAiiwaInterface monitorInterface = grl::flatbuffer::KUKAiiwaInterface::FRI; + + FRIMonitoringMessage monitoringMsg = friData_->monitoringMsg; + // RobotInfo + // how to use pb_callback_t driveState in RobotInfo? + ::RobotInfo robotInfo = monitoringMsg.robotInfo; + int NUM_DOF = robotInfo.has_numberOfJoints?robotInfo.numberOfJoints:7; + ::ControlMode controlMode = robotInfo.controlMode; + ::SafetyState safetyState = robotInfo.safetyState; + ::OperationMode operationMode = robotInfo.operationMode; + + // ConnectionInfo + // how to use uint32_t receiveMultiplier + ::ConnectionInfo connectionInfo = monitoringMsg.connectionInfo; + ::FRISessionState friSessionState = connectionInfo.sessionState; + ::FRIConnectionQuality friConnectionQuality = connectionInfo.quality; + // MessageIpoData + // JointValues jointPosition; double trackingPerformance; + ::MessageIpoData ipoData = monitoringMsg.ipoData; + ::ClientCommandMode clientCommandMode = ipoData.clientCommandMode; + ::OverlayType overlayType = ipoData.overlayType; + + // No MessageHeader in flatbuffer objects + ::MessageHeader messageHeader = monitoringMsg.header; + + ::MessageMonitorData messageMonitorData = monitoringMsg.monitorData; + + ::Transformation *transformation = new ::Transformation[5]; + + for (int i=0; i<5; i++) { + transformation[i] = monitoringMsg.requestedTransformations[i]; + } + // MessageEndOf exists in FRIMessage.pb.h, but it's never used in the FlatBuffer objects + ::MessageEndOf endOfMessageData = monitoringMsg.endOfMessageData; + auto bns = m_logFileBufferBuilderP->CreateString(basename); + double duration = boost::chrono::high_resolution_clock::now().time_since_epoch().count(); - bool startRecordingDataToFlatBuffer(flatbuffers::FlatBufferBuilder &fbb, std::shared_ptr &friData) - { + bool OK = true; - std::string RobotName("Robotiiwa" ); - std::string destination("where this message is going (URI)"); - std::string source("where this message came from (URI)"); - std::string basename = RobotName; //std::get<0>(params); - bool setArmConfiguration_ = true; // set the arm config first time - bool max_control_force_stop_ = false; - // Resolve the data in FRIMonitoringMessage - // Don't match the FRIMessage.pb.h - grl::flatbuffer::ArmState armControlMode = grl::flatbuffer::ArmState::StartArm; - grl::flatbuffer::KUKAiiwaInterface commandInterface = grl::flatbuffer::KUKAiiwaInterface::SmartServo;// KUKAiiwaInterface::SmartServo; - grl::flatbuffer::KUKAiiwaInterface monitorInterface = grl::flatbuffer::KUKAiiwaInterface::FRI; - - FRIMonitoringMessage monitoringMsg = friData->monitoringMsg; - // RobotInfo - // how to use pb_callback_t driveState in RobotInfo? - ::RobotInfo robotInfo = monitoringMsg.robotInfo; - int NUM_DOF = robotInfo.has_numberOfJoints?robotInfo.numberOfJoints:7; - ::ControlMode controlMode = robotInfo.controlMode; - ::SafetyState safetyState = robotInfo.safetyState; - ::OperationMode operationMode = robotInfo.operationMode; - - // ConnectionInfo - // how to use uint32_t receiveMultiplier - ::ConnectionInfo connectionInfo = monitoringMsg.connectionInfo; - ::FRISessionState friSessionState = connectionInfo.sessionState; - ::FRIConnectionQuality friConnectionQuality = connectionInfo.quality; - // MessageIpoData - // JointValues jointPosition; double trackingPerformance; - ::MessageIpoData ipoData = monitoringMsg.ipoData; - ::ClientCommandMode clientCommandMode = ipoData.clientCommandMode; - ::OverlayType overlayType = ipoData.overlayType; - - // No MessageHeader in flatbuffer objects - ::MessageHeader messageHeader = monitoringMsg.header; - - ::MessageMonitorData messageMonitorData = monitoringMsg.monitorData; - - ::Transformation *transformation = new ::Transformation[5]; - - for (int i=0; i<5; i++) { - transformation[i] = monitoringMsg.requestedTransformations[i]; - } - // MessageEndOf exists in FRIMessage.pb.h, but it's never used in the FlatBuffer objects - ::MessageEndOf endOfMessageData = monitoringMsg.endOfMessageData; + int64_t sequenceNumber = 0; - std::size_t builder_size_bytes = 0; - std::size_t previous_size = 0; - const std::size_t MegaByte = 1024*1024; - // In 32bits, the maximum size of a single flatbuffer is 2GB - 1 (defined in base.h), but actually single_buffer_limit_bytes can't be set beyond 2045 MB, otherwise assert might be launched. - const std::size_t single_buffer_limit_bytes = 2045*MegaByte; - auto bns = fbb.CreateString(basename); - double duration = boost::chrono::high_resolution_clock::now().time_since_epoch().count(); - flatbuffers::Offset controlState; - // @TODO(Chunting) Put all these parameters in a configuration file. - std::vector joint_stiffness_(7, 0.2); - std::vector joint_damping_(7, 0.3); - std::vector joint_AccelerationRel_(7, 0.5); - std::vector joint_VelocityRel_(7, 1.0); - bool updateMinimumTrajectoryExecutionTime = false; - double minimumTrajectoryExecutionTime = 4; - - - //Cartesian Impedance Values - grl::flatbuffer::Vector3d cart_stiffness_trans_ = grl::flatbuffer::Vector3d(500,500,500); - grl::flatbuffer::EulerRotation cart_stifness_rot_ = grl::flatbuffer::EulerRotation(200,200,200,grl::flatbuffer::EulerOrder::xyz); - grl::flatbuffer::Vector3d cart_damping_trans_ = grl::flatbuffer::Vector3d(0.3,0.3,0.3); - grl::flatbuffer::EulerRotation cart_damping_rot_ = grl::flatbuffer::EulerRotation(0.3,0.3,0.3,grl::flatbuffer::EulerOrder::xyz); - grl::flatbuffer::EulerPose cart_stiffness_ = grl::flatbuffer::EulerPose(cart_stiffness_trans_, cart_stifness_rot_); - grl::flatbuffer::EulerPose cart_damping_ = grl::flatbuffer::EulerPose(cart_damping_trans_, cart_damping_rot_); - grl::flatbuffer::EulerPose cart_max_path_deviation_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(1000,1000,100), grl::flatbuffer::EulerRotation(5.,5.,5., grl::flatbuffer::EulerOrder::xyz)); - grl::flatbuffer::EulerPose cart_max_ctrl_vel_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(1000,1000,1000), grl::flatbuffer::EulerRotation(6.3,6.3,6.3, grl::flatbuffer::EulerOrder::xyz)); - grl::flatbuffer::EulerPose cart_max_ctrl_force_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(200,200,200), grl::flatbuffer::EulerRotation(200.,200.,200., grl::flatbuffer::EulerOrder::xyz)); - double nullspace_stiffness_ = 0.1; - double nullspace_damping_ = 0.1; - bool updatePortOnRemote = false; - int16_t portOnRemote = 3501; - bool updatePortOnController = false; - int16_t portOnController = 3502; - bool OK = true; - std::vector> kukaiiwaStateVec; - int64_t sequenceNumber = 0; - while(OK && builder_size_bytes < single_buffer_limit_bytes ) - { copy(monitoringMsg, armState); - armState.sessionState = grl::toFlatBuffer(friSessionState); armState.connectionQuality = grl::toFlatBuffer(friConnectionQuality); armState.safetyState = grl::toFlatBuffer(safetyState); armState.operationMode = grl::toFlatBuffer(operationMode); // This parameter is never used. armState.driveState = grl::toFlatBuffer(::DriveState::DriveState_OFF); - flatbuffers::Offset controlState = grl::toFlatBuffer(fbb, basename, sequenceNumber++, duration, armState, armControlMode); - flatbuffers::Offset setCartesianImpedance = grl::toFlatBuffer(fbb, cart_stiffness_, cart_damping_, + flatbuffers::Offset controlState = grl::toFlatBuffer(*m_logFileBufferBuilderP, basename, sequenceNumber++, duration, armState, armControlMode); + flatbuffers::Offset setCartesianImpedance = grl::toFlatBuffer(*m_logFileBufferBuilderP, cart_stiffness_, cart_damping_, nullspace_stiffness_, nullspace_damping_, cart_max_path_deviation_, cart_max_ctrl_vel_, cart_max_ctrl_force_, max_control_force_stop_); - flatbuffers::Offset setJointImpedance = grl::toFlatBuffer(fbb, joint_stiffness_, joint_damping_); + flatbuffers::Offset setJointImpedance = grl::toFlatBuffer(*m_logFileBufferBuilderP, joint_stiffness_, joint_damping_); // normalized joint accelerations/velocities from 0 to 1 relative to system capabilities // how to get the acceleration of the robot? There is no acceleration information in KukaState (armState). - flatbuffers::Offset setSmartServo = grl::toFlatBuffer(fbb, joint_AccelerationRel_, joint_VelocityRel_, updateMinimumTrajectoryExecutionTime, minimumTrajectoryExecutionTime); + flatbuffers::Offset setSmartServo = grl::toFlatBuffer(*m_logFileBufferBuilderP, joint_AccelerationRel_, joint_VelocityRel_, updateMinimumTrajectoryExecutionTime, minimumTrajectoryExecutionTime); // FRI configuration parameters - flatbuffers::Offset FRIConfig = grl::toFlatBuffer(fbb, overlayType, connectionInfo, updatePortOnRemote, portOnRemote, updatePortOnController, portOnController); + flatbuffers::Offset FRIConfig = grl::toFlatBuffer(*m_logFileBufferBuilderP, overlayType, connectionInfo, updatePortOnRemote, portOnRemote, updatePortOnController, portOnController); std::vector> tools; std::vector> processData; // @TODO(Chunting) Initialize Pose with 0, we ought to calculate it later with ::Transformation; @@ -1458,10 +564,10 @@ class KukaFRIdriver : public std::enable_shared_from_this< // Compute pose later with transformation matrix grl::flatbuffer::Pose pose = grl::flatbuffer::Pose(grl::flatbuffer::Vector3d(0,0,0), grl::flatbuffer::Quaternion(0,0,0,0)); grl::flatbuffer::Inertia inertia = grl::flatbuffer::Inertia(1, pose, 1, 2, 3, 4, 5, 6); - flatbuffers::Offset linkObject = grl::toFlatBuffer(fbb, linkname, parent, pose, inertia); + flatbuffers::Offset linkObject = grl::toFlatBuffer(*m_logFileBufferBuilderP, linkname, parent, pose, inertia); tools.push_back(linkObject); processData.push_back( - grl::toFlatBuffer(fbb, + grl::toFlatBuffer(*m_logFileBufferBuilderP, "dataType"+ std::to_string(i), "defaultValue"+ std::to_string(i), "displayName"+ std::to_string(i), @@ -1473,7 +579,7 @@ class KukaFRIdriver : public std::enable_shared_from_this< } // Set the configuration of the Kuka iiwa flatbuffers::Offset kukaiiwaArmConfiguration = grl::toFlatBuffer( - fbb, + *m_logFileBufferBuilderP, RobotName, commandInterface, monitorInterface, @@ -1490,7 +596,7 @@ class KukaFRIdriver : public std::enable_shared_from_this< true); grl::TimeEvent time_event_stamp; flatbuffers::Offset friMessageLog = grl::toFlatBuffer( - fbb, + *m_logFileBufferBuilderP, friSessionState, friConnectionQuality, controlMode, @@ -1498,7 +604,6 @@ class KukaFRIdriver : public std::enable_shared_from_this< time_event_stamp); // getWrench() is availble in KukaJAVAdriver, so maybe it's better to log data in KukaDriver where user can access both KukaFRIDriver and KukaJAVADriver? grl::flatbuffer::Wrench cartesianWrench{grl::flatbuffer::Vector3d(1,1,1),grl::flatbuffer::Vector3d(1,1,1),grl::flatbuffer::Vector3d(1,1,1)}; - // In armState there is neither joint velocity nor acceleration std::vector position(7,0); std::vector velocity(7,0); @@ -1508,12 +613,12 @@ class KukaFRIdriver : public std::enable_shared_from_this< position.push_back(armState.position[i]); torque.push_back(armState.torque[i]); } - flatbuffers::Offset jointStatetab = grl::toFlatBuffer(fbb, position, velocity, acceleration, torque); + flatbuffers::Offset jointStatetab = grl::toFlatBuffer(*m_logFileBufferBuilderP, position, velocity, acceleration, torque); // Calculate the data later. // Cartesian pose of the flange relative to the base of the arm grl::flatbuffer::Pose cartesianFlangePose = grl::flatbuffer::Pose(grl::flatbuffer::Vector3d(1,1,1), grl::flatbuffer::Quaternion(2,3,4,5)); flatbuffers::Offset kukaiiwaMonitorState = grl::toFlatBuffer( - fbb, + *m_logFileBufferBuilderP, jointStatetab, // flatbuffers::Offset &measuredState, cartesianFlangePose, jointStatetab, // flatbuffers::Offset &jointStateReal, @@ -1522,21 +627,20 @@ class KukaFRIdriver : public std::enable_shared_from_this< friSessionState, robotInfo.operationMode, cartesianWrench); - // Set it up in the configuration file std::vector torqueSensorLimits(7,0.5); std::string hardwareVersion( "hardvareVersion"); bool isReadyToMove = true; bool isMastered = true; flatbuffers::Offset monitorConfig = grl::toFlatBuffer( - fbb, + *m_logFileBufferBuilderP, hardwareVersion, torqueSensorLimits, isReadyToMove, isMastered, processData); - flatbuffers::Offset kukaiiwaState = grl::toFlatBuffer( - fbb, + flatbuffers::Offset KUKAiiwaState = grl::toFlatBuffer( + *m_logFileBufferBuilderP, RobotName, destination, source, @@ -1546,81 +650,148 @@ class KukaFRIdriver : public std::enable_shared_from_this< true, kukaiiwaMonitorState, false, monitorConfig, friMessageLog); + m_KUKAiiwaStateBufferP->push_back(KUKAiiwaState); - kukaiiwaStateVec.push_back(kukaiiwaState); + return true; } - std::cout<< "kukaiiwaStateVec:" << kukaiiwaStateVec.size() << std::endl; - - flatbuffers::Offset kukaStates = grl::toFlatBuffer(fbb, kukaiiwaStateVec); - grl::flatbuffer::FinishKUKAiiwaStatesBuffer(fbb, kukaStates); - uint8_t *buf = fbb.GetBufferPointer(); - std::size_t bufsize = fbb.GetSize(); - /// To expand the capacity of a single buffer, _max_tables is set to 10000000 - flatbuffers::uoffset_t _max_depth = 64; - flatbuffers::uoffset_t _max_tables = 10000000; - flatbuffers::Verifier verifier(buf, bufsize, _max_depth, _max_tables); - OK = OK && grl::flatbuffer::VerifyKUKAiiwaStatesBuffer(verifier); - assert(OK && "VerifyKUKAiiwaStatesBuffer"); - std::cout << "Buffer size: " << bufsize << std::endl; - std::string binary_file_path = "Kuka_test_binary.iiwa"; - std::string json_file_path = "kuka_test_text.json"; - // Get the current working directory - std::string fbs_filename("KUKAiiwa.fbs"); - // OK = OK && grl::SaveFlatBufferFile( - // buf, - // fbb.GetSize(), - // binary_file_path, - // fbs_filename, - // json_file_path); - assert(OK && "SaveFlatBufferFile"); - - std::cout << "End of the program" << std::endl; - - - - - - return true; - } - - - - + // bool startRecordingDataToFlatBuffer(flatbuffers::FlatBufferBuilder &*m_logFileBufferBuilderP, std::shared_ptr &friData) + bool save_recording(std::string filename = std::string()) + { + if(filename.empty()) + { + /// TODO(ahundt) Saving the file twice in one second will overwrite!!!! + filename = current_date_and_time_string() + "_Kukaiiwa.iiwa"; + } + #ifdef HAVE_spdlog + loggerP->info("Save Recording as: ", filename); + #else // HAVE_spdlog + std::cout << "Save Recording as: " << filename << std::endl; + #endif // HAVE_spdlog + /// lock mutex before accessing file + boost::lock_guard lock(jt_mutex); + + auto saveLambdaFunction = [ + save_fbbP = std::move(m_logFileBufferBuilderP) + ,save_KUKAiiwaBufferP = std::move(m_KUKAiiwaStateBufferP) + ,filename + #ifdef HAVE_spdlog + ,lambdaLoggerP = loggerP + #endif // HAVE_spdlog + ]() mutable + { + bool success = grl::FinishAndVerifyBuffer(*save_fbbP, *save_KUKAiiwaBufferP); + bool write_binary_stream = true; + success = success && flatbuffers::SaveFile(filename.c_str(), reinterpret_cast(save_fbbP->GetBufferPointer()), save_fbbP->GetSize(), write_binary_stream); + /// TODO(ahFusionTrackLogAndTrackundt) replace cout with proper spdlog and vrep banner notification + #ifdef HAVE_spdlog + lambdaLoggerP->info("filename: ", filename, " verifier success: ", success); + #else // HAVE_spdlog + std::cout << "filename: " << filename << " verifier success: " << success << std::endl; + #endif // HAVE_spdlog + }; + + // save the recording to a file in a separate thread, memory will be freed up when file finishes saving + std::shared_ptr saveLogThread(std::make_shared(saveLambdaFunction)); + m_saveRecordingThreads.push_back(saveLogThread); + // flatbuffersbuilder does not yet exist + m_logFileBufferBuilderP = std::make_shared(); + m_KUKAiiwaStateBufferP = std::make_shared>>(); + std::cout << "End of the program" << std::endl; + + return true; + } + // clear the recording buffer from memory immediately to start fresh + void clear_recording() + { + boost::lock_guard lock(jt_mutex); + m_logFileBufferBuilderP.reset(); + m_KUKAiiwaStateBufferP.reset(); + } - // The total number of times the FRI interface has successfully received a UDP - // packet - // from the robot since this class was initialized. - volatile std::size_t m_haveReceivedRealDataCount = 0; - // The total number of times the FRI interface has attempted to receive a UDP - // packet - // from the robot since this class was initialized, regardless of if it was - // successful or not. - volatile std::size_t m_attemptedCommunicationCount = 0; - // The number of consecutive FRI receive calls that have failed to get data - // successfully, resets to 0 on a single success. - volatile std::size_t m_attemptedCommunicationConsecutiveFailureCount = 0; - // The number of consecutive FRI receive calls that have received data - // successfully, resets to 0 on a single failure. - volatile std::size_t m_attemptedCommunicationConsecutiveSuccessCount = 0; + // The total number of times the FRI interface has successfully received a UDP + // packet + // from the robot since this class was initialized. + volatile std::size_t m_haveReceivedRealDataCount = 0; + // The total number of times the FRI interface has attempted to receive a UDP + // packet + // from the robot since this class was initialized, regardless of if it was + // successful or not. + volatile std::size_t m_attemptedCommunicationCount = 0; + // The number of consecutive FRI receive calls that have failed to get data + // successfully, resets to 0 on a single success. + volatile std::size_t m_attemptedCommunicationConsecutiveFailureCount = 0; + // The number of consecutive FRI receive calls that have received data + // successfully, resets to 0 on a single failure. + volatile std::size_t m_attemptedCommunicationConsecutiveSuccessCount = 0; + + boost::asio::io_service device_driver_io_service; + // The work class is used to inform the io_service when work starts and finishes. + std::unique_ptr device_driver_workP_; + std::unique_ptr driver_threadP; + std::shared_ptr> kukaFRIClientDataDriverP_; +private: +void update() + { + const std::size_t MegaByte = 1024*1024; + // If we write too large a flatbuffer + const std::size_t single_buffer_limit_bytes = 2*MegaByte; - boost::asio::io_service device_driver_io_service; - // The work class is used to inform the io_service when work starts and finishes. - std::unique_ptr device_driver_workP_; - std::unique_ptr driver_threadP; - std::shared_ptr> kukaFRIClientDataDriverP_; + // run the primary update loop in a separate thread + bool saveFileNow = false; + while (!m_shouldStop) + { + boost::lock_guard lock(jt_mutex); + /// Temporarily set m_isRecording true. + m_isRecording = true; + if (m_isRecording) + { + run_one(); + // There is a flatbuffers file size limit of 2GB, but we use a conservative 512MB + if(m_logFileBufferBuilderP->GetSize() > single_buffer_limit_bytes) + { + // save the file if we are over the limit + saveFileNow = true; + } + } // end recording steps + /// TODO(ahundt) Let the user specify the filenames, or provide a way to check the flatbuffer size and know single_buffer_limit_bytes. + if(saveFileNow) + { + save_recording(); + saveFileNow = false; + } + std::this_thread::yield(); + } // end while loop that keeps driver alive + } private: - KukaState armState; - boost::mutex jt_mutex; - - Params params_; - std::shared_ptr friData_; + KukaState armState; + boost::mutex jt_mutex; + + Params params_; + std::shared_ptr friData_; + std::atomic m_shouldStop; + /// is data currently being recorded + std::atomic m_isRecording; + /// builds up the file log in memory as data is received + /// @todo TODO(ahundt) once using C++14 use unique_ptr https://stackoverflow.com/questions/8640393/move-capture-in-lambda + std::shared_ptr m_logFileBufferBuilderP; + /// this is the current log data stored in memory + /// @todo TODO(ahundt) once using C++14 use unique_ptr https://stackoverflow.com/questions/8640393/move-capture-in-lambda + std::shared_ptr>> m_KUKAiiwaStateBufferP; + /// thread that polls the driver for new data and puts the data into the recording + std::unique_ptr m_driverThread; + /// @todo TODO(ahundt) the threads that saved files will build up forever, figure out how they can clear themselves out + std::vector> m_saveRecordingThreads; + + #ifdef HAVE_spdlog + std::shared_ptr loggerP; + #endif // HAVE_spdlog }; /// End of KukaFRIdriver.hpp /// @brief nonmember wrapper function to help integrate KukaFRIdriver objects diff --git a/include/grl/kuka/KukaToFlatbuffer.hpp b/include/grl/kuka/KukaToFlatbuffer.hpp index e2a8eeb..0fccf59 100644 --- a/include/grl/kuka/KukaToFlatbuffer.hpp +++ b/include/grl/kuka/KukaToFlatbuffer.hpp @@ -721,6 +721,34 @@ flatbuffers::Offset toFlatBuffer( fbb, fbb.CreateVector>(kukaiiwastates)); } + + +/// Helper function for use when building up messages to save to a log file. +/// Call this just before SaveFlatBufferFile. See fusionTrackExample for how +/// and when to use it. +bool FinishAndVerifyBuffer( + flatbuffers::FlatBufferBuilder& fbb, + std::vector>& KUKAiiwaState_vector +) +{ + + auto states = fbb.CreateVector(KUKAiiwaState_vector); + auto fbLogKUKAiiwaStates = grl::flatbuffer::CreateKUKAiiwaStates(fbb, states); + + // Finish a buffer with given object + // Call `Finish()` to instruct the builder fbb that this frame is complete. + const char *file_identifier = grl::flatbuffer::KUKAiiwaStatesIdentifier(); + // fbb.Finish(oneKUKAiiwaFusionTrackMessage, file_identifier); + fbb.Finish(fbLogKUKAiiwaStates, file_identifier); + + flatbuffers::uoffset_t _max_depth = 64; + flatbuffers::uoffset_t _max_tables = 1000000000; + auto verifier = flatbuffers::Verifier(fbb.GetBufferPointer(), fbb.GetSize(), _max_depth, _max_tables); + bool success = grl::flatbuffer::VerifyKUKAiiwaStatesBuffer(verifier); + assert(success && "VerifyKUKAiiwaStatesBuffer"); + + return success; +} } // End of grl namespace From 9f6e120e33e62769fc23d231b410e9ac7e2a877a Mon Sep 17 00:00:00 2001 From: Chunting Date: Wed, 10 Jan 2018 23:19:08 -0500 Subject: [PATCH 028/102] Get flatbuffer the binary file --- include/grl/kuka/KukaFRIdriver.hpp | 54 +++++++++++++++++------------- test/KukaFRITest.cpp | 2 +- 2 files changed, 31 insertions(+), 25 deletions(-) diff --git a/include/grl/kuka/KukaFRIdriver.hpp b/include/grl/kuka/KukaFRIdriver.hpp index c6fcafb..8b294a1 100644 --- a/include/grl/kuka/KukaFRIdriver.hpp +++ b/include/grl/kuka/KukaFRIdriver.hpp @@ -103,7 +103,12 @@ class KukaFRIdriver : public std::enable_shared_from_this< LowLevelStepAlgorithmType>::run_automatically)) ); - m_driverThread.reset(new std::thread(&KukaFRIdriver::update, this)); + // flatbuffersbuilder does not yet exist + m_logFileBufferBuilderP = std::make_shared(); + m_KUKAiiwaStateBufferP = std::make_shared>>(); + #ifdef HAVE_spdlog + loggerP = spdlog::stdout_logger_mt("logs/kukaiiwa_logger.txt"); + #endif // HAVE_spdlog } const Params &getParams() { return params_; } @@ -282,6 +287,7 @@ class KukaFRIdriver : public std::enable_shared_from_this< // TODO(chunting) add data to log here, when full write to disk on a separate thread like FusionTrackLogAndTrack save_oneKUKAiiwaStateBuffer(); + update(); // m_driverThread.reset(new std::thread(&KukaFRIdriver::save_recording)); } else { m_attemptedCommunicationConsecutiveFailureCount++; @@ -530,6 +536,7 @@ class KukaFRIdriver : public std::enable_shared_from_this< } // MessageEndOf exists in FRIMessage.pb.h, but it's never used in the FlatBuffer objects ::MessageEndOf endOfMessageData = monitoringMsg.endOfMessageData; + auto bns = m_logFileBufferBuilderP->CreateString(basename); double duration = boost::chrono::high_resolution_clock::now().time_since_epoch().count(); @@ -660,6 +667,7 @@ class KukaFRIdriver : public std::enable_shared_from_this< bool save_recording(std::string filename = std::string()) { + loggerP->info("Here is in save_recording/n "); if(filename.empty()) { /// TODO(ahundt) Saving the file twice in one second will overwrite!!!! @@ -671,7 +679,7 @@ class KukaFRIdriver : public std::enable_shared_from_this< std::cout << "Save Recording as: " << filename << std::endl; #endif // HAVE_spdlog /// lock mutex before accessing file - boost::lock_guard lock(jt_mutex); + // boost::lock_guard lock(jt_mutex); auto saveLambdaFunction = [ save_fbbP = std::move(m_logFileBufferBuilderP) @@ -740,34 +748,32 @@ void update() { const std::size_t MegaByte = 1024*1024; // If we write too large a flatbuffer - const std::size_t single_buffer_limit_bytes = 2*MegaByte; + const std::size_t single_buffer_limit_bytes = 0.1*MegaByte; // run the primary update loop in a separate thread bool saveFileNow = false; - while (!m_shouldStop) - { - boost::lock_guard lock(jt_mutex); - /// Temporarily set m_isRecording true. - m_isRecording = true; - if (m_isRecording) - { - run_one(); - // There is a flatbuffers file size limit of 2GB, but we use a conservative 512MB - if(m_logFileBufferBuilderP->GetSize() > single_buffer_limit_bytes) - { - // save the file if we are over the limit - saveFileNow = true; - } - } // end recording steps - /// TODO(ahundt) Let the user specify the filenames, or provide a way to check the flatbuffer size and know single_buffer_limit_bytes. - if(saveFileNow) + // boost::lock_guard lock(jt_mutex); + /// Temporarily set m_isRecording true. + m_isRecording = true; + if (m_isRecording) + { + // There is a flatbuffers file size limit of 2GB, but we use a conservative 512MB + int buffsize = m_logFileBufferBuilderP->GetSize(); + std::cout << "Buffersize:" << buffsize << std::endl; + if( buffsize > single_buffer_limit_bytes) { - save_recording(); - saveFileNow = false; + // save the file if we are over the limit + saveFileNow = true; } - std::this_thread::yield(); - } // end while loop that keeps driver alive + } // end recording steps + /// TODO(ahundt) Let the user specify the filenames, or provide a way to check the flatbuffer size and know single_buffer_limit_bytes. + if(saveFileNow) + { + save_recording(); + saveFileNow = false; + } + } private: KukaState armState; diff --git a/test/KukaFRITest.cpp b/test/KukaFRITest.cpp index 8028efe..39ab815 100644 --- a/test/KukaFRITest.cpp +++ b/test/KukaFRITest.cpp @@ -53,7 +53,7 @@ int main(int argc, char* argv[]) int print_every_n = 100; std::size_t q_size = 4096; //queue size must be power of 2 spdlog::set_async_mode(q_size); - std::shared_ptr loggerPG; + std::shared_ptr loggerPG; try { loggerPG = spdlog::stdout_logger_mt("console"); } From cb4836182401022fc41d453d3e1bb9fae42f233a Mon Sep 17 00:00:00 2001 From: Chunting Date: Thu, 11 Jan 2018 15:35:17 -0500 Subject: [PATCH 029/102] Succeed logging data from the robot (position and torque) --- include/grl/flatbuffer/JointState.fbs | 2 +- include/grl/kuka/KukaFRIdriver.hpp | 38 +++++++++++++-------------- include/grl/kuka/KukaToFlatbuffer.hpp | 18 ++++++------- 3 files changed, 29 insertions(+), 29 deletions(-) diff --git a/include/grl/flatbuffer/JointState.fbs b/include/grl/flatbuffer/JointState.fbs index d36cec8..6bc3602 100644 --- a/include/grl/flatbuffer/JointState.fbs +++ b/include/grl/flatbuffer/JointState.fbs @@ -9,4 +9,4 @@ table JointState { torque:[double]; // Newton Meters (N*m) } -root_type JointState; \ No newline at end of file +// root_type JointState; \ No newline at end of file diff --git a/include/grl/kuka/KukaFRIdriver.hpp b/include/grl/kuka/KukaFRIdriver.hpp index 8b294a1..aff24c2 100644 --- a/include/grl/kuka/KukaFRIdriver.hpp +++ b/include/grl/kuka/KukaFRIdriver.hpp @@ -474,26 +474,26 @@ class KukaFRIdriver : public std::enable_shared_from_this< bool setArmConfiguration_ = true; // set the arm config first time bool max_control_force_stop_ = false; // @TODO(Chunting) Put all these parameters in a configuration file. - std::vector joint_stiffness_(7, 0.2); - std::vector joint_damping_(7, 0.3); - std::vector joint_AccelerationRel_(7, 0.5); - std::vector joint_VelocityRel_(7, 1.0); + std::vector joint_stiffness_(7, 0); + std::vector joint_damping_(7, 0); + std::vector joint_AccelerationRel_(7, 0); + std::vector joint_VelocityRel_(7, 0); bool updateMinimumTrajectoryExecutionTime = false; double minimumTrajectoryExecutionTime = 4; //Cartesian Impedance Values - grl::flatbuffer::Vector3d cart_stiffness_trans_ = grl::flatbuffer::Vector3d(500,500,500); - grl::flatbuffer::EulerRotation cart_stifness_rot_ = grl::flatbuffer::EulerRotation(200,200,200,grl::flatbuffer::EulerOrder::xyz); - grl::flatbuffer::Vector3d cart_damping_trans_ = grl::flatbuffer::Vector3d(0.3,0.3,0.3); - grl::flatbuffer::EulerRotation cart_damping_rot_ = grl::flatbuffer::EulerRotation(0.3,0.3,0.3,grl::flatbuffer::EulerOrder::xyz); + grl::flatbuffer::Vector3d cart_stiffness_trans_ = grl::flatbuffer::Vector3d(0,0,0); + grl::flatbuffer::EulerRotation cart_stifness_rot_ = grl::flatbuffer::EulerRotation(0,0,0,grl::flatbuffer::EulerOrder::xyz); + grl::flatbuffer::Vector3d cart_damping_trans_ = grl::flatbuffer::Vector3d(0,0,0); + grl::flatbuffer::EulerRotation cart_damping_rot_ = grl::flatbuffer::EulerRotation(0,0,0,grl::flatbuffer::EulerOrder::xyz); grl::flatbuffer::EulerPose cart_stiffness_ = grl::flatbuffer::EulerPose(cart_stiffness_trans_, cart_stifness_rot_); grl::flatbuffer::EulerPose cart_damping_ = grl::flatbuffer::EulerPose(cart_damping_trans_, cart_damping_rot_); - grl::flatbuffer::EulerPose cart_max_path_deviation_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(1000,1000,100), grl::flatbuffer::EulerRotation(5.,5.,5., grl::flatbuffer::EulerOrder::xyz)); - grl::flatbuffer::EulerPose cart_max_ctrl_vel_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(1000,1000,1000), grl::flatbuffer::EulerRotation(6.3,6.3,6.3, grl::flatbuffer::EulerOrder::xyz)); - grl::flatbuffer::EulerPose cart_max_ctrl_force_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(200,200,200), grl::flatbuffer::EulerRotation(200.,200.,200., grl::flatbuffer::EulerOrder::xyz)); - double nullspace_stiffness_ = 0.1; - double nullspace_damping_ = 0.1; + grl::flatbuffer::EulerPose cart_max_path_deviation_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(0,0,0), grl::flatbuffer::EulerRotation(0,0,0, grl::flatbuffer::EulerOrder::xyz)); + grl::flatbuffer::EulerPose cart_max_ctrl_vel_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(0,0,0), grl::flatbuffer::EulerRotation(0,0,0, grl::flatbuffer::EulerOrder::xyz)); + grl::flatbuffer::EulerPose cart_max_ctrl_force_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(0,0,0), grl::flatbuffer::EulerRotation(0.,0.,0., grl::flatbuffer::EulerOrder::xyz)); + double nullspace_stiffness_ = 0; + double nullspace_damping_ = 0; bool updatePortOnRemote = false; int16_t portOnRemote = 3501; bool updatePortOnController = false; @@ -570,7 +570,7 @@ class KukaFRIdriver : public std::enable_shared_from_this< std::string parent = i==0?"Base":("Link" + std::to_string(i-1)); // Compute pose later with transformation matrix grl::flatbuffer::Pose pose = grl::flatbuffer::Pose(grl::flatbuffer::Vector3d(0,0,0), grl::flatbuffer::Quaternion(0,0,0,0)); - grl::flatbuffer::Inertia inertia = grl::flatbuffer::Inertia(1, pose, 1, 2, 3, 4, 5, 6); + grl::flatbuffer::Inertia inertia = grl::flatbuffer::Inertia(1, pose, 0, 0, 0, 0, 0, 0); flatbuffers::Offset linkObject = grl::toFlatBuffer(*m_logFileBufferBuilderP, linkname, parent, pose, inertia); tools.push_back(linkObject); processData.push_back( @@ -610,12 +610,12 @@ class KukaFRIdriver : public std::enable_shared_from_this< monitoringMsg, time_event_stamp); // getWrench() is availble in KukaJAVAdriver, so maybe it's better to log data in KukaDriver where user can access both KukaFRIDriver and KukaJAVADriver? - grl::flatbuffer::Wrench cartesianWrench{grl::flatbuffer::Vector3d(1,1,1),grl::flatbuffer::Vector3d(1,1,1),grl::flatbuffer::Vector3d(1,1,1)}; + grl::flatbuffer::Wrench cartesianWrench{grl::flatbuffer::Vector3d(0, 0, 0),grl::flatbuffer::Vector3d(0, 0, 0),grl::flatbuffer::Vector3d(0, 0, 0)}; // In armState there is neither joint velocity nor acceleration - std::vector position(7,0); + std::vector position; std::vector velocity(7,0); std::vector acceleration(7,0); - std::vector torque(7,0); + std::vector torque; for(int i = 0; i<7; i++) { position.push_back(armState.position[i]); torque.push_back(armState.torque[i]); @@ -623,7 +623,7 @@ class KukaFRIdriver : public std::enable_shared_from_this< flatbuffers::Offset jointStatetab = grl::toFlatBuffer(*m_logFileBufferBuilderP, position, velocity, acceleration, torque); // Calculate the data later. // Cartesian pose of the flange relative to the base of the arm - grl::flatbuffer::Pose cartesianFlangePose = grl::flatbuffer::Pose(grl::flatbuffer::Vector3d(1,1,1), grl::flatbuffer::Quaternion(2,3,4,5)); + grl::flatbuffer::Pose cartesianFlangePose = grl::flatbuffer::Pose(grl::flatbuffer::Vector3d(0, 0, 0), grl::flatbuffer::Quaternion(0,0,0,0)); flatbuffers::Offset kukaiiwaMonitorState = grl::toFlatBuffer( *m_logFileBufferBuilderP, jointStatetab, // flatbuffers::Offset &measuredState, @@ -635,7 +635,7 @@ class KukaFRIdriver : public std::enable_shared_from_this< robotInfo.operationMode, cartesianWrench); // Set it up in the configuration file - std::vector torqueSensorLimits(7,0.5); + std::vector torqueSensorLimits(7,0); std::string hardwareVersion( "hardvareVersion"); bool isReadyToMove = true; bool isMastered = true; diff --git a/include/grl/kuka/KukaToFlatbuffer.hpp b/include/grl/kuka/KukaToFlatbuffer.hpp index 0fccf59..db461b0 100644 --- a/include/grl/kuka/KukaToFlatbuffer.hpp +++ b/include/grl/kuka/KukaToFlatbuffer.hpp @@ -232,10 +232,10 @@ flatbuffers::Offset toFlatBuffer( { return grl::flatbuffer::CreateJointState( fbb, - position.empty() ? fbb.CreateVector(position) : 0, - velocity.empty() ? fbb.CreateVector(velocity) : 0, - acceleration.empty() ? fbb.CreateVector(acceleration) : 0, - torque.empty() ? fbb.CreateVector(torque) : 0); + fbb.CreateVector(position), + fbb.CreateVector(velocity), + fbb.CreateVector(acceleration), + fbb.CreateVector(torque)); } /// JointState.fbs flatbuffers::Offset toFlatBuffer( @@ -254,10 +254,10 @@ flatbuffers::Offset toFlatBuffer( } return grl::flatbuffer::CreateJointState( fbb, - position.empty() ? fbb.CreateVector(position) : 0, - velocity.empty() ? fbb.CreateVector(velocity) : 0, - acceleration.empty() ? fbb.CreateVector(acceleration) : 0, - torque.empty() ? fbb.CreateVector(torque) : 0); + fbb.CreateVector(position), + fbb.CreateVector(velocity), + fbb.CreateVector(acceleration), + fbb.CreateVector(torque)); } @@ -268,7 +268,7 @@ flatbuffers::Offset toFlatBuffer( { return grl::flatbuffer::CreateMoveArmTrajectory( fbb, - traj.empty()?fbb.CreateVector>(traj):0); + fbb.CreateVector>(traj)); } /// ArmControlState.fbs From b816b53e72ffb3ef1fac1e61267b4366eff3a6fa Mon Sep 17 00:00:00 2001 From: Chunting Date: Mon, 15 Jan 2018 20:06:17 -0500 Subject: [PATCH 030/102] Log data from robot --- include/grl/flatbuffer/KUKAiiwa.fbs | 4 +- include/grl/flatbuffer/flatbuffer.hpp | 1 - include/grl/kuka/KukaFRIClientDataDriver.hpp | 9 +- include/grl/kuka/KukaFRIdriver.hpp | 211 +++++++++++++----- include/grl/kuka/KukaToFlatbuffer.hpp | 53 +++-- .../grl/sensor/FusionTrackToFlatbuffer.hpp | 37 --- test/KukaFRITest.cpp | 5 +- test/kukaiiwaTest.cpp | 4 +- 8 files changed, 201 insertions(+), 123 deletions(-) diff --git a/include/grl/flatbuffer/KUKAiiwa.fbs b/include/grl/flatbuffer/KUKAiiwa.fbs index 3774019..634a74d 100644 --- a/include/grl/flatbuffer/KUKAiiwa.fbs +++ b/include/grl/flatbuffer/KUKAiiwa.fbs @@ -299,7 +299,6 @@ table FRIMessageLog { sessionState:ESessionState; connectionQuality:EConnectionQuality; controlMode:EControlMode; - // MessageHeader; messageIdentifier:int; sequenceCounter:int; reflectedSequenceCounter:int; @@ -323,7 +322,8 @@ table KUKAiiwaState { name:string; // The name of the robot to update (identifier used when applicable, doesn't ever change or set the name) destination:string; // where this message is going (URI) source:string; // where this message came from (URI) - timestamp:double; // timestamp in seconds + // timestamp:double; // timestamp in seconds + timeStamp:TimeEvent; setArmControlState:bool = false; // only actually change the arm state when this is true. armControlState:ArmControlState; // Command state and mode of the arm diff --git a/include/grl/flatbuffer/flatbuffer.hpp b/include/grl/flatbuffer/flatbuffer.hpp index fa1c80f..acfc927 100644 --- a/include/grl/flatbuffer/flatbuffer.hpp +++ b/include/grl/flatbuffer/flatbuffer.hpp @@ -2,7 +2,6 @@ #define GRL_FLATBUFFER_HPP #include - #include diff --git a/include/grl/kuka/KukaFRIClientDataDriver.hpp b/include/grl/kuka/KukaFRIClientDataDriver.hpp index 8449454..d8a9450 100644 --- a/include/grl/kuka/KukaFRIClientDataDriver.hpp +++ b/include/grl/kuka/KukaFRIClientDataDriver.hpp @@ -411,7 +411,8 @@ void update_state(boost::asio::ip::udp::socket &socket, boost::asio::ip::udp::endpoint sender_endpoint = boost::asio::ip::udp::endpoint()) { static const int message_flags = 0; - // get a local clock timestamp, then the latest frame from the device, then another timestamp + + // get a local clock timestamp, then the latest frame from the device, then another timestamp timeEvent.local_request_time = cartographer::common::UniversalTimeScaleClock::now(); receive_bytes_transferred = socket.receive_from( boost::asio::buffer(friData.receiveBuffer, @@ -629,11 +630,6 @@ class KukaFRIClientDataDriver // return the latest state to the caller std::tie(step_alg_params,friData, receive_ec, receive_bytes_transferred, send_ec, send_bytes_transferred, timeEvent) = std::move(latestStateForUser_); - // TODO(chunting) consider adding to log buffer here, data is in latestStateForUser_, alternate might be latestClientData - - - - haveNewData = true; } else if (std::get( @@ -767,6 +763,7 @@ class KukaFRIClientDataDriver // newCommandForDriver_: needs to be cleared with 100% certainty BOOST_VERIFY(spareStates_.size() > 0); + if (!std::get(nextState) && spareStates_.size()) { // move the last element out and shorten the vector diff --git a/include/grl/kuka/KukaFRIdriver.hpp b/include/grl/kuka/KukaFRIdriver.hpp index aff24c2..a62ef32 100644 --- a/include/grl/kuka/KukaFRIdriver.hpp +++ b/include/grl/kuka/KukaFRIdriver.hpp @@ -148,6 +148,41 @@ class KukaFRIdriver : public std::enable_shared_from_this< return maxVel; } + + /// A custom clock for the FusionTrack microsecond time stamps + struct MicrosecondClock + { + using rep = int64_t; + /// 1 microsecond + using period = std::ratio<1, 1000000>; + using duration = std::chrono::duration; + using time_point = std::chrono::time_point; + static constexpr bool is_steady = true; + + /// TODO(ahundt) currently assuming the FusionTrack timestamp is from the unix time epoch + static time_point now() noexcept + { + using namespace std::chrono; + return time_point( + duration_cast(system_clock::now().time_since_epoch())); + } + }; + + + /// TODO(ahundt) currently assuming the FusionTrack timestamp is from the unix time epoch + cartographer::common::Time KukaTimeToCommonTime(typename MicrosecondClock::time_point Kukatime) + { + return cartographer::common::Time( + std::chrono::duration_cast(Kukatime.time_since_epoch()) + + std::chrono::seconds(cartographer::common::kUtsEpochOffsetFromUnixEpochInSeconds)); + } + + cartographer::common::Time FRITimeStampToCommonTime(const ::TimeStamp &friTimeStamp) + { + typename MicrosecondClock::time_point fritp(MicrosecondClock::duration(friTimeStamp.nanosec)); + return KukaTimeToCommonTime(fritp); + } + /** * spin once, this is what you call each time you synchronize the client with * the robot over UDP @@ -158,6 +193,9 @@ class KukaFRIdriver : public std::enable_shared_from_this< */ bool run_one() { grl::TimeEvent time_event_stamp; + + + // note: this one sends *and* receives the joint data! BOOST_VERIFY(kukaFRIClientDataDriverP_.get() != nullptr); /// @todo use runtime calculation of NUM_JOINTS instead of constant @@ -203,29 +241,43 @@ class KukaFRIdriver : public std::enable_shared_from_this< std::size_t send_bytes, recv_bytes; // sync with device over network haveNewData = !kukaFRIClientDataDriverP_->update_state( - lowLevelStepAlgorithmCommandParamsP, friData_, recv_ec, recv_bytes, send_ec, - send_bytes, time_event_stamp); + lowLevelStepAlgorithmCommandParamsP, + friData_, + recv_ec, + recv_bytes, + send_ec, + send_bytes, + time_event_stamp); + + std::string s_event_name = "/kukaiiwa/state"; + + /// Name for the clock on the kukaiiws + /// Useful for timing calculations and debugging. + /// This one string you will want to check when analyzing + /// logged data for time differences and error. + std::string device_clock_id_str = "/kukaiiwa/state/clock"; + + /// Name for the local clock on which this driver runs + /// Useful for timing calculations and debugging. + /// defaults to "/control_computer/clock/steady" + /// This one string you will want to check when analyzing + /// logged data for time differences and error. + std::string local_clock_id_str = "/control_computer/clock/steady"; + + TimeEvent::UnsignedCharArray event_name; + s_event_name.copy(event_name.begin(), std::min(s_event_name.size(), event_name.size())); + time_event_stamp.event_name = event_name; + TimeEvent::UnsignedCharArray device_clock_id; + device_clock_id_str.copy(device_clock_id.begin(), std::min(device_clock_id_str.size(), device_clock_id.size())); + time_event_stamp.device_clock_id = device_clock_id; + TimeEvent::UnsignedCharArray local_clock_name_arr; + local_clock_id_str.copy(local_clock_name_arr.begin(), std::min(local_clock_id_str.size(),local_clock_name_arr.size())); + time_event_stamp.local_clock_id = local_clock_name_arr; + m_attemptedCommunicationCount++; if (haveNewData) { boost::lock_guard lock(jt_mutex); - // if there were problems sending commands, start by sending the current - // position - // if(this->m_haveReceivedRealDataCount > - // minimumConsecutiveSuccessesBeforeSendingCommands-1) - // { - // boost::lock_guard lock(jt_mutex); - // // initialize arm commands to current arm position - // armState.clearCommands(); - //// armState.commandedPosition.clear(); - //// armState.commandedTorque.clear(); - //// grl::robot::arm::copy(friData_->monitoringMsg, - /// std::back_inserter(armState.commandedPosition), - /// grl::revolute_joint_angle_open_chain_command_tag()); - //// grl::robot::arm::copy(friData_->monitoringMsg, - /// std::back_inserter(armState.commandedTorque) , - /// grl::revolute_joint_torque_open_chain_command_tag()); - // } m_attemptedCommunicationConsecutiveSuccessCount++; this->m_attemptedCommunicationConsecutiveFailureCount = 0; @@ -250,11 +302,21 @@ class KukaFRIdriver : public std::enable_shared_from_this< // only supported for kuka sunrise OS 1.9 #ifdef KUKA_SUNRISE_1_9 - armState.externalForce.clear(); - grl::robot::arm::copy(friData_->monitoringMsg, - std::back_inserter(armState.externalForce), - grl::cartesian_external_force_tag()); + armState.externalForce.clear(); + grl::robot::arm::copy(friData_->monitoringMsg, + std::back_inserter(armState.externalForce), + grl::cartesian_external_force_tag()); #endif // KUKA_SUNRISE_1_9 + armState.commandedPosition.clear(); + grl::robot::arm::copy( + friData_->monitoringMsg, std::back_inserter(armState.commandedPosition), + grl::revolute_joint_angle_open_chain_command_tag()); + + armState.commandedTorque.clear(); + grl::robot::arm::copy( + friData_->monitoringMsg, std::back_inserter(armState.commandedTorque), + grl::revolute_joint_torque_open_chain_command_tag()); + armState.ipoJointPosition.clear(); grl::robot::arm::copy( friData_->monitoringMsg, @@ -266,23 +328,53 @@ class KukaFRIdriver : public std::enable_shared_from_this< armState.time_event_stamp = time_event_stamp; - std::cout << "Measured Torque: "; - std::cout << std::setw(6); - for (float t:armState.torque) { - std::cout << t << " "; - } - std::cout << '\n'; - std::cout << "External Torque: "; - std::cout << std::setw(6); - for (float t:armState.externalTorque) { - std::cout << t << " "; - } - std::cout << '\n'; - std::cout << "External Force: "; - for (float t:armState.externalForce) { - std::cout << t << " "; - } - std::cout << '\n'; + // std::cout<<" armState.time_event_stamp -------------" << std::endl; + // for (int i = 0; i FRIConfig = grl::toFlatBuffer(*m_logFileBufferBuilderP, overlayType, connectionInfo, updatePortOnRemote, portOnRemote, updatePortOnController, portOnController); std::vector> tools; - std::vector> processData; + std::vector> processData_vec; // @TODO(Chunting) Initialize Pose with 0, we ought to calculate it later with ::Transformation; // Also assign the configuration parameters with random values, we can figure them out later. for(int i=0; i<7; i++) { @@ -572,9 +664,8 @@ class KukaFRIdriver : public std::enable_shared_from_this< grl::flatbuffer::Pose pose = grl::flatbuffer::Pose(grl::flatbuffer::Vector3d(0,0,0), grl::flatbuffer::Quaternion(0,0,0,0)); grl::flatbuffer::Inertia inertia = grl::flatbuffer::Inertia(1, pose, 0, 0, 0, 0, 0, 0); flatbuffers::Offset linkObject = grl::toFlatBuffer(*m_logFileBufferBuilderP, linkname, parent, pose, inertia); - tools.push_back(linkObject); - processData.push_back( - grl::toFlatBuffer(*m_logFileBufferBuilderP, + auto singleprocessdata = grl::toFlatBuffer( + *m_logFileBufferBuilderP, "dataType"+ std::to_string(i), "defaultValue"+ std::to_string(i), "displayName"+ std::to_string(i), @@ -582,7 +673,9 @@ class KukaFRIdriver : public std::enable_shared_from_this< "min"+ std::to_string(i), "max"+ std::to_string(i), "unit"+ std::to_string(i), - "value"+ std::to_string(i))); + "value"+ std::to_string(i)); + tools.push_back(linkObject); + processData_vec.push_back(singleprocessdata); } // Set the configuration of the Kuka iiwa flatbuffers::Offset kukaiiwaArmConfiguration = grl::toFlatBuffer( @@ -598,17 +691,17 @@ class KukaFRIdriver : public std::enable_shared_from_this< setSmartServo, FRIConfig, tools, - processData, + processData_vec, "currentMotionCenter", true); - grl::TimeEvent time_event_stamp; + flatbuffers::Offset friMessageLog = grl::toFlatBuffer( *m_logFileBufferBuilderP, friSessionState, friConnectionQuality, controlMode, monitoringMsg, - time_event_stamp); + armState.time_event_stamp); // getWrench() is availble in KukaJAVAdriver, so maybe it's better to log data in KukaDriver where user can access both KukaFRIDriver and KukaJAVADriver? grl::flatbuffer::Wrench cartesianWrench{grl::flatbuffer::Vector3d(0, 0, 0),grl::flatbuffer::Vector3d(0, 0, 0),grl::flatbuffer::Vector3d(0, 0, 0)}; // In armState there is neither joint velocity nor acceleration @@ -616,11 +709,20 @@ class KukaFRIdriver : public std::enable_shared_from_this< std::vector velocity(7,0); std::vector acceleration(7,0); std::vector torque; + std::vector jointIpoPostion; + std::vector externalTorque; for(int i = 0; i<7; i++) { position.push_back(armState.position[i]); torque.push_back(armState.torque[i]); + jointIpoPostion.push_back(armState.ipoJointPosition[i]); + externalTorque.push_back(armState.externalTorque[i]); + } flatbuffers::Offset jointStatetab = grl::toFlatBuffer(*m_logFileBufferBuilderP, position, velocity, acceleration, torque); + torque.clear(); + flatbuffers::Offset jointIpoState = grl::toFlatBuffer(*m_logFileBufferBuilderP, jointIpoPostion, velocity, acceleration, torque); + position.clear(); + flatbuffers::Offset externalState = grl::toFlatBuffer(*m_logFileBufferBuilderP, position, velocity, acceleration, externalTorque); // Calculate the data later. // Cartesian pose of the flange relative to the base of the arm grl::flatbuffer::Pose cartesianFlangePose = grl::flatbuffer::Pose(grl::flatbuffer::Vector3d(0, 0, 0), grl::flatbuffer::Quaternion(0,0,0,0)); @@ -629,14 +731,14 @@ class KukaFRIdriver : public std::enable_shared_from_this< jointStatetab, // flatbuffers::Offset &measuredState, cartesianFlangePose, jointStatetab, // flatbuffers::Offset &jointStateReal, - jointStatetab, // flatbuffers::Offset &jointStateInterpolated, - jointStatetab, // flatbuffers::Offset &externalState, + jointIpoState, // flatbuffers::Offset &jointStateInterpolated, + externalState, // flatbuffers::Offset &externalState, friSessionState, robotInfo.operationMode, cartesianWrench); // Set it up in the configuration file - std::vector torqueSensorLimits(7,0); - std::string hardwareVersion( "hardvareVersion"); + std::vector torqueSensorLimits(7,0.1); + std::string hardwareVersion("hardvareVersion"); bool isReadyToMove = true; bool isMastered = true; flatbuffers::Offset monitorConfig = grl::toFlatBuffer( @@ -645,13 +747,14 @@ class KukaFRIdriver : public std::enable_shared_from_this< torqueSensorLimits, isReadyToMove, isMastered, - processData); + processData_vec); flatbuffers::Offset KUKAiiwaState = grl::toFlatBuffer( *m_logFileBufferBuilderP, RobotName, destination, source, - duration, + // duration, + armState.time_event_stamp, true, controlState, true, kukaiiwaArmConfiguration, true, kukaiiwaMonitorState, diff --git a/include/grl/kuka/KukaToFlatbuffer.hpp b/include/grl/kuka/KukaToFlatbuffer.hpp index db461b0..b75c91a 100644 --- a/include/grl/kuka/KukaToFlatbuffer.hpp +++ b/include/grl/kuka/KukaToFlatbuffer.hpp @@ -393,7 +393,7 @@ flatbuffers::Offset toFlatBuffer( { return grl::flatbuffer::CreateArmControlSeries( fbb, - armcontrolstates.empty()?fbb.CreateVector>(armcontrolstates):0); + fbb.CreateVector>(armcontrolstates)); } /// KUKAiiwa.fbs flatbuffers::Offset toFlatBuffer( @@ -527,9 +527,9 @@ flatbuffers::Offset toFlatBuffer( setJointImpedance, smartServoConfig, FRIConfig, - tools.empty() ? fbb.CreateVector>(tools):0, - processData.empty() ? fbb.CreateVector>(processData) : 0, - currentMotionCenter.empty() ? fbb.CreateString(currentMotionCenter) : 0, + fbb.CreateVector>(tools), + fbb.CreateVector>(processData), + fbb.CreateString(currentMotionCenter), requestMonitorProcessData); } @@ -543,11 +543,11 @@ flatbuffers::Offset toFlatBuffer( { return grl::flatbuffer::CreateKUKAiiwaMonitorConfiguration( fbb, - hardwareVersion.empty() ? fbb.CreateString(hardwareVersion) : 0, - torqueSensorLimits.empty() ? fbb.CreateVector(torqueSensorLimits) : 0, + fbb.CreateString(hardwareVersion), + fbb.CreateVector(torqueSensorLimits), isReadyToMove, isMastered, - processData.empty() ? fbb.CreateVector>(processData) : 0); + fbb.CreateVector>(processData)); } /// KUKAiiwa.fbs @@ -582,7 +582,8 @@ flatbuffers::Offset toFlatBuffer( const std::string &name, const std::string &destination, const std::string &source, - const double timestamp, + // const double timestamp, + const grl::TimeEvent &timeEvent, const bool setArmControlState, flatbuffers::Offset &armControlState, const bool setArmConfiguration, @@ -592,12 +593,15 @@ flatbuffers::Offset toFlatBuffer( const bool hasMonitorConfig, flatbuffers::Offset &monitorConfig) { + + flatbuffers::Offset _timeEvent = toFlatBuffer(fbb, timeEvent); + return grl::flatbuffer::CreateKUKAiiwaState( fbb, fbb.CreateString(name), fbb.CreateString(destination), fbb.CreateString(source), - timestamp, + _timeEvent, setArmControlState, armControlState, setArmConfiguration, @@ -626,7 +630,7 @@ flatbuffers::Offset toFlatBuffer( auto _sequenceCounter = friMonitoringMessage.header.sequenceCounter; auto _reflectedSequenceCounter = friMonitoringMessage.header.reflectedSequenceCounter; std::vector data; - // get measured joint position + // get measured joint grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_angle_open_chain_state_tag()); flatbuffers::Offset> _measuredJointPosition = fbb.CreateVector(data); @@ -637,14 +641,23 @@ flatbuffers::Offset toFlatBuffer( data.clear(); // get measured joint torque - grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_torque_open_chain_command_tag()); + grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_angle_open_chain_command_tag()); flatbuffers::Offset> _commandedJointPosition = fbb.CreateVector(data); - + std::cout<<"Command Joint Position:"; + for (int i=0; i<7; i++) { + std::cout<> _commandedTorque = fbb.CreateVector(data); - + std::cout<<"Command Joint TOrque:"; + for (int i=0; i<7; i++) { + std::cout< toFlatBuffer( grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_angle_interpolated_open_chain_state_tag()); flatbuffers::Offset> _jointStateInterpolated = fbb.CreateVector(data); - flatbuffers::Offset _timeEvent = grl::toFlatBuffer(fbb, timeEvent); - + flatbuffers::Offset _timeEvent = toFlatBuffer(fbb, timeEvent); auto _overlayType = toFlatBuffer(friMonitoringMessage.ipoData.overlayType); - return grl::flatbuffer::CreateFRIMessageLog( + return grl::flatbuffer::CreateFRIMessageLog( fbb, _sessionState, _connectionQuality, @@ -683,7 +695,8 @@ flatbuffers::Offset toFlatBuffer( const std::string &name, const std::string &destination, const std::string &source, - const double timestamp, + // const double timestamp, + const grl::TimeEvent &timeEvent, const bool setArmControlState, const flatbuffers::Offset &armControlState, const bool setArmConfiguration, @@ -695,12 +708,14 @@ flatbuffers::Offset toFlatBuffer( const flatbuffers::Offset &FRIMessage) { + std::cout<<"To generate TimeEvent flatbuffer Object:" < _timeEvent = grl::toFlatBuffer(fbb, timeEvent); return grl::flatbuffer::CreateKUKAiiwaState( fbb, fbb.CreateString(name), fbb.CreateString(destination), fbb.CreateString(source), - timestamp, + _timeEvent, setArmControlState, armControlState, setArmConfiguration, @@ -738,7 +753,7 @@ bool FinishAndVerifyBuffer( // Finish a buffer with given object // Call `Finish()` to instruct the builder fbb that this frame is complete. const char *file_identifier = grl::flatbuffer::KUKAiiwaStatesIdentifier(); - // fbb.Finish(oneKUKAiiwaFusionTrackMessage, file_identifier); + fbb.Finish(fbLogKUKAiiwaStates, file_identifier); flatbuffers::uoffset_t _max_depth = 64; diff --git a/include/grl/sensor/FusionTrackToFlatbuffer.hpp b/include/grl/sensor/FusionTrackToFlatbuffer.hpp index 3cf346e..bf144b0 100644 --- a/include/grl/sensor/FusionTrackToFlatbuffer.hpp +++ b/include/grl/sensor/FusionTrackToFlatbuffer.hpp @@ -469,43 +469,6 @@ toFlatBuffer(flatbuffers::FlatBufferBuilder &fbb, fb_m_device_types); } -// template -// typename T::value_type stringLength(const T &array) -// { - -// auto iter = std::find(array.begin(), array.end(), '\0'); -// auto len = std::distance(array.begin(), iter); -// return len; -// } - -// flatbuffers::Offset -// toFlatBuffer(flatbuffers::FlatBufferBuilder &fbb, -// const grl::TimeEvent &timeStamp) -// { -// flatbuffers::Offset event_name = fbb.CreateString(const_cast(timeStamp.event_name.begin()), stringLength(timeStamp.event_name)); -// /// https://github.com/googlecartographer/cartographer/blob/master/cartographer/common/time.cc -// /// convert time to int64 -// int64_t local_request_time = cartographer::common::ToUniversal(timeStamp.local_request_time); -// flatbuffers::Offset device_clock_id = fbb.CreateString(const_cast(timeStamp.device_clock_id.begin()), stringLength(timeStamp.device_clock_id)); -// int64_t device_time = cartographer::common::ToUniversal(timeStamp.device_time); -// flatbuffers::Offset local_clock_id = fbb.CreateString(const_cast(timeStamp.local_clock_id.begin()), stringLength(timeStamp.local_clock_id)); -// int64_t local_receive_time = cartographer::common::ToUniversal(timeStamp.local_receive_time); -// int64_t corrected_local_time = cartographer::common::ToUniversal(timeStamp.corrected_local_time); -// int64_t clock_skew = cartographer::common::ToSeconds(timeStamp.clock_skew); -// int64_t min_transport_delay = cartographer::common::ToSeconds(timeStamp.min_transport_delay); -// return grl::flatbuffer::CreateTimeEvent( -// fbb, -// event_name, -// local_request_time, -// device_clock_id, -// device_time, -// local_clock_id, -// local_receive_time, -// corrected_local_time, -// clock_skew, -// min_transport_delay); -// } - flatbuffers::Offset toFlatBuffer(flatbuffers::FlatBufferBuilder &fbb, const grl::sensor::FusionTrack &fusiontrack, diff --git a/test/KukaFRITest.cpp b/test/KukaFRITest.cpp index 39ab815..721e68b 100644 --- a/test/KukaFRITest.cpp +++ b/test/KukaFRITest.cpp @@ -147,7 +147,8 @@ int main(int argc, char* argv[]) { /// @todo TODO(ahundt) BUG: Need way to supply time to reach specified goal for position control and eliminate this allocation internally in the kuka driver. See similar comment in KukaFRIDriver.hpp /// IDEA: PASS A LOW LEVEL STEP ALGORITHM PARAMS OBJECT ON EACH UPDATE AND ONLY ONE INSTANCE OF THE ALGORITHM OBJECT ITSELF - highLevelDriverClassP = std::make_shared>(io_service, + highLevelDriverClassP = std::make_shared>( + io_service, std::make_tuple("KUKA_LBR_IIWA_14_R820", localhost, localport, @@ -189,7 +190,7 @@ int main(int argc, char* argv[]) "FRI" // KukaMonitorMode (options are FRI, JAVA) ); /// @todo TODO(ahundt) Currently assumes ip address - kukaDriverP=std::make_shared(params); + kukaDriverP = std::make_shared(params); kukaDriverP->construct(); // Default to joint servo mode for commanding motion kukaDriverP->set(grl::flatbuffer::ArmState::MoveArmJointServo); diff --git a/test/kukaiiwaTest.cpp b/test/kukaiiwaTest.cpp index f62aca4..f76f221 100644 --- a/test/kukaiiwaTest.cpp +++ b/test/kukaiiwaTest.cpp @@ -141,7 +141,7 @@ BOOST_AUTO_TEST_CASE(runRepeatedly) ); /// @todo TODO(ahundt) Currently assumes ip address kukaDriverP=std::make_shared(params); - kukaDriverP->construct(); + // kukaDriverP->construct(); // Default to joint servo mode for commanding motion kukaDriverP->set(grl::flatbuffer::ArmState::MoveArmJointServo); kukaDriverP->set(goal_position_command_time_duration,grl::time_duration_command_tag()); @@ -368,7 +368,7 @@ BOOST_AUTO_TEST_CASE(runRepeatedly) RobotName, destination, source, - duration, + time_event_stamp, true, controlState, true, kukaiiwaArmConfiguration, true, kukaiiwaMonitorState, From 480eea4af5d0284281f907b6e466c839448c2081 Mon Sep 17 00:00:00 2001 From: Chunting Date: Wed, 17 Jan 2018 11:34:31 -0500 Subject: [PATCH 031/102] Log data and generate json file --- include/grl/flatbuffer/HelperToFlatbuffer.hpp | 4 + include/grl/kuka/KukaFRIClientDataDriver.hpp | 1 + include/grl/kuka/KukaFRIdriver.hpp | 238 +++++++++--------- include/grl/kuka/KukaToFlatbuffer.hpp | 9 +- 4 files changed, 130 insertions(+), 122 deletions(-) diff --git a/include/grl/flatbuffer/HelperToFlatbuffer.hpp b/include/grl/flatbuffer/HelperToFlatbuffer.hpp index d65d04b..9d96c53 100644 --- a/include/grl/flatbuffer/HelperToFlatbuffer.hpp +++ b/include/grl/flatbuffer/HelperToFlatbuffer.hpp @@ -53,6 +53,10 @@ toFlatBuffer(flatbuffers::FlatBufferBuilder &fbb, const grl::TimeEvent &timeStamp) { flatbuffers::Offset event_name = fbb.CreateString(const_cast(timeStamp.event_name.begin()), stringLength(timeStamp.event_name)); + for(int i = 0; i; using duration = std::chrono::duration; using time_point = std::chrono::time_point; + static constexpr bool is_steady = true; /// TODO(ahundt) currently assuming the FusionTrack timestamp is from the unix time epoch @@ -168,7 +168,6 @@ class KukaFRIdriver : public std::enable_shared_from_this< } }; - /// TODO(ahundt) currently assuming the FusionTrack timestamp is from the unix time epoch cartographer::common::Time KukaTimeToCommonTime(typename MicrosecondClock::time_point Kukatime) { @@ -179,8 +178,14 @@ class KukaFRIdriver : public std::enable_shared_from_this< cartographer::common::Time FRITimeStampToCommonTime(const ::TimeStamp &friTimeStamp) { - typename MicrosecondClock::time_point fritp(MicrosecondClock::duration(friTimeStamp.nanosec)); + // Convert the time to microseconds + int64_t seconds = friTimeStamp.sec; + int64_t nanosecs = friTimeStamp.nanosec; + int64_t microseconds = seconds *1000000 + nanosecs/1000; + typename MicrosecondClock::time_point fritp = typename MicrosecondClock::time_point(typename MicrosecondClock::duration(microseconds)); + return KukaTimeToCommonTime(fritp); + } /** @@ -192,9 +197,6 @@ class KukaFRIdriver : public std::enable_shared_from_this< * */ bool run_one() { - grl::TimeEvent time_event_stamp; - - // note: this one sends *and* receives the joint data! BOOST_VERIFY(kukaFRIClientDataDriverP_.get() != nullptr); @@ -220,7 +222,7 @@ class KukaFRIdriver : public std::enable_shared_from_this< /// @todo TODO(ahundt) Need to eliminate this allocation boost::lock_guard lock(jt_mutex); - boost::container::static_vector jointStateToCommand; + boost::container::static_vector jointStateToCommand; boost::copy(armState.commandedPosition,std::back_inserter(jointStateToCommand)); // pass time to reach specified goal for position control lowLevelStepAlgorithmCommandParamsP = std::make_shared(std::make_tuple(jointStateToCommand,armState.goal_position_command_time_duration)); @@ -249,31 +251,6 @@ class KukaFRIdriver : public std::enable_shared_from_this< send_bytes, time_event_stamp); - std::string s_event_name = "/kukaiiwa/state"; - - /// Name for the clock on the kukaiiws - /// Useful for timing calculations and debugging. - /// This one string you will want to check when analyzing - /// logged data for time differences and error. - std::string device_clock_id_str = "/kukaiiwa/state/clock"; - - /// Name for the local clock on which this driver runs - /// Useful for timing calculations and debugging. - /// defaults to "/control_computer/clock/steady" - /// This one string you will want to check when analyzing - /// logged data for time differences and error. - std::string local_clock_id_str = "/control_computer/clock/steady"; - - TimeEvent::UnsignedCharArray event_name; - s_event_name.copy(event_name.begin(), std::min(s_event_name.size(), event_name.size())); - time_event_stamp.event_name = event_name; - TimeEvent::UnsignedCharArray device_clock_id; - device_clock_id_str.copy(device_clock_id.begin(), std::min(device_clock_id_str.size(), device_clock_id.size())); - time_event_stamp.device_clock_id = device_clock_id; - TimeEvent::UnsignedCharArray local_clock_name_arr; - local_clock_id_str.copy(local_clock_name_arr.begin(), std::min(local_clock_id_str.size(),local_clock_name_arr.size())); - time_event_stamp.local_clock_id = local_clock_name_arr; - m_attemptedCommunicationCount++; if (haveNewData) { @@ -326,10 +303,11 @@ class KukaFRIdriver : public std::enable_shared_from_this< armState.sendPeriod = std::chrono::milliseconds( grl::robot::arm::get(friData_->monitoringMsg, grl::time_step_tag())); + oneKUKAiiwaStateBuffer(); armState.time_event_stamp = time_event_stamp; // std::cout<<" armState.time_event_stamp -------------" << std::endl; - // for (int i = 0; i lock(jt_mutex); - std::string RobotName("Robotiiwa" ); - std::string destination("where this message is going (URI)"); - std::string source("where this message came from (URI)"); + std::string RobotName = std::string(std::get(params_)); + + std:: string destination = std::string(std::get(params_)); + std::string source = std::string(std::get(params_)); + int16_t portOnRemote = std::stoi(std::string(std::get(params_))); + + int16_t portOnController = std::stoi(std::string(std::get(params_))); std::string basename = RobotName; //std::get<0>(params); + bool setArmConfiguration_ = true; // set the arm config first time bool max_control_force_stop_ = false; - // @TODO(Chunting) Put all these parameters in a configuration file. - std::vector joint_stiffness_(7, 0); - std::vector joint_damping_(7, 0); - std::vector joint_AccelerationRel_(7, 0); - std::vector joint_VelocityRel_(7, 0); + std::vector joint_stiffness_(KUKA::LBRState::NUM_DOF, 0); + std::vector joint_damping_(KUKA::LBRState::NUM_DOF, 0); + std::vector joint_AccelerationRel_(KUKA::LBRState::NUM_DOF, 0); + std::vector joint_VelocityRel_(KUKA::LBRState::NUM_DOF, 0); bool updateMinimumTrajectoryExecutionTime = false; double minimumTrajectoryExecutionTime = 4; + //Cartesian Impedance Values - grl::flatbuffer::Vector3d cart_stiffness_trans_ = grl::flatbuffer::Vector3d(0,0,0); - grl::flatbuffer::EulerRotation cart_stifness_rot_ = grl::flatbuffer::EulerRotation(0,0,0,grl::flatbuffer::EulerOrder::xyz); - grl::flatbuffer::Vector3d cart_damping_trans_ = grl::flatbuffer::Vector3d(0,0,0); - grl::flatbuffer::EulerRotation cart_damping_rot_ = grl::flatbuffer::EulerRotation(0,0,0,grl::flatbuffer::EulerOrder::xyz); - grl::flatbuffer::EulerPose cart_stiffness_ = grl::flatbuffer::EulerPose(cart_stiffness_trans_, cart_stifness_rot_); - grl::flatbuffer::EulerPose cart_damping_ = grl::flatbuffer::EulerPose(cart_damping_trans_, cart_damping_rot_); - grl::flatbuffer::EulerPose cart_max_path_deviation_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(0,0,0), grl::flatbuffer::EulerRotation(0,0,0, grl::flatbuffer::EulerOrder::xyz)); - grl::flatbuffer::EulerPose cart_max_ctrl_vel_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(0,0,0), grl::flatbuffer::EulerRotation(0,0,0, grl::flatbuffer::EulerOrder::xyz)); - grl::flatbuffer::EulerPose cart_max_ctrl_force_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(0,0,0), grl::flatbuffer::EulerRotation(0.,0.,0., grl::flatbuffer::EulerOrder::xyz)); - double nullspace_stiffness_ = 0; - double nullspace_damping_ = 0; + // grl::flatbuffer::Vector3d cart_stiffness_trans_ = grl::flatbuffer::Vector3d(0,0,0); + // grl::flatbuffer::EulerRotation cart_stifness_rot_ = grl::flatbuffer::EulerRotation(0,0,0,grl::flatbuffer::EulerOrder::xyz); + // grl::flatbuffer::Vector3d cart_damping_trans_ = grl::flatbuffer::Vector3d(0,0,0); + // grl::flatbuffer::EulerRotation cart_damping_rot_ = grl::flatbuffer::EulerRotation(0,0,0,grl::flatbuffer::EulerOrder::xyz); + // grl::flatbuffer::EulerPose cart_stiffness_ = grl::flatbuffer::EulerPose(cart_stiffness_trans_, cart_stifness_rot_); + // grl::flatbuffer::EulerPose cart_damping_ = grl::flatbuffer::EulerPose(cart_damping_trans_, cart_damping_rot_); + // grl::flatbuffer::EulerPose cart_max_path_deviation_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(0,0,0), grl::flatbuffer::EulerRotation(0,0,0, grl::flatbuffer::EulerOrder::xyz)); + // grl::flatbuffer::EulerPose cart_max_ctrl_vel_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(0,0,0), grl::flatbuffer::EulerRotation(0,0,0, grl::flatbuffer::EulerOrder::xyz)); + // grl::flatbuffer::EulerPose cart_max_ctrl_force_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(0,0,0), grl::flatbuffer::EulerRotation(0.,0.,0., grl::flatbuffer::EulerOrder::xyz)); + // double nullspace_stiffness_ = 0; + // double nullspace_damping_ = 0; bool updatePortOnRemote = false; - int16_t portOnRemote = 3501; + bool updatePortOnController = false; - int16_t portOnController = 3502; + // Resolve the data in FRIMonitoringMessage // Don't match the FRIMessage.pb.h grl::flatbuffer::ArmState armControlMode = grl::flatbuffer::ArmState::StartArm; @@ -600,7 +582,7 @@ class KukaFRIdriver : public std::enable_shared_from_this< // RobotInfo // how to use pb_callback_t driveState in RobotInfo? ::RobotInfo robotInfo = monitoringMsg.robotInfo; - int NUM_DOF = robotInfo.has_numberOfJoints?robotInfo.numberOfJoints:7; + int NUM_DOF = robotInfo.has_numberOfJoints?robotInfo.numberOfJoints:KUKA::LBRState::NUM_DOF; ::ControlMode controlMode = robotInfo.controlMode; ::SafetyState safetyState = robotInfo.safetyState; ::OperationMode operationMode = robotInfo.operationMode; @@ -621,6 +603,21 @@ class KukaFRIdriver : public std::enable_shared_from_this< ::MessageMonitorData messageMonitorData = monitoringMsg.monitorData; + std::string s_event_name = RobotName + "/state"; + std::string device_clock_id_str = s_event_name + "/device/clock"; + std::string local_clock_id_str = s_event_name + "/control_computer/clock/steady"; + + TimeEvent::UnsignedCharArray event_name; + s_event_name.copy(event_name.begin(), std::min(s_event_name.size(), event_name.size())); + time_event_stamp.event_name = event_name; + TimeEvent::UnsignedCharArray device_clock_id; + device_clock_id_str.copy(device_clock_id.begin(), std::min(device_clock_id_str.size(), device_clock_id.size())); + time_event_stamp.device_clock_id = device_clock_id; + TimeEvent::UnsignedCharArray local_clock_name_arr; + local_clock_id_str.copy(local_clock_name_arr.begin(), std::min(local_clock_id_str.size(),local_clock_name_arr.size())); + time_event_stamp.local_clock_id = local_clock_name_arr; + time_event_stamp.device_time = FRITimeStampToCommonTime(messageMonitorData.timestamp); + //std::cout<< time_event_stamp.event_name << std::endl << time_event_stamp.device_clock_id << std::endl << time_event_stamp.local_clock_id < controlState = grl::toFlatBuffer(*m_logFileBufferBuilderP, basename, sequenceNumber++, duration, armState, armControlMode); - flatbuffers::Offset setCartesianImpedance = grl::toFlatBuffer(*m_logFileBufferBuilderP, cart_stiffness_, cart_damping_, - nullspace_stiffness_, nullspace_damping_, cart_max_path_deviation_, cart_max_ctrl_vel_, cart_max_ctrl_force_, max_control_force_stop_); - flatbuffers::Offset setJointImpedance = grl::toFlatBuffer(*m_logFileBufferBuilderP, joint_stiffness_, joint_damping_); + // flatbuffers::Offset setCartesianImpedance = grl::toFlatBuffer(*m_logFileBufferBuilderP, cart_stiffness_, cart_damping_, + // nullspace_stiffness_, nullspace_damping_, cart_max_path_deviation_, cart_max_ctrl_vel_, cart_max_ctrl_force_, max_control_force_stop_); + flatbuffers::Offset setCartesianImpedance = 0; + // flatbuffers::Offset setJointImpedance = grl::toFlatBuffer(*m_logFileBufferBuilderP, joint_stiffness_, joint_damping_); + flatbuffers::Offset setJointImpedance = 0; // normalized joint accelerations/velocities from 0 to 1 relative to system capabilities // how to get the acceleration of the robot? There is no acceleration information in KukaState (armState). flatbuffers::Offset setSmartServo = grl::toFlatBuffer(*m_logFileBufferBuilderP, joint_AccelerationRel_, joint_VelocityRel_, updateMinimumTrajectoryExecutionTime, minimumTrajectoryExecutionTime); @@ -657,43 +656,44 @@ class KukaFRIdriver : public std::enable_shared_from_this< std::vector> processData_vec; // @TODO(Chunting) Initialize Pose with 0, we ought to calculate it later with ::Transformation; // Also assign the configuration parameters with random values, we can figure them out later. - for(int i=0; i<7; i++) { - std::string linkname = "Link" + std::to_string(i); - std::string parent = i==0?"Base":("Link" + std::to_string(i-1)); - // Compute pose later with transformation matrix - grl::flatbuffer::Pose pose = grl::flatbuffer::Pose(grl::flatbuffer::Vector3d(0,0,0), grl::flatbuffer::Quaternion(0,0,0,0)); - grl::flatbuffer::Inertia inertia = grl::flatbuffer::Inertia(1, pose, 0, 0, 0, 0, 0, 0); - flatbuffers::Offset linkObject = grl::toFlatBuffer(*m_logFileBufferBuilderP, linkname, parent, pose, inertia); - auto singleprocessdata = grl::toFlatBuffer( - *m_logFileBufferBuilderP, - "dataType"+ std::to_string(i), - "defaultValue"+ std::to_string(i), - "displayName"+ std::to_string(i), - "id"+ std::to_string(i), - "min"+ std::to_string(i), - "max"+ std::to_string(i), - "unit"+ std::to_string(i), - "value"+ std::to_string(i)); - tools.push_back(linkObject); - processData_vec.push_back(singleprocessdata); - } + // for(int i=0; i linkObject = grl::toFlatBuffer(*m_logFileBufferBuilderP, linkname, parent, pose, inertia); + // auto singleprocessdata = grl::toFlatBuffer( + // *m_logFileBufferBuilderP, + // "dataType"+ std::to_string(i), + // "defaultValue"+ std::to_string(i), + // "displayName"+ std::to_string(i), + // "id"+ std::to_string(i), + // "min"+ std::to_string(i), + // "max"+ std::to_string(i), + // "unit"+ std::to_string(i), + // "value"+ std::to_string(i)); + // tools.push_back(linkObject); + // processData_vec.push_back(singleprocessdata); + // } // Set the configuration of the Kuka iiwa - flatbuffers::Offset kukaiiwaArmConfiguration = grl::toFlatBuffer( - *m_logFileBufferBuilderP, - RobotName, - commandInterface, - monitorInterface, - clientCommandMode, - overlayType, - controlMode, - setCartesianImpedance, - setJointImpedance, - setSmartServo, - FRIConfig, - tools, - processData_vec, - "currentMotionCenter", - true); + // flatbuffers::Offset kukaiiwaArmConfiguration = grl::toFlatBuffer( + // *m_logFileBufferBuilderP, + // RobotName, + // commandInterface, + // monitorInterface, + // clientCommandMode, + // overlayType, + // controlMode, + // setCartesianImpedance, + // setJointImpedance, + // setSmartServo, + // FRIConfig, + // tools, + // processData_vec, + // "currentMotionCenter", + // true); + flatbuffers::Offset kukaiiwaArmConfiguration = 0; flatbuffers::Offset friMessageLog = grl::toFlatBuffer( *m_logFileBufferBuilderP, @@ -703,20 +703,20 @@ class KukaFRIdriver : public std::enable_shared_from_this< monitoringMsg, armState.time_event_stamp); // getWrench() is availble in KukaJAVAdriver, so maybe it's better to log data in KukaDriver where user can access both KukaFRIDriver and KukaJAVADriver? - grl::flatbuffer::Wrench cartesianWrench{grl::flatbuffer::Vector3d(0, 0, 0),grl::flatbuffer::Vector3d(0, 0, 0),grl::flatbuffer::Vector3d(0, 0, 0)}; + // grl::flatbuffer::Wrench cartesianWrench{grl::flatbuffer::Vector3d(0, 0, 0),grl::flatbuffer::Vector3d(0, 0, 0),grl::flatbuffer::Vector3d(0, 0, 0)}; + grl::flatbuffer::Wrench cartesianWrench; // In armState there is neither joint velocity nor acceleration std::vector position; - std::vector velocity(7,0); - std::vector acceleration(7,0); + std::vector velocity; + std::vector acceleration; std::vector torque; std::vector jointIpoPostion; std::vector externalTorque; - for(int i = 0; i<7; i++) { + for(int i = 0; i jointStatetab = grl::toFlatBuffer(*m_logFileBufferBuilderP, position, velocity, acceleration, torque); torque.clear(); @@ -725,7 +725,9 @@ class KukaFRIdriver : public std::enable_shared_from_this< flatbuffers::Offset externalState = grl::toFlatBuffer(*m_logFileBufferBuilderP, position, velocity, acceleration, externalTorque); // Calculate the data later. // Cartesian pose of the flange relative to the base of the arm - grl::flatbuffer::Pose cartesianFlangePose = grl::flatbuffer::Pose(grl::flatbuffer::Vector3d(0, 0, 0), grl::flatbuffer::Quaternion(0,0,0,0)); + // grl::flatbuffer::Pose cartesianFlangePose = grl::flatbuffer::Pose(grl::flatbuffer::Vector3d(0, 0, 0), grl::flatbuffer::Quaternion(0,0,0,0)); + grl::flatbuffer::Pose cartesianFlangePose{}; + std::cout <<"Size of an empty struct: " << sizeof(cartesianFlangePose) << std::endl; flatbuffers::Offset kukaiiwaMonitorState = grl::toFlatBuffer( *m_logFileBufferBuilderP, jointStatetab, // flatbuffers::Offset &measuredState, @@ -737,17 +739,18 @@ class KukaFRIdriver : public std::enable_shared_from_this< robotInfo.operationMode, cartesianWrench); // Set it up in the configuration file - std::vector torqueSensorLimits(7,0.1); + std::vector torqueSensorLimits(KUKA::LBRState::NUM_DOF,0.1); std::string hardwareVersion("hardvareVersion"); bool isReadyToMove = true; bool isMastered = true; - flatbuffers::Offset monitorConfig = grl::toFlatBuffer( - *m_logFileBufferBuilderP, - hardwareVersion, - torqueSensorLimits, - isReadyToMove, - isMastered, - processData_vec); + // flatbuffers::Offset monitorConfig = grl::toFlatBuffer( + // *m_logFileBufferBuilderP, + // hardwareVersion, + // torqueSensorLimits, + // isReadyToMove, + // isMastered, + // processData_vec); + flatbuffers::Offset monitorConfig = 0; flatbuffers::Offset KUKAiiwaState = grl::toFlatBuffer( *m_logFileBufferBuilderP, RobotName, @@ -847,7 +850,8 @@ class KukaFRIdriver : public std::enable_shared_from_this< std::unique_ptr driver_threadP; std::shared_ptr> kukaFRIClientDataDriverP_; private: -void update() +/// Check the size of the buffer, when it hit the limit, save it to disk. +void saveToDisk() { const std::size_t MegaByte = 1024*1024; // If we write too large a flatbuffer @@ -855,8 +859,6 @@ void update() // run the primary update loop in a separate thread bool saveFileNow = false; - - // boost::lock_guard lock(jt_mutex); /// Temporarily set m_isRecording true. m_isRecording = true; if (m_isRecording) @@ -898,6 +900,8 @@ void update() /// @todo TODO(ahundt) the threads that saved files will build up forever, figure out how they can clear themselves out std::vector> m_saveRecordingThreads; + grl::TimeEvent time_event_stamp; + #ifdef HAVE_spdlog std::shared_ptr loggerP; #endif // HAVE_spdlog diff --git a/include/grl/kuka/KukaToFlatbuffer.hpp b/include/grl/kuka/KukaToFlatbuffer.hpp index b75c91a..71ab77d 100644 --- a/include/grl/kuka/KukaToFlatbuffer.hpp +++ b/include/grl/kuka/KukaToFlatbuffer.hpp @@ -232,10 +232,10 @@ flatbuffers::Offset toFlatBuffer( { return grl::flatbuffer::CreateJointState( fbb, - fbb.CreateVector(position), - fbb.CreateVector(velocity), - fbb.CreateVector(acceleration), - fbb.CreateVector(torque)); + !position.empty() ? fbb.CreateVector(position):0, + !velocity.empty() ? fbb.CreateVector(velocity):0, + !acceleration.empty() ? fbb.CreateVector(acceleration):0, + !torque.empty() ? fbb.CreateVector(torque):0); } /// JointState.fbs flatbuffers::Offset toFlatBuffer( @@ -569,7 +569,6 @@ flatbuffers::Offset toFlatBuffer( jointStateReal, jointStateInterpolated, externalState, - toFlatBuffer(operationMode), toFlatBuffer(sessionState), std::addressof(CartesianWrench) From 074d9e54276bbb646e0b69b9cd6d1457278ab522 Mon Sep 17 00:00:00 2001 From: Chunting Date: Wed, 17 Jan 2018 17:27:33 -0500 Subject: [PATCH 032/102] Log data from vrep plugin --- include/grl/kuka/KukaDriver.hpp | 112 ++++++++++------ include/grl/kuka/KukaFRIdriver.hpp | 13 +- include/grl/kuka/KukaToFlatbuffer.hpp | 5 +- include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp | 124 +++++++++++------- .../v_repExtKukaLBRiiwa.cpp | 105 +++++++++++++-- 5 files changed, 250 insertions(+), 109 deletions(-) diff --git a/include/grl/kuka/KukaDriver.hpp b/include/grl/kuka/KukaDriver.hpp index 62b9e62..7dcc70a 100644 --- a/include/grl/kuka/KukaDriver.hpp +++ b/include/grl/kuka/KukaDriver.hpp @@ -180,69 +180,97 @@ namespace grl { namespace robot { namespace arm { if(JAVAdriverP_.get() != nullptr) { - if (debug) { - std::cout << "commandedpos:" << armState_.commandedPosition << "\n"; - } + if (debug) { + std::cout << "commandedpos:" << armState_.commandedPosition << "\n"; + } - ///////////////////////////////////////// - // Do some configuration - if(boost::iequals(std::get(params_),std::string("FRI"))) - { - // configure to send commands over FRI interface - JAVAdriverP_->set(flatbuffer::KUKAiiwaInterface::FRI,command_tag()); - } + ///////////////////////////////////////// + // Do some configuration + if(boost::iequals(std::get(params_),std::string("FRI"))) + { + // configure to send commands over FRI interface + JAVAdriverP_->set(flatbuffer::KUKAiiwaInterface::FRI,command_tag()); + } - if(boost::iequals(std::get(params_),std::string("FRI"))) - { - // configure to send commands over FRI interface - JAVAdriverP_->set(flatbuffer::KUKAiiwaInterface::FRI,state_tag()); - } + if(boost::iequals(std::get(params_),std::string("FRI"))) + { + // configure to send commands over FRI interface + JAVAdriverP_->set(flatbuffer::KUKAiiwaInterface::FRI,state_tag()); + } - ///////////////////////////////////////// - // set new destination + ///////////////////////////////////////// + // set new destination - if( boost::iequals(std::get(params_),std::string("JAVA"))) - { - JAVAdriverP_->set(armState_.commandedPosition,revolute_joint_angle_open_chain_command_tag()); + if( boost::iequals(std::get(params_),std::string("JAVA"))) + { + JAVAdriverP_->set(armState_.commandedPosition,revolute_joint_angle_open_chain_command_tag()); - // configure to send commands over JAVA interface - JAVAdriverP_->set(flatbuffer::KUKAiiwaInterface::SmartServo,command_tag()); + // configure to send commands over JAVA interface + JAVAdriverP_->set(flatbuffer::KUKAiiwaInterface::SmartServo,command_tag()); - } + } - // sync JAVA driver with the robot, note client sends to server asynchronously! - haveNewData = JAVAdriverP_->run_one(); + // sync JAVA driver with the robot, note client sends to server asynchronously! + haveNewData = JAVAdriverP_->run_one(); - if( boost::iequals(std::get(params_),std::string("JAVA"))) - { - JAVAdriverP_->get(armState_); - JAVAdriverP_->set(flatbuffer::KUKAiiwaInterface::SmartServo,state_tag()); + if( boost::iequals(std::get(params_),std::string("JAVA"))) + { + JAVAdriverP_->get(armState_); + JAVAdriverP_->set(flatbuffer::KUKAiiwaInterface::SmartServo,state_tag()); - } + } } if(FRIdriverP_.get() != nullptr) { - if( boost::iequals(std::get(params_),std::string("FRI"))) - { - FRIdriverP_->set(armState_.commandedPosition,revolute_joint_angle_open_chain_command_tag()); - } - - haveNewData = FRIdriverP_->run_one(); - - if( boost::iequals(std::get(params_),std::string("FRI"))) - { - FRIdriverP_->get(armState_); - //JAVAdriverP_->getWrench(armState_); - } + if( boost::iequals(std::get(params_),std::string("FRI"))) + { + FRIdriverP_->set(armState_.commandedPosition,revolute_joint_angle_open_chain_command_tag()); + } + + haveNewData = FRIdriverP_->run_one(); + + if( boost::iequals(std::get(params_),std::string("FRI"))) + { + FRIdriverP_->get(armState_); + //JAVAdriverP_->getWrench(armState_); + } } return haveNewData; } + /// start recording the kuka state data in memory + /// return true on success, false on failure + bool start_recording() + { + if(FRIdriverP_.get() != nullptr) { + return FRIdriverP_->start_recording(); + } + return false; + } + /// stop recording the kuka state data in memory + /// return true on success, false on failure + bool stop_recording() + { + if(FRIdriverP_.get() != nullptr) { + return FRIdriverP_->stop_recording(); + } + return false; + } + bool save_recording(std::string filename = std::string()) { + if(FRIdriverP_.get() != nullptr) { + return FRIdriverP_->save_recording(filename); + } + return false; + } + void clear_recording() + { + FRIdriverP_->clear_recording(); + } /// set the mode of the arm. Examples: Teach or MoveArmJointServo /// @see grl::flatbuffer::ArmState in ArmControlState_generated.h void set(const flatbuffer::ArmState & armControlMode) diff --git a/include/grl/kuka/KukaFRIdriver.hpp b/include/grl/kuka/KukaFRIdriver.hpp index 8fe73d6..5ef5bb4 100644 --- a/include/grl/kuka/KukaFRIdriver.hpp +++ b/include/grl/kuka/KukaFRIdriver.hpp @@ -514,7 +514,7 @@ class KukaFRIdriver : public std::enable_shared_from_this< state = armState; } - /// start recording the fusiontrack frame data in memory + /// start recording the kuka state data in memory /// return true on success, false on failure bool start_recording() { @@ -522,7 +522,7 @@ class KukaFRIdriver : public std::enable_shared_from_this< m_isRecording = true; return m_isRecording; } - /// stop recording the fusiontrack frame data in memory + /// stop recording the kuka state data in memory /// return true on success, false on failure bool stop_recording() { @@ -727,7 +727,6 @@ class KukaFRIdriver : public std::enable_shared_from_this< // Cartesian pose of the flange relative to the base of the arm // grl::flatbuffer::Pose cartesianFlangePose = grl::flatbuffer::Pose(grl::flatbuffer::Vector3d(0, 0, 0), grl::flatbuffer::Quaternion(0,0,0,0)); grl::flatbuffer::Pose cartesianFlangePose{}; - std::cout <<"Size of an empty struct: " << sizeof(cartesianFlangePose) << std::endl; flatbuffers::Offset kukaiiwaMonitorState = grl::toFlatBuffer( *m_logFileBufferBuilderP, jointStatetab, // flatbuffers::Offset &measuredState, @@ -768,8 +767,6 @@ class KukaFRIdriver : public std::enable_shared_from_this< return true; } - - // bool startRecordingDataToFlatBuffer(flatbuffers::FlatBufferBuilder &*m_logFileBufferBuilderP, std::shared_ptr &friData) bool save_recording(std::string filename = std::string()) { @@ -784,9 +781,6 @@ class KukaFRIdriver : public std::enable_shared_from_this< #else // HAVE_spdlog std::cout << "Save Recording as: " << filename << std::endl; #endif // HAVE_spdlog - /// lock mutex before accessing file - // boost::lock_guard lock(jt_mutex); - auto saveLambdaFunction = [ save_fbbP = std::move(m_logFileBufferBuilderP) ,save_KUKAiiwaBufferP = std::move(m_KUKAiiwaStateBufferP) @@ -799,6 +793,7 @@ class KukaFRIdriver : public std::enable_shared_from_this< bool success = grl::FinishAndVerifyBuffer(*save_fbbP, *save_KUKAiiwaBufferP); bool write_binary_stream = true; success = success && flatbuffers::SaveFile(filename.c_str(), reinterpret_cast(save_fbbP->GetBufferPointer()), save_fbbP->GetSize(), write_binary_stream); + assert(success); /// TODO(ahFusionTrackLogAndTrackundt) replace cout with proper spdlog and vrep banner notification #ifdef HAVE_spdlog lambdaLoggerP->info("filename: ", filename, " verifier success: ", success); @@ -860,7 +855,7 @@ void saveToDisk() // run the primary update loop in a separate thread bool saveFileNow = false; /// Temporarily set m_isRecording true. - m_isRecording = true; + /// m_isRecording = true; if (m_isRecording) { // There is a flatbuffers file size limit of 2GB, but we use a conservative 512MB diff --git a/include/grl/kuka/KukaToFlatbuffer.hpp b/include/grl/kuka/KukaToFlatbuffer.hpp index 71ab77d..a33152c 100644 --- a/include/grl/kuka/KukaToFlatbuffer.hpp +++ b/include/grl/kuka/KukaToFlatbuffer.hpp @@ -707,7 +707,6 @@ flatbuffers::Offset toFlatBuffer( const flatbuffers::Offset &FRIMessage) { - std::cout<<"To generate TimeEvent flatbuffer Object:" < _timeEvent = grl::toFlatBuffer(fbb, timeEvent); return grl::flatbuffer::CreateKUKAiiwaState( fbb, @@ -722,8 +721,8 @@ flatbuffers::Offset toFlatBuffer( hasMonitorState, monitorState, hasMonitorConfig, - monitorConfig - // FRIMessage + monitorConfig, + FRIMessage ); } /// KUKAiiwa.fbs diff --git a/include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp b/include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp index 5f5184a..3b9fbdc 100644 --- a/include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp +++ b/include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp @@ -52,7 +52,7 @@ class KukaVrepPlugin : public std::enable_shared_from_this { KukaMonitorMode, IKGroupName }; - + typedef std::tuple< std::vector, std::string, @@ -71,11 +71,11 @@ class KukaVrepPlugin : public std::enable_shared_from_this { std::string, std::string > Params; - - + + static const Params defaultParams(){ std::vector jointHandles; - + jointHandles.push_back("LBR_iiwa_14_R820_joint1"); // Joint1Handle, jointHandles.push_back("LBR_iiwa_14_R820_joint2"); // Joint2Handle, jointHandles.push_back("LBR_iiwa_14_R820_joint3"); // Joint3Handle, @@ -102,12 +102,12 @@ class KukaVrepPlugin : public std::enable_shared_from_this { "IK_Group1_iiwa" // IKGroupName ); } - + /// @todo measuredArmParams are hardcoded, parameterize them // parameters for measured arm static const Params measuredArmParams(){ std::vector jointHandles; - + jointHandles.push_back("LBR_iiwa_14_R820_joint1#0"); // Joint1Handle, jointHandles.push_back("LBR_iiwa_14_R820_joint2#0"); // Joint2Handle, jointHandles.push_back("LBR_iiwa_14_R820_joint3#0"); // Joint3Handle, @@ -167,7 +167,7 @@ void construct(Params params){ // keep driver threads from exiting immediately after creation, because they have work to do! device_driver_workP_.reset(new boost::asio::io_service::work(device_driver_io_service)); kukaDriverP_=std::make_shared(std::make_tuple( - + std::get(params), std::get(params), std::get(params), @@ -180,7 +180,7 @@ void construct(Params params){ std::get(params), std::get(params) - + )); kukaDriverP_->construct(); // Default to joint servo mode for commanding motion @@ -189,6 +189,36 @@ void construct(Params params){ initHandles(); } +/// start recording the kuka state data in memory +/// return true on success, false on failure +bool start_recording() +{ + if(kukaDriverP_.get() != nullptr) { + return kukaDriverP_->start_recording(); + } + return false; +} +/// stop recording the kuka state data in memory +/// return true on success, false on failure +bool stop_recording() +{ + if(kukaDriverP_.get() != nullptr) { + return kukaDriverP_->stop_recording(); + } + return false; + +} +bool save_recording(std::string filename = std::string()) { + if(kukaDriverP_.get() != nullptr) { + return kukaDriverP_->save_recording(filename); + } + return false; +} +void clear_recording() +{ + kukaDriverP_->clear_recording(); +} + void run_one(){ @@ -203,7 +233,7 @@ void run_one(){ ~KukaVrepPlugin(){ device_driver_workP_.reset(); - + if(driver_threadP){ device_driver_io_service.stop(); driver_threadP->join(); @@ -215,7 +245,7 @@ void run_one(){ void initHandles() { - + vrepRobotArmDriverP_ = std::make_shared ( std::make_tuple( @@ -227,10 +257,10 @@ void initHandles() { std::get (params_) ) ); - - + + vrepRobotArmDriverP_->construct(); - + measuredParams_ = measuredArmParams(); vrepMeasuredRobotArmDriverP_ = std::make_shared ( @@ -244,8 +274,8 @@ void initHandles() { ) ); vrepMeasuredRobotArmDriverP_->construct(); - - + + /// @todo remove this assumption allHandlesSet = true; } @@ -259,30 +289,30 @@ void getRealKukaAngles() { } void syncVrepAndKuka(){ - + if(!allHandlesSet || !m_haveReceivedRealData) return; - + /// @todo make this handled by template driver implementations/extensions kukaDriverP_->set(simulationTimeStep_,time_duration_command_tag()); kukaDriverP_->set( simJointPosition, grl::revolute_joint_angle_open_chain_command_tag()); if(0) kukaDriverP_->set( simJointForce , grl::revolute_joint_torque_open_chain_command_tag()); - + kukaDriverP_->run_one(); // We have the real kuka state read from the device now // update real joint angle data realJointPosition.clear(); kukaDriverP_->get(std::back_inserter(realJointPosition), grl::revolute_joint_angle_open_chain_state_tag()); - + realJointForce.clear(); kukaDriverP_->get(std::back_inserter(realJointForce), grl::revolute_joint_torque_open_chain_state_tag()); - + realExternalJointTorque.clear(); kukaDriverP_->get(std::back_inserter(realExternalJointTorque), grl::revolute_joint_torque_external_open_chain_state_tag()); - + realExternalForce.clear(); kukaDriverP_->get(std::back_inserter(realExternalForce), grl::cartesian_external_force_tag()); - + if(0){ // debug output std::cout << "Measured Torque: "; @@ -291,32 +321,32 @@ void syncVrepAndKuka(){ std::cout << t << " "; } std::cout << '\n'; - + std::cout << "External Torque: "; std::cout << std::setw(6); for (float t:realExternalJointTorque) { std::cout << t << " "; } std::cout << '\n'; - + std::cout << "Measured Position: "; for (float t:realJointPosition) { std::cout << t << " "; } std::cout << '\n'; } - + if(!allHandlesSet || !vrepMeasuredRobotArmDriverP_) return; - + if (realJointPosition.size() > 0) { // if there are valid measured states VrepRobotArmDriver::State measuredArmState; std::get(measuredArmState) = realJointPosition; std::get(measuredArmState) = realJointForce; std::get(measuredArmState) = realExternalJointTorque; - + vrepMeasuredRobotArmDriverP_->setState(measuredArmState); } - + } /// @todo if there aren't real limits set via the kuka model already then implement me @@ -329,42 +359,42 @@ void setArmLimits(){ /// @return isError - true if there is an error, false if there is no Error bool getStateFromVrep(){ if(!allHandlesSet || !vrepRobotArmDriverP_) return false; - + VrepRobotArmDriver::State armState; - + bool isError = vrepRobotArmDriverP_->getState(armState); - + if(isError) return isError; - + simJointPosition = std::get (armState); simJointForce = std::get (armState); simJointTargetPosition = std::get(armState); simJointTransformationMatrix = std::get (armState); - - + + /// need to provide tick time in double seconds and get from vrep API call simulationTimeStep_ = simGetSimulationTimeStep()*1000; - + // for (int i=0 ; i < KUKA::LBRState::NUM_DOF ; i++) -// { +// { // simGetJointPosition(jointHandle[i],&simJointPosition[i]); //retrieves the intrinsic position of a joint (Angle for revolute joint) -// simGetJointForce(jointHandle[i],&simJointForce[i]); //retrieves the force or torque applied to a joint along/about its active axis. This function retrieves meaningful information only if the joint is prismatic or revolute, and is dynamically enabled. +// simGetJointForce(jointHandle[i],&simJointForce[i]); //retrieves the force or torque applied to a joint along/about its active axis. This function retrieves meaningful information only if the joint is prismatic or revolute, and is dynamically enabled. // simGetJointTargetPosition(jointHandle[i],&simJointTargetPosition[i]); //retrieves the target position of a joint // simGetJointMatrix(jointHandle[i],&simJointTransformationMatrix[i]); //retrieves the intrinsic transformation matrix of a joint (the transformation caused by the joint movement) // // } -// +// // BOOST_LOG_TRIVIAL(info) << "simJointPostition = " << simJointPosition << std::endl; // BOOST_LOG_TRIVIAL(info) << "simJointForce = " << simJointForce << std::endl; // BOOST_LOG_TRIVIAL(info) << "simJointTargetPostition = " << simJointTargetPosition << std::endl; // BOOST_LOG_TRIVIAL(info) << "simJointTransformationMatrix = " << simJointTransformationMatrix << std::endl; -// +// // float simTipPosition[3]; // float simTipOrientation[3]; // // simGetObjectPosition(target, targetBase, simTipPosition); // simGetObjectOrientation(target, targetBase, simTipOrientation); -// +// // for (int i = 0 ; i < 3 ; i++) // { // BOOST_LOG_TRIVIAL(info) << "simTipPosition[" << i << "] = " << simTipPosition[i] << std::endl; @@ -387,7 +417,7 @@ bool updateVrepFromKuka() { // then use the functions below to set the simulation state // to match - /////////////////// assuming given real joint position (angles), forces, target position and target velocity + /////////////////// assuming given real joint position (angles), forces, target position and target velocity // setting the simulation variables to data from real robot (here they have been assumed given) @@ -395,13 +425,13 @@ bool updateVrepFromKuka() { for (int i=0 ; i < 7 ; i++) { //simSetJointPosition(jointHandle[i],realJointPosition[i]); //Sets the intrinsic position of a joint. May have no effect depending on the joint mode - - + + //simSetJointTargetPosition(jointHandle[i],realJointTargetPosition[i]); //Sets the target position of a joint if the joint is in torque/force mode (also make sure that the joint's motor and position control are enabled - + //simSetJointForce(jointHandle[i],realJointForce[i]); //Sets the maximum force or torque that a joint can exert. This function has no effect when the joint is not dynamically enabled - - + + //simSetJointTargetVelocity(jointHandle[i],realJointTargetVelocity[i]); //Sets the intrinsic target velocity of a non-spherical joint. This command makes only sense when the joint mode is: (a) motion mode: the joint's motion handling feature must be enabled (simHandleJoint must be called (is called by default in the main script), and the joint motion properties must be set in the joint settings dialog), (b) torque/force mode: the dynamics functionality and the joint motor have to be enabled (position control should however be disabled) } return false; @@ -444,7 +474,7 @@ std::shared_ptr friData_; std::shared_ptr logger_; }; - + }} // grl::vrep #endif diff --git a/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp b/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp index fd45653..95cba7e 100755 --- a/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp +++ b/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp @@ -159,6 +159,92 @@ void LUA_SIM_EXT_KUKA_LBR_IIWA_START(SLuaCallBack* p) } +///////////////////////////////////////////////////////////////////////////////////////// +/// LUA function to actually start recording the fusiontrack frame data in memory. +///////////////////////////////////////////////////////////////////////////////////////// + +// simExtKUKAiiwaStartRecording +void LUA_SIM_EXT_KUKA_IIWA_STATE_START_RECORDING(SLuaCallBack *p) +{ + CLuaFunctionData D; + bool success = false; + if (kukaPluginPG) + { + std::string log_message("Starting the recording of KUKAiiwa state data in memory.\n"); + simAddStatusbarMessage(log_message.c_str()); + loggerPG->info(log_message); + success = kukaPluginPG->start_recording(); + } + D.pushOutData(CLuaFunctionDataItem(success)); + D.writeDataToLua(p); +} +// simExtKUKAiiwaStopRecording +void LUA_SIM_EXT_KUKA_IIWA_STATE_STOP_RECORDING(SLuaCallBack *p) +{ + CLuaFunctionData D; + bool success = false; + if (kukaPluginPG) + { + std::string log_message("Stoping the recording of KUKAiiwa state data in memory.\n"); + simAddStatusbarMessage(log_message.c_str()); + loggerPG->info(log_message); + success = kukaPluginPG->stop_recording(); + } + D.pushOutData(CLuaFunctionDataItem(success)); + D.writeDataToLua(p); +} + +///////////////////////////////////////////////////////////////////////////// +// Save the currently recorded fusiontrack frame data, this also clears the recording. +///////////////////////////////////////////////////////////////////////////// +// simExtKUKAiiwaStartRecording +#define LUA_SIM_EXT_KUKA_IIWA_STATE_SAVE_RECORDING_COMMAND "simExtKUKAiiwaStateSaveRecording" + +const int inArgs_LUA_SIM_EXT_KUKA_IIWA_STATE_SAVE_RECORDING[] = { + 1, + sim_lua_arg_string, 0 // string file name +}; + +std::string LUA_SIM_EXT_KUKA_IIWA_STATE_SAVE_RECORDING_CALL_TIP("number result=simExtKUKAiiwaStateSaveRecording(string filename)"); + +void LUA_SIM_EXT_KUKA_IIWA_STATE_SAVE_RECORDING(SLuaCallBack *p) +{ + CLuaFunctionData D; + bool success = false; + if (D.readDataFromLua(p, inArgs_LUA_SIM_EXT_KUKA_IIWA_STATE_SAVE_RECORDING, inArgs_LUA_SIM_EXT_KUKA_IIWA_STATE_SAVE_RECORDING[0], LUA_SIM_EXT_KUKA_IIWA_STATE_SAVE_RECORDING_COMMAND)) + { + std::vector *inData = D.getInDataPtr(); + std::string filename_lua_param(inData->at(0).stringData[0]); + if (kukaPluginPG) + { + std::string log_message("Saving the recording of KUKAiiwa state data in memory.\n"); + simAddStatusbarMessage(log_message.c_str()); + loggerPG->info(log_message); + success = kukaPluginPG->save_recording(); + } + D.pushOutData(CLuaFunctionDataItem(success)); + D.writeDataToLua(p); + } + + +} + +// simExtKUKAiiwaStartRecording +void LUA_SIM_EXT_KUKA_IIWA_STATE_CLEAR_RECORDING(SLuaCallBack *p) +{ + CLuaFunctionData D; + bool success = false; + if (kukaPluginPG) + { + std::string log_message("Clearing the recording of KUKAiiwa state data in memory.\n"); + simAddStatusbarMessage(log_message.c_str()); + loggerPG->info(log_message); + kukaPluginPG->clear_recording(); + success = true; + } + D.pushOutData(CLuaFunctionDataItem(success)); + D.writeDataToLua(p); +} // This is the plugin start routine (called just once, just after the plugin was loaded): VREP_DLLEXPORT unsigned char v_repStart(void* reservedPointer,int reservedInt) { @@ -225,14 +311,17 @@ VREP_DLLEXPORT unsigned char v_repStart(void* reservedPointer,int reservedInt) LUA_SIM_EXT_KUKA_LBR_IIWA_START ); - - - // Expected input arguments are: int sensorIndex, float floatParameters[3], int intParameters[2] - //int inArgs_getSensorData[]={3,sim_lua_arg_int,sim_lua_arg_float|sim_lua_arg_table,sim_lua_arg_int|sim_lua_arg_table}; // this says we expect 3 arguments (1 integer, a table of floats, and a table of ints) - // Return value can change on the fly, so no need to specify them here, except for the calltip. - // Now register the callback: - //simRegisterCustomLuaFunction(LUA_GET_SENSOR_DATA_COMMAND,strConCat("number result,table data,number distance=",LUA_GET_SENSOR_DATA_COMMAND,"(number sensorIndex,table_3 floatParams,table_2 intParams)"),inArgs_getSensorData,LUA_GET_SENSOR_DATA_CALLBACK); - // ****************************************** + int inArgs1[] = {0}; // no input arguments + /// Register functions to control the recording procedure of the fusiontrack frame data in memory + simRegisterCustomLuaFunction("simExtKUKAiiwaStateStartRecording", "number result=simExtKUKAiiwaStateStartRecording()", inArgs1, LUA_SIM_EXT_KUKA_IIWA_STATE_START_RECORDING); + simRegisterCustomLuaFunction("simExtKUKAiiwaStateStopRecording", "number result=simExtKUKAiiwaStateStopRecording()", inArgs1, LUA_SIM_EXT_KUKA_IIWA_STATE_STOP_RECORDING); + simRegisterCustomLuaFunction("simExtKUKAiiwaStateClearRecording", "number result=simExtKUKAiiwaStateClearRecording()", inArgs1, LUA_SIM_EXT_KUKA_IIWA_STATE_CLEAR_RECORDING); + + CLuaFunctionData::getInputDataForFunctionRegistration(inArgs_LUA_SIM_EXT_KUKA_IIWA_STATE_SAVE_RECORDING, inArgs); + simRegisterCustomLuaFunction(LUA_SIM_EXT_KUKA_IIWA_STATE_SAVE_RECORDING_COMMAND, + LUA_SIM_EXT_KUKA_IIWA_STATE_SAVE_RECORDING_CALL_TIP.c_str(), + &inArgs[0], + LUA_SIM_EXT_KUKA_IIWA_STATE_SAVE_RECORDING); loggerPG->error("KUKA LBR iiwa plugin initialized. Build date/time: ", __DATE__, " ", __TIME__ ); From 34b0dd9397cb3600528ceec6f3cb0400c8a67d95 Mon Sep 17 00:00:00 2001 From: Chunting Date: Wed, 17 Jan 2018 18:12:05 -0500 Subject: [PATCH 033/102] Add a method to log data in vrep plugin from starting the program --- include/grl/kuka/KukaDriver.hpp | 10 ++- include/grl/kuka/KukaFRIdriver.hpp | 5 +- include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp | 10 ++- src/lua/robone.lua | 4 +- .../v_repExtKukaLBRiiwa.cpp | 82 +++++++++++++++---- 5 files changed, 92 insertions(+), 19 deletions(-) diff --git a/include/grl/kuka/KukaDriver.hpp b/include/grl/kuka/KukaDriver.hpp index 7dcc70a..49d2e72 100644 --- a/include/grl/kuka/KukaDriver.hpp +++ b/include/grl/kuka/KukaDriver.hpp @@ -269,7 +269,15 @@ namespace grl { namespace robot { namespace arm { } void clear_recording() { - FRIdriverP_->clear_recording(); + if(FRIdriverP_.get() != nullptr) { + FRIdriverP_->clear_recording(); + } + } + bool is_recording() + { if(FRIdriverP_.get() != nullptr) { + return FRIdriverP_->is_recording(); + } + return false; } /// set the mode of the arm. Examples: Teach or MoveArmJointServo /// @see grl::flatbuffer::ArmState in ArmControlState_generated.h diff --git a/include/grl/kuka/KukaFRIdriver.hpp b/include/grl/kuka/KukaFRIdriver.hpp index 5ef5bb4..84ebcc1 100644 --- a/include/grl/kuka/KukaFRIdriver.hpp +++ b/include/grl/kuka/KukaFRIdriver.hpp @@ -518,7 +518,6 @@ class KukaFRIdriver : public std::enable_shared_from_this< /// return true on success, false on failure bool start_recording() { - m_isRecording = true; return m_isRecording; } @@ -530,6 +529,10 @@ class KukaFRIdriver : public std::enable_shared_from_this< return !m_isRecording; } + bool is_recording() + { + return m_isRecording; + } bool oneKUKAiiwaStateBuffer() diff --git a/include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp b/include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp index 3b9fbdc..4e732b7 100644 --- a/include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp +++ b/include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp @@ -216,9 +216,17 @@ bool save_recording(std::string filename = std::string()) { } void clear_recording() { - kukaDriverP_->clear_recording(); + if(kukaDriverP_.get() != nullptr) { + kukaDriverP_->clear_recording(); + } } +bool is_recording() +{ if(kukaDriverP_.get() != nullptr) { + return kukaDriverP_->is_recording(); + } + return false; +} void run_one(){ diff --git a/src/lua/robone.lua b/src/lua/robone.lua index 24f550a..bbdd592 100644 --- a/src/lua/robone.lua +++ b/src/lua/robone.lua @@ -433,6 +433,8 @@ robone.startRealArmDriverScript=function() 'FRI' , -- KukaMonitorMode (options are FRI, JAVA) "IK_Group1_iiwa" -- IKGroupName ) + + simExtKukaLBRiiwaRecordWhileSimulationIsRunning(false) else simDisplayDialog('Error','KukaLBRiiwa plugin was not found. (v_repExtKukaLBRiiwa.dll)&&nSimulation will run without hardware',sim_dlgstyle_ok,true,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1}) end @@ -470,7 +472,7 @@ robone.configureOpticalTracker=function() -------------------------------------------------- -- Move the Tracker -- true enables moving the tracker, false disables it - moveTracker = false + moveTracker = true if (moveTracker) then simAddStatusbarMessage('robone.configureOpticalTracker() + v_repExtAtracsysFusionTrackVrepPlugin: Moving Optical tracker position relative to marker on robot end effector.') simExtAtracsysFusionTrackClearObjects() diff --git a/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp b/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp index 95cba7e..ed4912b 100755 --- a/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp +++ b/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp @@ -45,6 +45,8 @@ LIBRARY vrepLib; // the V-REP library that we will dynamically load and bind std::shared_ptr kukaPluginPG; std::shared_ptr loggerPG; +/// Recording will begin when the simulation starts running, and log files will be saved every time it stops running. +bool recordWhileSimulationIsRunningG = false; const int inArgs_KUKA_LBR_IIWA_START[]={ 16, // Example Value // Parameter name @@ -164,7 +166,7 @@ void LUA_SIM_EXT_KUKA_LBR_IIWA_START(SLuaCallBack* p) ///////////////////////////////////////////////////////////////////////////////////////// // simExtKUKAiiwaStartRecording -void LUA_SIM_EXT_KUKA_IIWA_STATE_START_RECORDING(SLuaCallBack *p) +void LUA_SIM_EXT_KUKA_LBR_IIWA_START_RECORDING(SLuaCallBack *p) { CLuaFunctionData D; bool success = false; @@ -179,7 +181,7 @@ void LUA_SIM_EXT_KUKA_IIWA_STATE_START_RECORDING(SLuaCallBack *p) D.writeDataToLua(p); } // simExtKUKAiiwaStopRecording -void LUA_SIM_EXT_KUKA_IIWA_STATE_STOP_RECORDING(SLuaCallBack *p) +void LUA_SIM_EXT_KUKA_LBR_IIWA_STOP_RECORDING(SLuaCallBack *p) { CLuaFunctionData D; bool success = false; @@ -198,20 +200,20 @@ void LUA_SIM_EXT_KUKA_IIWA_STATE_STOP_RECORDING(SLuaCallBack *p) // Save the currently recorded fusiontrack frame data, this also clears the recording. ///////////////////////////////////////////////////////////////////////////// // simExtKUKAiiwaStartRecording -#define LUA_SIM_EXT_KUKA_IIWA_STATE_SAVE_RECORDING_COMMAND "simExtKUKAiiwaStateSaveRecording" +#define LUA_SIM_EXT_KUKA_LBR_IIWA_SAVE_RECORDING_COMMAND "simExtKukaLBRiiwaSaveRecording" -const int inArgs_LUA_SIM_EXT_KUKA_IIWA_STATE_SAVE_RECORDING[] = { +const int inArgs_LUA_SIM_EXT_KUKA_LBR_IIWA_SAVE_RECORDING[] = { 1, sim_lua_arg_string, 0 // string file name }; -std::string LUA_SIM_EXT_KUKA_IIWA_STATE_SAVE_RECORDING_CALL_TIP("number result=simExtKUKAiiwaStateSaveRecording(string filename)"); +std::string LUA_SIM_EXT_KUKA_LBR_IIWA_SAVE_RECORDING_CALL_TIP("number result=simExtKukaLBRiiwaSaveRecording(string filename)"); -void LUA_SIM_EXT_KUKA_IIWA_STATE_SAVE_RECORDING(SLuaCallBack *p) +void LUA_SIM_EXT_KUKA_LBR_IIWA_SAVE_RECORDING(SLuaCallBack *p) { CLuaFunctionData D; bool success = false; - if (D.readDataFromLua(p, inArgs_LUA_SIM_EXT_KUKA_IIWA_STATE_SAVE_RECORDING, inArgs_LUA_SIM_EXT_KUKA_IIWA_STATE_SAVE_RECORDING[0], LUA_SIM_EXT_KUKA_IIWA_STATE_SAVE_RECORDING_COMMAND)) + if (D.readDataFromLua(p, inArgs_LUA_SIM_EXT_KUKA_LBR_IIWA_SAVE_RECORDING, inArgs_LUA_SIM_EXT_KUKA_LBR_IIWA_SAVE_RECORDING[0], LUA_SIM_EXT_KUKA_LBR_IIWA_SAVE_RECORDING_COMMAND)) { std::vector *inData = D.getInDataPtr(); std::string filename_lua_param(inData->at(0).stringData[0]); @@ -230,7 +232,7 @@ void LUA_SIM_EXT_KUKA_IIWA_STATE_SAVE_RECORDING(SLuaCallBack *p) } // simExtKUKAiiwaStartRecording -void LUA_SIM_EXT_KUKA_IIWA_STATE_CLEAR_RECORDING(SLuaCallBack *p) +void LUA_SIM_EXT_KUKA_LBR_IIWA_CLEAR_RECORDING(SLuaCallBack *p) { CLuaFunctionData D; bool success = false; @@ -245,6 +247,42 @@ void LUA_SIM_EXT_KUKA_IIWA_STATE_CLEAR_RECORDING(SLuaCallBack *p) D.pushOutData(CLuaFunctionDataItem(success)); D.writeDataToLua(p); } + +///////////////////////////////////////////////////////////////////////////// +// Set recordWhileSimulationIsRunningG +///////////////////////////////////////////////////////////////////////////// + +#define LUA_SIM_EXT_KUKA_LBR_IIWA_RECORD_WHILE_SIMULATION_IS_RUNNING_COMMAND "simExtKukaLBRiiwaRecordWhileSimulationIsRunning" + +const int inArgs_LUA_SIM_EXT_KUKA_LBR_IIWA_RECORD_WHILE_SIMULATION_IS_RUNNING[] = { + 1, + sim_lua_arg_bool, 1 // string file name +}; + +std::string LUA_SIM_EXT_KUKA_LBR_IIWA_RECORD_WHILE_SIMULATION_IS_RUNNING_CALL_TIP("number result=simExtKukaLBRiiwaRecordWhileSimulationIsRunning(bool recording)"); + +void LUA_SIM_EXT_KUKA_LBR_IIWA_RECORD_WHILE_SIMULATION_IS_RUNNING(SLuaCallBack *p) +{ + + CLuaFunctionData D; + bool success = false; + if (D.readDataFromLua(p, + inArgs_LUA_SIM_EXT_KUKA_LBR_IIWA_RECORD_WHILE_SIMULATION_IS_RUNNING, + inArgs_LUA_SIM_EXT_KUKA_LBR_IIWA_RECORD_WHILE_SIMULATION_IS_RUNNING[0], + LUA_SIM_EXT_KUKA_LBR_IIWA_RECORD_WHILE_SIMULATION_IS_RUNNING_COMMAND)) + { + std::vector *inData = D.getInDataPtr(); + recordWhileSimulationIsRunningG = inData->at(0).boolData[0]; + + std::string log_message = std::string("simExtKukaLBRiiwaRecordWhileSimulationIsRunning: ") + boost::lexical_cast(recordWhileSimulationIsRunningG); + simAddStatusbarMessage(log_message.c_str()); + loggerPG->info(log_message); + + D.pushOutData(CLuaFunctionDataItem(success)); + D.writeDataToLua(p); + } +} + // This is the plugin start routine (called just once, just after the plugin was loaded): VREP_DLLEXPORT unsigned char v_repStart(void* reservedPointer,int reservedInt) { @@ -313,15 +351,21 @@ VREP_DLLEXPORT unsigned char v_repStart(void* reservedPointer,int reservedInt) int inArgs1[] = {0}; // no input arguments /// Register functions to control the recording procedure of the fusiontrack frame data in memory - simRegisterCustomLuaFunction("simExtKUKAiiwaStateStartRecording", "number result=simExtKUKAiiwaStateStartRecording()", inArgs1, LUA_SIM_EXT_KUKA_IIWA_STATE_START_RECORDING); - simRegisterCustomLuaFunction("simExtKUKAiiwaStateStopRecording", "number result=simExtKUKAiiwaStateStopRecording()", inArgs1, LUA_SIM_EXT_KUKA_IIWA_STATE_STOP_RECORDING); - simRegisterCustomLuaFunction("simExtKUKAiiwaStateClearRecording", "number result=simExtKUKAiiwaStateClearRecording()", inArgs1, LUA_SIM_EXT_KUKA_IIWA_STATE_CLEAR_RECORDING); + simRegisterCustomLuaFunction("simExtKukaLBRiiwaStartRecording", "number result=simExtKukaLBRiiwaStartRecording()", inArgs1, LUA_SIM_EXT_KUKA_LBR_IIWA_START_RECORDING); + simRegisterCustomLuaFunction("simExtKukaLBRiiwaStopRecording", "number result=simExtKukaLBRiiwaStopRecording()", inArgs1, LUA_SIM_EXT_KUKA_LBR_IIWA_STOP_RECORDING); + simRegisterCustomLuaFunction("simExtKukaLBRiiwaClearRecording", "number result=simExtKukaLBRiiwaClearRecording()", inArgs1, LUA_SIM_EXT_KUKA_LBR_IIWA_CLEAR_RECORDING); - CLuaFunctionData::getInputDataForFunctionRegistration(inArgs_LUA_SIM_EXT_KUKA_IIWA_STATE_SAVE_RECORDING, inArgs); - simRegisterCustomLuaFunction(LUA_SIM_EXT_KUKA_IIWA_STATE_SAVE_RECORDING_COMMAND, - LUA_SIM_EXT_KUKA_IIWA_STATE_SAVE_RECORDING_CALL_TIP.c_str(), + CLuaFunctionData::getInputDataForFunctionRegistration(inArgs_LUA_SIM_EXT_KUKA_LBR_IIWA_SAVE_RECORDING, inArgs); + simRegisterCustomLuaFunction(LUA_SIM_EXT_KUKA_LBR_IIWA_SAVE_RECORDING_COMMAND, + LUA_SIM_EXT_KUKA_LBR_IIWA_SAVE_RECORDING_CALL_TIP.c_str(), &inArgs[0], - LUA_SIM_EXT_KUKA_IIWA_STATE_SAVE_RECORDING); + LUA_SIM_EXT_KUKA_LBR_IIWA_SAVE_RECORDING); + + CLuaFunctionData::getInputDataForFunctionRegistration(inArgs_LUA_SIM_EXT_KUKA_LBR_IIWA_RECORD_WHILE_SIMULATION_IS_RUNNING, inArgs); + simRegisterCustomLuaFunction(LUA_SIM_EXT_KUKA_LBR_IIWA_RECORD_WHILE_SIMULATION_IS_RUNNING_COMMAND, + LUA_SIM_EXT_KUKA_LBR_IIWA_RECORD_WHILE_SIMULATION_IS_RUNNING_CALL_TIP.c_str(), + &inArgs[0], + LUA_SIM_EXT_KUKA_LBR_IIWA_RECORD_WHILE_SIMULATION_IS_RUNNING); loggerPG->error("KUKA LBR iiwa plugin initialized. Build date/time: ", __DATE__, " ", __TIME__ ); @@ -460,6 +504,9 @@ VREP_DLLEXPORT void* v_repMessage(int message,int* auxiliaryData,void* customDat // simAddStatusbarMessage( initerr.c_str()); // loggerPG->error( initerr ); // } + if(kukaPluginPG && recordWhileSimulationIsRunningG) { + kukaPluginPG->start_recording(); + } } if (message==sim_message_eventcallback_simulationended) @@ -470,6 +517,11 @@ VREP_DLLEXPORT void* v_repMessage(int message,int* auxiliaryData,void* customDat // close out as necessary //////////////////// loggerPG->error("Ending KUKA LBR iiwa plugin connection to Kuka iiwa\n" ); + if(kukaPluginPG && recordWhileSimulationIsRunningG && kukaPluginPG->is_recording()) { + kukaPluginPG->save_recording(); + kukaPluginPG->stop_recording(); + } + kukaPluginPG->clear_recording(); kukaPluginPG.reset(); } From f7112ac224e637cb22de1458ccf98723d96d1af2 Mon Sep 17 00:00:00 2001 From: Chunting Date: Fri, 19 Jan 2018 17:58:02 -0500 Subject: [PATCH 034/102] Clean some codes and add some comments about socket programming --- include/grl/kuka/KukaDriver.hpp | 215 +++--- include/grl/kuka/KukaFRIdriver.hpp | 108 +-- include/grl/kuka/KukaJAVAdriver.hpp | 695 +++++++++--------- include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp | 595 +++++++-------- include/grl/vrep/VrepRobotArmDriver.hpp | 98 ++- .../include/cartographer/common/time.h | 21 +- .../v_repExtKukaLBRiiwa.cpp | 79 +- test/KukaLBRiiwaVrepPluginTest.cpp | 4 +- test/fusionTrackTest.cpp | 6 +- 9 files changed, 858 insertions(+), 963 deletions(-) diff --git a/include/grl/kuka/KukaDriver.hpp b/include/grl/kuka/KukaDriver.hpp index 49d2e72..461cb77 100644 --- a/include/grl/kuka/KukaDriver.hpp +++ b/include/grl/kuka/KukaDriver.hpp @@ -29,8 +29,6 @@ namespace grl { namespace robot { namespace arm { - - /// /// /// @brief Kuka LBR iiwa Primary Multi Mode Driver, supports communication over FRI and JAVA interfaces @@ -71,7 +69,6 @@ namespace grl { namespace robot { namespace arm { std::string > Params; - static const Params defaultParams(){ return std::make_tuple( "Robotiiwa" , // RobotName, @@ -88,81 +85,74 @@ namespace grl { namespace robot { namespace arm { ); } + KukaDriver(Params params = defaultParams()) + : params_(params), debug(false) + {} - KukaDriver(Params params = defaultParams()) - : params_(params), debug(false) - {} + void construct(){ construct(params_);} - void construct(){ construct(params_);} + bool destruct(){ return JAVAdriverP_->destruct(); } - bool destruct(){ return JAVAdriverP_->destruct(); } + /// @todo create a function that calls simGetObjectHandle and throws an exception when it fails + /// @warning getting the ik group is optional, so it does not throw an exception + void construct(Params params ) { + std::cout<< "Start KukaDriver->construct(), initialize params for both FRI and JAVA driver..." << std::endl; - /// @todo create a function that calls simGetObjectHandle and throws an exception when it fails - /// @warning getting the ik group is optional, so it does not throw an exception - void construct(Params params ) { + params_ = params; + // keep driver threads from exiting immediately after creation, because they have work to do! + //device_driver_workP_.reset(new boost::asio::io_service::work(device_driver_io_service)); - params_ = params; - // keep driver threads from exiting immediately after creation, because they have work to do! - //device_driver_workP_.reset(new boost::asio::io_service::work(device_driver_io_service)); - - /// @todo figure out how to re-enable when .so isn't loaded - if( boost::iequals(std::get(params_),std::string("FRI")) - || boost::iequals(std::get(params_),std::string("FRI"))) - { - FRIdriverP_.reset( - new grl::robot::arm::KukaFRIdriver( - //device_driver_io_service, - std::make_tuple( - std::string(std::get (params)), - std::string(std::get (params)), - std::string(std::get (params)), - std::string(std::get (params)), - std::string(std::get (params)), - grl::robot::arm::KukaFRIClientDataDriver::run_automatically - ) - ) - - ); - FRIdriverP_->construct(); - } + /// @todo figure out how to re-enable when .so isn't loaded + if( boost::iequals(std::get(params_),std::string("FRI")) + || boost::iequals(std::get(params_),std::string("FRI"))) + { + FRIdriverP_.reset( + new grl::robot::arm::KukaFRIdriver( + //device_driver_io_service, + std::make_tuple( + std::string(std::get (params)), + std::string(std::get (params)), + std::string(std::get (params)), + std::string(std::get (params)), + std::string(std::get (params)), + grl::robot::arm::KukaFRIClientDataDriver::run_automatically + ) + ) + ); + FRIdriverP_->construct(); + } - /// @todo implement reading configuration in both FRI and JAVA mode from JAVA interface - if( boost::iequals(std::get(params_),std::string("JAVA")) - || boost::iequals(std::get(params_),std::string("FRI"))) - { - try { - JAVAdriverP_ = boost::make_shared(params_); - JAVAdriverP_->construct(); - - // start up the driver thread - /// @todo perhaps allow user to control this? - //driver_threadP.reset(new std::thread([&]{ device_driver_io_service.run(); })); - } catch( boost::exception &e) { - e << errmsg_info("KukaDriver: Unable to connect to ZeroMQ Socket from " + - std::get (params_) + " to " + - std::get (params_)); - throw; + /// @todo implement reading configuration in both FRI and JAVA mode from JAVA interface + if( boost::iequals(std::get(params_),std::string("JAVA")) + || boost::iequals(std::get(params_),std::string("FRI"))) + { + try { + JAVAdriverP_ = boost::make_shared(params_); + JAVAdriverP_->construct(); + + // start up the driver thread + /// @todo perhaps allow user to control this? + //driver_threadP.reset(new std::thread([&]{ device_driver_io_service.run(); })); + } catch( boost::exception &e) { + e << errmsg_info("KukaDriver: Unable to connect to ZeroMQ Socket from " + + std::get (params_) + " to " + + std::get (params_)); + throw; + } + } } + const Params & getParams(){ + return params_; } - } - - - - - - - const Params & getParams(){ - return params_; - } - ~KukaDriver(){ - device_driver_workP_.reset(); + ~KukaDriver(){ + device_driver_workP_.reset(); if(driver_threadP){ - device_driver_io_service.stop(); - driver_threadP->join(); + device_driver_io_service.stop(); + driver_threadP->join(); } } @@ -175,7 +165,7 @@ namespace grl { namespace robot { namespace arm { // @todo CHECK FOR REAL DATA BEFORE SENDING COMMANDS //if(!m_haveReceivedRealDataCount) return; bool haveNewData = false; - + std::cout<< "Start KukaDriver->run_one()..." << std::endl; /// @todo make this handled by template driver implementations/extensions if(JAVAdriverP_.get() != nullptr) @@ -227,7 +217,8 @@ namespace grl { namespace robot { namespace arm { { if( boost::iequals(std::get(params_),std::string("FRI"))) { - FRIdriverP_->set(armState_.commandedPosition,revolute_joint_angle_open_chain_command_tag()); + FRIdriverP_->set(armState_.commandedPosition,revolute_joint_angle_open_chain_command_tag()); + std::cout << "armState_.commandedPosition in KukaDriver(FRI) run_one():" << armState_.commandedPosition << "\n"; } haveNewData = FRIdriverP_->run_one(); @@ -299,49 +290,49 @@ namespace grl { namespace robot { namespace arm { } } - bool setPositionControlMode() - { - if(JAVAdriverP_) + bool setPositionControlMode() { - JAVAdriverP_->setPositionControlMode(); - return true; + if(JAVAdriverP_) + { + JAVAdriverP_->setPositionControlMode(); + return true; + } + else + return false; } - else - return false; - } - bool setJointImpedanceMode(std::vector joint_stiffnes, std::vectorjoint_damping) - { - if(JAVAdriverP_) + bool setJointImpedanceMode(std::vector joint_stiffnes, std::vectorjoint_damping) { - JAVAdriverP_->setJointImpedanceMode(joint_stiffnes, joint_damping); - return true; + if(JAVAdriverP_) + { + JAVAdriverP_->setJointImpedanceMode(joint_stiffnes, joint_damping); + return true; + } + else + return false; } - else - return false; - } - - // TODO: define custom flatbuffer for Cartesion Quantities, currently flatbuffer::CartesianImpedenceControlMode - bool setCartesianImpedanceMode( - grl::flatbuffer::EulerPose cart_stiffness, grl::flatbuffer::EulerPose cart_damping, - double nullspace_stiffness, double nullspace_damping, - grl::flatbuffer::EulerPose cart_max_path_deviation, - grl::flatbuffer::EulerPose cart_max_ctrl_vel, - grl::flatbuffer::EulerPose cart_max_ctrl_force, - bool max_control_force_stop) - { - if(JAVAdriverP_) + + // TODO: define custom flatbuffer for Cartesion Quantities, currently flatbuffer::CartesianImpedenceControlMode + bool setCartesianImpedanceMode( + grl::flatbuffer::EulerPose cart_stiffness, grl::flatbuffer::EulerPose cart_damping, + double nullspace_stiffness, double nullspace_damping, + grl::flatbuffer::EulerPose cart_max_path_deviation, + grl::flatbuffer::EulerPose cart_max_ctrl_vel, + grl::flatbuffer::EulerPose cart_max_ctrl_force, + bool max_control_force_stop) { - JAVAdriverP_->setCartesianImpedanceMode(cart_stiffness, cart_damping, - nullspace_stiffness, nullspace_damping, - cart_max_path_deviation, cart_max_ctrl_vel, cart_max_ctrl_force, - max_control_force_stop); - return true; + if(JAVAdriverP_) + { + JAVAdriverP_->setCartesianImpedanceMode(cart_stiffness, cart_damping, + nullspace_stiffness, nullspace_damping, + cart_max_path_deviation, cart_max_ctrl_vel, cart_max_ctrl_force, + max_control_force_stop); + return true; + } + else + return false; } - else - return false; - } /** * \brief Set the joint positions for the current interpolation step. @@ -394,16 +385,16 @@ namespace grl { namespace robot { namespace arm { * */ void set(double duration_to_goal_command, time_duration_command_tag tag) { - boost::lock_guard lock(jt_mutex); - armState_.goal_position_command_time_duration = duration_to_goal_command; - if(FRIdriverP_) - { - FRIdriverP_->set(duration_to_goal_command,tag); - } - if(JAVAdriverP_) - { - JAVAdriverP_->set(duration_to_goal_command,tag); - } + boost::lock_guard lock(jt_mutex); + armState_.goal_position_command_time_duration = duration_to_goal_command; + if(FRIdriverP_) + { + FRIdriverP_->set(duration_to_goal_command,tag); + } + if(JAVAdriverP_) + { + JAVAdriverP_->set(duration_to_goal_command,tag); + } } diff --git a/include/grl/kuka/KukaFRIdriver.hpp b/include/grl/kuka/KukaFRIdriver.hpp index 84ebcc1..32da97e 100644 --- a/include/grl/kuka/KukaFRIdriver.hpp +++ b/include/grl/kuka/KukaFRIdriver.hpp @@ -84,6 +84,7 @@ class KukaFRIdriver : public std::enable_shared_from_this< /// @warning getting the ik group is optional, so it does not throw an /// exception void construct(Params params) { + std::cout<< "Start KukaFRIdriver->construct()..." << std::endl; params_ = params; // keep driver threads from exiting immediately after creation, because they @@ -110,6 +111,7 @@ class KukaFRIdriver : public std::enable_shared_from_this< #ifdef HAVE_spdlog loggerP = spdlog::stdout_logger_mt("logs/kukaiiwa_logger.txt"); #endif // HAVE_spdlog + std::cout<< "End KukaFRIdriver->construct()..." << std::endl; } const Params &getParams() { return params_; } @@ -118,8 +120,8 @@ class KukaFRIdriver : public std::enable_shared_from_this< device_driver_workP_.reset(); if (driver_threadP) { - device_driver_io_service.stop(); - driver_threadP->join(); + device_driver_io_service.stop(); + driver_threadP->join(); } } @@ -197,7 +199,7 @@ class KukaFRIdriver : public std::enable_shared_from_this< * */ bool run_one() { - + std::cout<< "Start KukaFRIdriver->run_one()..." << std::endl; // note: this one sends *and* receives the joint data! BOOST_VERIFY(kukaFRIClientDataDriverP_.get() != nullptr); /// @todo use runtime calculation of NUM_JOINTS instead of constant @@ -305,58 +307,6 @@ class KukaFRIdriver : public std::enable_shared_from_this< oneKUKAiiwaStateBuffer(); armState.time_event_stamp = time_event_stamp; - - // std::cout<<" armState.time_event_stamp -------------" << std::endl; - // for (int i = 0; i void set(Range &&range, grl::revolute_joint_angle_open_chain_command_tag) { - boost::lock_guard lock(jt_mutex); - armState.clearCommands(); - boost::copy(range, std::back_inserter(armState.commandedPosition)); - boost::copy(range, std::back_inserter(armState.commandedPosition_goal)); + boost::lock_guard lock(jt_mutex); + armState.clearCommands(); + boost::copy(range, std::back_inserter(armState.commandedPosition)); + boost::copy(range, std::back_inserter(armState.commandedPosition_goal)); } /** @@ -433,8 +383,8 @@ class KukaFRIdriver : public std::enable_shared_from_this< * */ void set(double duration_to_goal_command, time_duration_command_tag) { - boost::lock_guard lock(jt_mutex); - armState.goal_position_command_time_duration = duration_to_goal_command; + boost::lock_guard lock(jt_mutex); + armState.goal_position_command_time_duration = duration_to_goal_command; } /** @@ -451,10 +401,10 @@ class KukaFRIdriver : public std::enable_shared_from_this< // return armState.timestamp; // } - cartographer::common::Time get(time_point_tag) { - boost::lock_guard lock(jt_mutex); - return armState.time_event_stamp.device_time; - } + cartographer::common::Time get(time_point_tag) { + boost::lock_guard lock(jt_mutex); + return armState.time_event_stamp.device_time; + } /** * \brief Set the applied joint torques for the current interpolation step. @@ -471,9 +421,9 @@ class KukaFRIdriver : public std::enable_shared_from_this< */ template void set(Range &&range, grl::revolute_joint_torque_open_chain_command_tag) { - boost::lock_guard lock(jt_mutex); - armState.clearCommands(); - boost::copy(range, armState.commandedTorque); + boost::lock_guard lock(jt_mutex); + armState.clearCommands(); + boost::copy(range, armState.commandedTorque); } /** @@ -503,30 +453,30 @@ class KukaFRIdriver : public std::enable_shared_from_this< */ template void set(Range &&range, grl::cartesian_wrench_command_tag) { - boost::lock_guard lock(jt_mutex); - armState.clearCommands(); - std::copy(range, armState.commandedCartesianWrenchFeedForward); + boost::lock_guard lock(jt_mutex); + armState.clearCommands(); + std::copy(range, armState.commandedCartesianWrenchFeedForward); } /// @todo should this exist, is it a good design? is it written correctly? void get(KukaState &state) { - boost::lock_guard lock(jt_mutex); - state = armState; + boost::lock_guard lock(jt_mutex); + state = armState; } /// start recording the kuka state data in memory /// return true on success, false on failure bool start_recording() { - m_isRecording = true; - return m_isRecording; + m_isRecording = true; + return m_isRecording; } /// stop recording the kuka state data in memory /// return true on success, false on failure bool stop_recording() { - m_isRecording = false; - return !m_isRecording; + m_isRecording = false; + return !m_isRecording; } bool is_recording() @@ -537,8 +487,6 @@ class KukaFRIdriver : public std::enable_shared_from_this< bool oneKUKAiiwaStateBuffer() { - - std::string RobotName = std::string(std::get(params_)); std:: string destination = std::string(std::get(params_)); @@ -557,8 +505,6 @@ class KukaFRIdriver : public std::enable_shared_from_this< bool updateMinimumTrajectoryExecutionTime = false; double minimumTrajectoryExecutionTime = 4; - - //Cartesian Impedance Values // grl::flatbuffer::Vector3d cart_stiffness_trans_ = grl::flatbuffer::Vector3d(0,0,0); // grl::flatbuffer::EulerRotation cart_stifness_rot_ = grl::flatbuffer::EulerRotation(0,0,0,grl::flatbuffer::EulerOrder::xyz); diff --git a/include/grl/kuka/KukaJAVAdriver.hpp b/include/grl/kuka/KukaJAVAdriver.hpp index e2f6feb..3c63fef 100644 --- a/include/grl/kuka/KukaJAVAdriver.hpp +++ b/include/grl/kuka/KukaJAVAdriver.hpp @@ -68,408 +68,386 @@ namespace grl { namespace robot { namespace arm { * @todo make sure mutex is locked when appropriate * */ - class KukaJAVAdriver : public std::enable_shared_from_this { +class KukaJAVAdriver : public std::enable_shared_from_this { public: - enum ParamIndex { - RobotName, - RobotModel, - LocalUDPAddress, - LocalUDPPort, - RemoteUDPAddress, - LocalHostKukaKoniUDPAddress, - LocalHostKukaKoniUDPPort, - RemoteHostKukaKoniUDPAddress, - RemoteHostKukaKoniUDPPort, - KukaCommandMode, - KukaMonitorMode - }; - - typedef std::tuple< - std::string, - std::string, - std::string, - std::string, - std::string, - std::string, - std::string, - std::string, - std::string, - std::string, - std::string - > Params; - - - static const Params defaultParams(){ - return std::make_tuple( - "Robotiiwa" , // RobotName, - "KUKA_LBR_IIWA_14_R820" , // RobotModel (options are KUKA_LBR_IIWA_14_R820, KUKA_LBR_IIWA_7_R800) - "0.0.0.0" , // LocalUDPAddress - "30010" , // LocalUDPPort - "172.31.1.147" , // RemoteUDPAddress - "192.170.10.100" , // LocalHostKukaKoniUDPAddress, - "30200" , // LocalHostKukaKoniUDPPort, - "192.170.10.2" , // RemoteHostKukaKoniUDPAddress, - "30200" , // RemoteHostKukaKoniUDPPort - "JAVA" , // KukaCommandMode (options are FRI, JAVA) - "FRI" // KukaMonitorMode (options are FRI, JAVA) - ); - } - - - /// unique tag type so State never - /// conflicts with a similar tuple - struct JointStateTag{}; - - enum JointStateIndex { - JointPosition, - JointForce, - JointTargetPosition, - JointLowerPositionLimit, - JointUpperPositionLimit, - JointMatrix, - JointStateTagIndex - }; - - - typedef std::vector JointScalar; - - /// @see http://www.coppeliarobotics.com/helpFiles/en/apiFunctions.htm#simGetJointMatrix for data layout information - typedef std::array TransformationMatrix; - typedef std::vector TransformationMatrices; - - typedef std::tuple< - JointScalar, // jointPosition - // JointScalar // JointVelocity // no velocity yet - JointScalar, // jointForce - JointScalar, // jointTargetPosition - JointScalar, // JointLowerPositionLimit - JointScalar, // JointUpperPositionLimit - TransformationMatrices, // jointTransformation - JointStateTag // JointStateTag unique identifying type so tuple doesn't conflict - > State; - - - KukaJAVAdriver(Params params = defaultParams()) - : params_(params), armControlMode_(flatbuffer::ArmState::NONE) - {} - - void construct(){ construct(params_); sequenceNumber = 0; } - - /// @todo create a function that calls simGetObjectHandle and throws an exception when it fails - /// @warning getting the ik group is optional, so it does not throw an exception - void construct(Params params) { - try { logger_ = spdlog::stdout_logger_mt("console"); } catch (spdlog::spdlog_ex ex) { logger_ = spdlog::get("console"); } - params_ = params; - - - try { - logger_->info("KukaLBRiiwaRosPlugin: Connecting UDP Socket from ", - std::get (params_), ":", std::get (params_), " to ", - std::get (params_)); - - /// @todo TODO(ahundt) switch from linux socket to boost::asio::ip::udp::socket, see Kuka.hpp and KukaFRIdriver.hpp for examples, and make use of KukaUDP class. - socket_local = socket(AF_INET, SOCK_DGRAM, 0); - if (socket_local < 0) { - BOOST_THROW_EXCEPTION(std::runtime_error("KukaJAVAdriver Error opening socket. Check that the port is available, and that all the cables are connected tightly. If you have no other options try restarting your computer.")); - } - - port = boost::lexical_cast( std::get (params_)); - // convert the string to network presentation value - inet_pton(AF_INET, std::get(params_).c_str(), &(local_sockaddr.sin_addr)); - local_sockaddr.sin_family = AF_INET; - local_sockaddr.sin_port = htons(port); - // local_sockaddr.sin_addr.s_addr = INADDR_ANY; - - /// @todo TODO(ahundt) Consider switching to boost::asio synchronous calls (async has high latency)! - /// @todo TODO(ahundt) Need to switch back to an appropriate exception rather than exiting so VREP isn't taken down. - /// @todo TODO(ahundt) switch from linux socket to boost::asio::ip::udp::socket, see Kuka.hpp and KukaFRIdriver.hpp for examples, and make use of KukaUDP class. - if (bind(socket_local, (struct sockaddr *)&local_sockaddr, sizeof(local_sockaddr)) < 0) { - printf("Error binding sr_joint!\n"); - BOOST_THROW_EXCEPTION(std::runtime_error("KukaJAVAdriver Error opening socket. Check that the port is available, and that all the cables are connected tightly. If you have no other options try restarting your computer.")); - } - - FD_ZERO(&mask); - FD_ZERO(&dummy_mask); - FD_SET(socket_local, &mask); - - // set arm to StartArm mode on initalization - //set(grl::flatbuffer::ArmState::StartArm); - set(grl::flatbuffer::ArmState::MoveArmJointServo); - - } catch( boost::exception &e) { - e << errmsg_info("KukaLBRiiwaRosPlugin: Unable to connect to UDP Socket from {}{}{}" + - std::get (params_) + " to " + - std::get (params_)); - throw; + enum ParamIndex { + RobotName, + RobotModel, + LocalUDPAddress, + LocalUDPPort, + RemoteUDPAddress, + LocalHostKukaKoniUDPAddress, + LocalHostKukaKoniUDPPort, + RemoteHostKukaKoniUDPAddress, + RemoteHostKukaKoniUDPPort, + KukaCommandMode, + KukaMonitorMode + }; + + typedef std::tuple< + std::string, + std::string, + std::string, + std::string, + std::string, + std::string, + std::string, + std::string, + std::string, + std::string, + std::string + > Params; + + + static const Params defaultParams(){ + return std::make_tuple( + "Robotiiwa" , // RobotName, + "KUKA_LBR_IIWA_14_R820" , // RobotModel (options are KUKA_LBR_IIWA_14_R820, KUKA_LBR_IIWA_7_R800) + "0.0.0.0" , // LocalUDPAddress + "30010" , // LocalUDPPort + "172.31.1.147" , // RemoteUDPAddress + "192.170.10.100" , // LocalHostKukaKoniUDPAddress, + "30200" , // LocalHostKukaKoniUDPPort, + "192.170.10.2" , // RemoteHostKukaKoniUDPAddress, + "30200" , // RemoteHostKukaKoniUDPPort + "JAVA" , // KukaCommandMode (options are FRI, JAVA) + "FRI" // KukaMonitorMode (options are FRI, JAVA) + ); } - } - - - - - const Params & getParams(){ - return params_; - } - - /// shuts down the arm - bool destruct(){ - close(socket_local); - return true; - } - - ~KukaJAVAdriver(){ - /// @todo TODO(ahundt) switch to asio, remove destruct call from destructor - destruct(); - } - - /// @brief SEND COMMAND TO ARM. Call this often - /// Performs the main update spin once. - /// @todo ADD SUPPORT FOR READING ARM STATE OVER JAVA INTERFACE - bool run_one(){ - // @todo CHECK FOR REAL DATA BEFORE SENDING COMMANDS - //if(!m_haveReceivedRealDataCount) return; - - bool haveNewData = false; + /// unique tag type so State never + /// conflicts with a similar tuple + struct JointStateTag{}; + + enum JointStateIndex { + JointPosition, + JointForce, + JointTargetPosition, + JointLowerPositionLimit, + JointUpperPositionLimit, + JointMatrix, + JointStateTagIndex + }; + + typedef std::vector JointScalar; + + /// @see http://www.coppeliarobotics.com/helpFiles/en/apiFunctions.htm#simGetJointMatrix for data layout information + typedef std::array TransformationMatrix; + typedef std::vector TransformationMatrices; + + typedef std::tuple< + JointScalar, // jointPosition + // JointScalar // JointVelocity // no velocity yet + JointScalar, // jointForce + JointScalar, // jointTargetPosition + JointScalar, // JointLowerPositionLimit + JointScalar, // JointUpperPositionLimit + TransformationMatrices, // jointTransformation + JointStateTag // JointStateTag unique identifying type so tuple doesn't conflict + > State; + + + KukaJAVAdriver(Params params = defaultParams()) + : params_(params), armControlMode_(flatbuffer::ArmState::NONE) + {} + + void construct(){ construct(params_); sequenceNumber = 0; } + + /// @todo create a function that calls simGetObjectHandle and throws an exception when it fails + /// @warning getting the ik group is optional, so it does not throw an exception + void construct(Params params) { + try { logger_ = spdlog::stdout_logger_mt("console"); } catch (spdlog::spdlog_ex ex) { logger_ = spdlog::get("console"); } + params_ = params; + + try { + logger_->info("KukaLBRiiwaRosPlugin: Connecting UDP Socket from ", + std::get (params_), ":", std::get (params_), " to ", + std::get (params_)); + + /// @todo TODO(ahundt) switch from linux socket to boost::asio::ip::udp::socket, see Kuka.hpp and KukaFRIdriver.hpp for examples, and make use of KukaUDP class. + /// AF_INET (IPv4 protocol) , AF_INET6 (IPv6 protocol) + /// communication type: SOCK_STREAM: TCP(reliable, connection oriented), SOCK_DGRAM: UDP(unreliable, connectionless) + /// Protocol value for Internet Protocol(IP), which is 0. + /// socket_local is the socket descriptor, which is an integer. + socket_local = socket(AF_INET, SOCK_DGRAM, 0); + if (socket_local < 0) { + BOOST_THROW_EXCEPTION(std::runtime_error("KukaJAVAdriver Error opening socket. Check that the port is available, and that all the cables are connected tightly. If you have no other options try restarting your computer.")); + } + /// convert string to int + port = boost::lexical_cast( std::get (params_)); + /// convert the string to network presentation value + /// inet_pton() returns 1 on success. It returns -1 if there was an error (errno is set), or 0 if the input isn't a valid IP address. + inet_pton(AF_INET, std::get(params_).c_str(), &(local_sockaddr.sin_addr)); + local_sockaddr.sin_family = AF_INET; + /// htons: host to network short + local_sockaddr.sin_port = htons(port); + /// local_sockaddr.sin_addr.s_addr = INADDR_ANY; + + /// @todo TODO(ahundt) Consider switching to boost::asio synchronous calls (async has high latency)! + /// @todo TODO(ahundt) Need to switch back to an appropriate exception rather than exiting so VREP isn't taken down. + /// @todo TODO(ahundt) switch from linux socket to boost::asio::ip::udp::socket, see Kuka.hpp and KukaFRIdriver.hpp for examples, and make use of KukaUDP class. + /// After creation of the socket, bind function binds the socket to the address and port number specified in local_sockaddr(custom data structure). + if (bind(socket_local, (struct sockaddr *)&local_sockaddr, sizeof(local_sockaddr)) < 0) { + printf("Error binding sr_joint!\n"); + BOOST_THROW_EXCEPTION(std::runtime_error("KukaJAVAdriver Error opening socket. Check that the port is available, and that all the cables are connected tightly. If you have no other options try restarting your computer.")); + } + /// clear the socket set defined by fd_set + FD_ZERO(&mask); + FD_ZERO(&dummy_mask); + /// if valid socket descriptor then add to socket set + FD_SET(socket_local, &mask); + // set arm to StartArm mode on initalization + //set(grl::flatbuffer::ArmState::StartArm); + set(grl::flatbuffer::ArmState::MoveArmJointServo); + } catch( boost::exception &e) { + e << errmsg_info("KukaLBRiiwaRosPlugin: Unable to connect to UDP Socket from {}{}{}" + + std::get (params_) + " to " + + std::get (params_)); + throw; + } + } - /// @todo make this handled by template driver implementations/extensions + const Params & getParams(){ + return params_; + } + /// shuts down the arm + bool destruct(){ + close(socket_local); + return true; + } + ~KukaJAVAdriver(){ + /// @todo TODO(ahundt) switch to asio, remove destruct call from destructor + destruct(); + } + /// @brief SEND COMMAND TO ARM. Call this often + /// Performs the main update spin once. + /// @todo ADD SUPPORT FOR READING ARM STATE OVER JAVA INTERFACE + bool run_one(){ + // @todo CHECK FOR REAL DATA BEFORE SENDING COMMANDS + //if(!m_haveReceivedRealDataCount) return; + bool haveNewData = false; + /// @todo make this handled by template driver implementations/extensions - std::shared_ptr fbbP; - fbbP = std::make_shared(); + std::shared_ptr fbbP; + fbbP = std::make_shared(); boost::lock_guard lock(jt_mutex); - double duration = boost::chrono::high_resolution_clock::now().time_since_epoch().count(); - - /// @todo is this the best string to pass for the full arm's name? - auto basename = std::get(params_); - - auto bns = fbbP->CreateString(basename); - - flatbuffers::Offset controlState; - + double duration = boost::chrono::high_resolution_clock::now().time_since_epoch().count(); - switch (armControlMode_) { + /// @todo is this the best string to pass for the full arm's name? + auto basename = std::get(params_); - case flatbuffer::ArmState::StartArm: { - controlState = flatbuffer::CreateArmControlState(*fbbP,bns,sequenceNumber++,duration,armControlMode_,flatbuffer::CreateStartArm(*fbbP).Union()); - break; - } - case flatbuffer::ArmState::MoveArmJointServo: { + auto bns = fbbP->CreateString(basename); - /// @todo when new - auto armPositionBuffer = fbbP->CreateVector(armState_.commandedPosition_goal.data(),armState_.commandedPosition_goal.size()); - auto commandedTorque = fbbP->CreateVector(armState_.commandedTorque.data(),armState_.commandedTorque.size()); - auto goalJointState = grl::flatbuffer::CreateJointState(*fbbP,armPositionBuffer,0/*no velocity*/,0/*no acceleration*/,commandedTorque); - auto moveArmJointServo = grl::flatbuffer::CreateMoveArmJointServo(*fbbP,goalJointState); - controlState = flatbuffer::CreateArmControlState(*fbbP,bns,sequenceNumber++,duration,armControlMode_,moveArmJointServo.Union()); - logger_->info("C++ KukaJAVAdriver: sending armposition command: {}{}", armState_.commandedPosition_goal); - break; - } - case flatbuffer::ArmState::TeachArm: { - controlState = flatbuffer::CreateArmControlState(*fbbP,bns,sequenceNumber++,duration,armControlMode_,flatbuffer::CreateTeachArm(*fbbP).Union()); - break; - } - case flatbuffer::ArmState::PauseArm: { - controlState = flatbuffer::CreateArmControlState(*fbbP,bns,sequenceNumber++,duration,armControlMode_,flatbuffer::CreatePauseArm(*fbbP).Union()); - break; - } - case flatbuffer::ArmState::StopArm: { - controlState = flatbuffer::CreateArmControlState(*fbbP,bns,sequenceNumber++,duration,armControlMode_,flatbuffer::CreateStopArm(*fbbP).Union()); - break; - } - case flatbuffer::ArmState::ShutdownArm: { - controlState = flatbuffer::CreateArmControlState(*fbbP,bns,sequenceNumber++,duration,armControlMode_,flatbuffer::CreateStopArm(*fbbP).Union()); - break; - } - case flatbuffer::ArmState::NONE: { - //std::cerr << "Waiting for interation mode... (currently NONE)\n"; - break; - } - default: - logger_->error("C++ KukaJAVAdriver: unsupported use case: {}", EnumNameArmState(armControlMode_)); - } + flatbuffers::Offset controlState; - auto name = fbbP->CreateString(std::get(params_)); + switch (armControlMode_) { - auto clientCommandMode = grl::flatbuffer::EClientCommandMode::POSITION; - auto overlayType = grl::flatbuffer::EOverlayType::NO_OVERLAY; + case flatbuffer::ArmState::StartArm: { + controlState = flatbuffer::CreateArmControlState(*fbbP,bns,sequenceNumber++,duration,armControlMode_,flatbuffer::CreateStartArm(*fbbP).Union()); + break; + } + case flatbuffer::ArmState::MoveArmJointServo: { + + /// @todo when new + auto armPositionBuffer = fbbP->CreateVector(armState_.commandedPosition_goal.data(),armState_.commandedPosition_goal.size()); + auto commandedTorque = fbbP->CreateVector(armState_.commandedTorque.data(),armState_.commandedTorque.size()); + auto goalJointState = grl::flatbuffer::CreateJointState(*fbbP,armPositionBuffer,0/*no velocity*/,0/*no acceleration*/,commandedTorque); + auto moveArmJointServo = grl::flatbuffer::CreateMoveArmJointServo(*fbbP,goalJointState); + controlState = flatbuffer::CreateArmControlState(*fbbP,bns,sequenceNumber++,duration,armControlMode_,moveArmJointServo.Union()); + logger_->info("C++ KukaJAVAdriver: sending armposition command: {}{}", armState_.commandedPosition_goal); + break; + } + case flatbuffer::ArmState::TeachArm: { + controlState = flatbuffer::CreateArmControlState(*fbbP,bns,sequenceNumber++,duration,armControlMode_,flatbuffer::CreateTeachArm(*fbbP).Union()); + break; + } + case flatbuffer::ArmState::PauseArm: { + controlState = flatbuffer::CreateArmControlState(*fbbP,bns,sequenceNumber++,duration,armControlMode_,flatbuffer::CreatePauseArm(*fbbP).Union()); + break; + } + case flatbuffer::ArmState::StopArm: { + controlState = flatbuffer::CreateArmControlState(*fbbP,bns,sequenceNumber++,duration,armControlMode_,flatbuffer::CreateStopArm(*fbbP).Union()); + break; + } + case flatbuffer::ArmState::ShutdownArm: { + controlState = flatbuffer::CreateArmControlState(*fbbP,bns,sequenceNumber++,duration,armControlMode_,flatbuffer::CreateStopArm(*fbbP).Union()); + break; + } + case flatbuffer::ArmState::NONE: { + //std::cerr << "Waiting for interation mode... (currently NONE)\n"; + break; + } + default: + logger_->error("C++ KukaJAVAdriver: unsupported use case: {}", EnumNameArmState(armControlMode_)); + } - //auto stiffnessPose = flatbuffer::CreateEulerPoseParams(*fbbP,&cart_stiffness_trans_,&cart_stiffness_rot_); - //auto dampingPose = flatbuffer::CreateEulerPoseParams(*fbbP,&cart_damping_trans_,&cart_damping_rot_); + auto name = fbbP->CreateString(std::get(params_)); - auto setCartesianImpedance = grl::flatbuffer::CreateCartesianImpedenceControlMode(*fbbP, &cart_stiffness_, &cart_damping_, - nullspace_stiffness_, nullspace_damping_, &cart_max_path_deviation_, &cart_max_ctrl_vel_, &cart_max_ctrl_force_, max_control_force_stop_); + auto clientCommandMode = grl::flatbuffer::EClientCommandMode::POSITION; + auto overlayType = grl::flatbuffer::EOverlayType::NO_OVERLAY; - auto jointStiffnessBuffer = fbbP->CreateVector(joint_stiffness_.data(),joint_stiffness_.size()); - auto jointDampingBuffer = fbbP->CreateVector(joint_damping_.data(),joint_damping_.size()); + //auto stiffnessPose = flatbuffer::CreateEulerPoseParams(*fbbP,&cart_stiffness_trans_,&cart_stiffness_rot_); + //auto dampingPose = flatbuffer::CreateEulerPoseParams(*fbbP,&cart_damping_trans_,&cart_damping_rot_); - auto setJointImpedance = grl::flatbuffer::CreateJointImpedenceControlMode(*fbbP, jointStiffnessBuffer, jointDampingBuffer); + auto setCartesianImpedance = grl::flatbuffer::CreateCartesianImpedenceControlMode(*fbbP, &cart_stiffness_, &cart_damping_, + nullspace_stiffness_, nullspace_damping_, &cart_max_path_deviation_, &cart_max_ctrl_vel_, &cart_max_ctrl_force_, max_control_force_stop_); - auto kukaiiwaArmConfiguration = flatbuffer::CreateKUKAiiwaArmConfiguration(*fbbP,name,commandInterface_,monitorInterface_, clientCommandMode, overlayType, - controlMode_, setCartesianImpedance, setJointImpedance); + auto jointStiffnessBuffer = fbbP->CreateVector(joint_stiffness_.data(),joint_stiffness_.size()); + auto jointDampingBuffer = fbbP->CreateVector(joint_damping_.data(),joint_damping_.size()); - bool setArmControlState = true; // only actually change the arm state when this is true. + auto setJointImpedance = grl::flatbuffer::CreateJointImpedenceControlMode(*fbbP, jointStiffnessBuffer, jointDampingBuffer); - // TODO fill the 0s - auto kukaiiwastate = flatbuffer::CreateKUKAiiwaState(*fbbP,0,0,0,0,1,controlState,setArmConfiguration_,kukaiiwaArmConfiguration); + auto kukaiiwaArmConfiguration = flatbuffer::CreateKUKAiiwaArmConfiguration(*fbbP,name,commandInterface_,monitorInterface_, clientCommandMode, overlayType, + controlMode_, setCartesianImpedance, setJointImpedance); - auto kukaiiwaStateVec = fbbP->CreateVector(&kukaiiwastate, 1); + bool setArmControlState = true; // only actually change the arm state when this is true. - auto states = flatbuffer::CreateKUKAiiwaStates(*fbbP,kukaiiwaStateVec); + // TODO fill the 0s + auto kukaiiwastate = flatbuffer::CreateKUKAiiwaState(*fbbP,0,0,0,0,1,controlState,setArmConfiguration_,kukaiiwaArmConfiguration); - grl::flatbuffer::FinishKUKAiiwaStatesBuffer(*fbbP, states); + auto kukaiiwaStateVec = fbbP->CreateVector(&kukaiiwastate, 1); - flatbuffers::Verifier verifier(fbbP->GetBufferPointer(),fbbP->GetSize()); - BOOST_VERIFY(grl::flatbuffer::VerifyKUKAiiwaStatesBuffer(verifier)); + auto states = flatbuffer::CreateKUKAiiwaStates(*fbbP,kukaiiwaStateVec); + grl::flatbuffer::FinishKUKAiiwaStatesBuffer(*fbbP, states); - if(armControlMode_ == flatbuffer::ArmState::MoveArmJointServo) - { - auto states2 = flatbuffer::GetKUKAiiwaStates(fbbP->GetBufferPointer()); - auto movearm = static_cast(states2->states()->Get(0)->armControlState()->state()); - std::vector angles; - for(std::size_t i = 0; i < movearm->goal()->position()->size(); ++i) - { - angles.push_back(movearm->goal()->position()->Get(i)); - } - logger_->info("re-extracted {}{}{}", movearm->goal()->position()->size(), " joint angles: ",angles); - } + flatbuffers::Verifier verifier(fbbP->GetBufferPointer(),fbbP->GetSize()); + BOOST_VERIFY(grl::flatbuffer::VerifyKUKAiiwaStatesBuffer(verifier)); - if(debug_) logger_->info("sending packet to KUKA iiwa: len = {}", fbbP->GetSize()); - int ret; - // Send UDP packet to Robot - ret = sendto(socket_local, fbbP->GetBufferPointer(), fbbP->GetSize(), 0, (struct sockaddr *)&dst_sockaddr, sizeof(dst_sockaddr)); - if (static_cast(ret) != static_cast(fbbP->GetSize())) logger_->error("Error sending packet to KUKA iiwa: ret = {}, len = {}", ret, fbbP->GetSize()); - - setArmConfiguration_ = false; - - // Receiving data from Sunrise - - int num; - temp_mask = mask; - - // tv is the time that you wait for a new message to arrive, - // since we don't wont to hinder the execution of each cycle, it is set to zero... - // (The FRI interface read rate is much higher than the data coming for the controller, so we don't wait for controller data) - struct timeval tv; - tv.tv_sec = 0; - tv.tv_usec = 0; - - // Check to see if any packets are available with waiting time of 0 - // Please note that with this configuration some packets may be dropped. - /// @todo TODO(ahundt) eventually run this in a separate thread so we can receive packets asap and minimize dropping - num = select(FD_SETSIZE, &temp_mask, &dummy_mask, &dummy_mask, &tv); - - if (num > 0) + if(armControlMode_ == flatbuffer::ArmState::MoveArmJointServo) + { + auto states2 = flatbuffer::GetKUKAiiwaStates(fbbP->GetBufferPointer()); + auto movearm = static_cast(states2->states()->Get(0)->armControlState()->state()); + std::vector angles; + for(std::size_t i = 0; i < movearm->goal()->position()->size(); ++i) { - // packets are available, process them - if (FD_ISSET(socket_local, &temp_mask)) - { - // allocate the buffer, should only happen once - if(!java_interface_received_statesP_) { - java_interface_received_statesP_ = std::make_shared>(fbs_tk::Buffer(udp_size_)); - } - if(!java_interface_next_statesP_) { - java_interface_next_statesP_ = std::make_shared>(fbs_tk::Buffer(udp_size_)); - } - - - static const int flags = 0; - // get a reference to the buffer object - const fbs_tk::Buffer& internal_buffer = java_interface_received_statesP_->get_data(); - - // receive an update from the java driver over UDP - std::vector bufferdata = internal_buffer.get_data(); - ret = recvfrom(socket_local, reinterpret_cast(&bufferdata[0]), internal_buffer.size(), flags, (struct sockaddr *)&dst_sockaddr, &dst_sockaddr_len); - if (ret <= 0) { - bool java_state_received_successfully = false; - logger_->error("C++ KukaJAVAdriver Error: Receive failed with ret = {}", ret); - } else { - - if(debug_) logger_->info("C++ KukaJAVAdriver received message size: {}",ret); - - java_interface_received_statesP_->update_root(); - - // Flatbuffer has been verified as valid - if (java_interface_received_statesP_->valid()) { - bool java_state_received_successfully = true; - std::swap(java_interface_received_statesP_, java_interface_next_statesP_); - if(debug_) logger_->info("C++ KukaJAVAdriver: flatbuffer verified successfully"); - } else { - // TODO(ahundt) consider specific error codes for verifier failure vs udp receive failure - bool java_state_received_successfully = false; - logger_->error("C++ KukaJAVAdriver Error: flatbuff failed verification. bufOk: {}", java_state_received_successfully); - } - - } - - } + angles.push_back(movearm->goal()->position()->Get(i)); } + logger_->info("re-extracted {}{}{}", movearm->goal()->position()->size(), " joint angles: ",angles); + } + if(debug_) logger_->info("sending packet to KUKA iiwa: len = {}", fbbP->GetSize()); + int ret; + // Send UDP packet to Robot + ret = sendto(socket_local, fbbP->GetBufferPointer(), fbbP->GetSize(), 0, (struct sockaddr *)&dst_sockaddr, sizeof(dst_sockaddr)); + + if (static_cast(ret) != static_cast(fbbP->GetSize())) logger_->error("Error sending packet to KUKA iiwa: ret = {}, len = {}", ret, fbbP->GetSize()); + + setArmConfiguration_ = false; + + // Receiving data from Sunrise + int num; + temp_mask = mask; + // tv is the time that you wait for a new message to arrive, + // since we don't wont to hinder the execution of each cycle, it is set to zero... + // (The FRI interface read rate is much higher than the data coming for the controller, so we don't wait for controller data) + struct timeval tv; + tv.tv_sec = 0; + tv.tv_usec = 0; + // Check to see if any packets are available with waiting time of 0 + // Please note that with this configuration some packets may be dropped. + /// @todo TODO(ahundt) eventually run this in a separate thread so we can receive packets asap and minimize dropping + num = select(FD_SETSIZE, &temp_mask, &dummy_mask, &dummy_mask, &tv); + + if (num > 0) + { + // packets are available, process them + if (FD_ISSET(socket_local, &temp_mask)) + { + // allocate the buffer, should only happen once + if(!java_interface_received_statesP_) { + java_interface_received_statesP_ = std::make_shared>(fbs_tk::Buffer(udp_size_)); + } + if(!java_interface_next_statesP_) { + java_interface_next_statesP_ = std::make_shared>(fbs_tk::Buffer(udp_size_)); + } + static const int flags = 0; + // get a reference to the buffer object + const fbs_tk::Buffer& internal_buffer = java_interface_received_statesP_->get_data(); + // receive an update from the java driver over UDP + std::vector bufferdata = internal_buffer.get_data(); + ret = recvfrom(socket_local, reinterpret_cast(&bufferdata[0]), internal_buffer.size(), flags, (struct sockaddr *)&dst_sockaddr, &dst_sockaddr_len); + if (ret <= 0) { + bool java_state_received_successfully = false; + logger_->error("C++ KukaJAVAdriver Error: Receive failed with ret = {}", ret); + } else { + if(debug_) logger_->info("C++ KukaJAVAdriver received message size: {}",ret); + java_interface_received_statesP_->update_root(); + // Flatbuffer has been verified as valid + if (java_interface_received_statesP_->valid()) { + bool java_state_received_successfully = true; + std::swap(java_interface_received_statesP_, java_interface_next_statesP_); + if(debug_) logger_->info("C++ KukaJAVAdriver: flatbuffer verified successfully"); + } else { + // TODO(ahundt) consider specific error codes for verifier failure vs udp receive failure + bool java_state_received_successfully = false; + logger_->error("C++ KukaJAVAdriver Error: flatbuff failed verification. bufOk: {}", java_state_received_successfully); + } + } + + + } + } - return haveNewData; - } - - volatile std::size_t m_haveReceivedRealDataCount = 0; - volatile std::size_t m_attemptedCommunicationCount = 0; - volatile std::size_t m_attemptedCommunicationConsecutiveFailureCount = 0; - volatile std::size_t m_attemptedCommunicationConsecutiveSuccessCount = 0; - - - - void setPositionControlMode() - { - boost::lock_guard lock(jt_mutex); - controlMode_ = grl::flatbuffer::EControlMode::POSITION_CONTROL_MODE; - setArmConfiguration_ = true; - } + return haveNewData; + } - bool setJointImpedanceMode(std::vector joint_stiffnes, std::vectorjoint_damping) { - boost::lock_guard lock(jt_mutex); - //TODO use tags - joint_stiffness_ = joint_stiffnes; - joint_damping_ = joint_damping; - controlMode_ = grl::flatbuffer::EControlMode::JOINT_IMP_CONTROL_MODE; - setArmConfiguration_ = true; - } - - // TODO: define custom flatbuffer for Cartesion Quantities - void setCartesianImpedanceMode( - const grl::flatbuffer::EulerPose cart_stiffness, - const grl::flatbuffer::EulerPose cart_damping, - const double nullspace_stiffness, - const double nullspace_damping, - const grl::flatbuffer::EulerPose cart_max_path_deviation, - const grl::flatbuffer::EulerPose cart_max_ctrl_vel, - const grl::flatbuffer::EulerPose cart_max_ctrl_force, - const bool max_control_force_stop) - { - boost::lock_guard lock(jt_mutex); + volatile std::size_t m_haveReceivedRealDataCount = 0; + volatile std::size_t m_attemptedCommunicationCount = 0; + volatile std::size_t m_attemptedCommunicationConsecutiveFailureCount = 0; + volatile std::size_t m_attemptedCommunicationConsecutiveSuccessCount = 0; + void setPositionControlMode() + { + boost::lock_guard lock(jt_mutex); + controlMode_ = grl::flatbuffer::EControlMode::POSITION_CONTROL_MODE; + setArmConfiguration_ = true; + } + bool setJointImpedanceMode(std::vector joint_stiffnes, std::vectorjoint_damping) { + boost::lock_guard lock(jt_mutex); + //TODO use tags + joint_stiffness_ = joint_stiffnes; + joint_damping_ = joint_damping; + controlMode_ = grl::flatbuffer::EControlMode::JOINT_IMP_CONTROL_MODE; + setArmConfiguration_ = true; + } + // TODO: define custom flatbuffer for Cartesion Quantities + void setCartesianImpedanceMode( + const grl::flatbuffer::EulerPose cart_stiffness, + const grl::flatbuffer::EulerPose cart_damping, + const double nullspace_stiffness, + const double nullspace_damping, + const grl::flatbuffer::EulerPose cart_max_path_deviation, + const grl::flatbuffer::EulerPose cart_max_ctrl_vel, + const grl::flatbuffer::EulerPose cart_max_ctrl_force, + const bool max_control_force_stop) + { + boost::lock_guard lock(jt_mutex); - cart_stiffness_ = cart_stiffness; - cart_damping_ = cart_damping; + cart_stiffness_ = cart_stiffness; + cart_damping_ = cart_damping; - cart_max_path_deviation_ = cart_max_path_deviation; - cart_max_ctrl_vel_ = cart_max_ctrl_vel; - cart_max_ctrl_force_ = cart_max_ctrl_force; + cart_max_path_deviation_ = cart_max_path_deviation; + cart_max_ctrl_vel_ = cart_max_ctrl_vel; + cart_max_ctrl_force_ = cart_max_ctrl_force; - nullspace_stiffness_ = nullspace_stiffness; - nullspace_damping_ = nullspace_damping; + nullspace_stiffness_ = nullspace_stiffness; + nullspace_damping_ = nullspace_damping; - max_control_force_stop_ = max_control_force_stop; + max_control_force_stop_ = max_control_force_stop; - controlMode_ = grl::flatbuffer::EControlMode::CART_IMP_CONTROL_MODE; - setArmConfiguration_ = true; - } + controlMode_ = grl::flatbuffer::EControlMode::CART_IMP_CONTROL_MODE; + setArmConfiguration_ = true; + } /** * \brief Set the joint positions for the current interpolation step. @@ -697,6 +675,7 @@ namespace grl { namespace robot { namespace arm { socklen_t dst_sockaddr_len = sizeof(dst_sockaddr); struct sockaddr_in local_sockaddr; long dst_ip; + /// An fd_set is a set of sockets to "monitor" for some activity (set of socket descriptors). fd_set mask, temp_mask, dummy_mask; Params params_; @@ -770,7 +749,7 @@ namespace grl { namespace robot { namespace arm { std::vector joint_stiffness_; std::vector joint_damping_; - }; + }; // End of class }}}// namespace grl::robot::arm diff --git a/include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp b/include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp index 4e732b7..e3c7c9b 100644 --- a/include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp +++ b/include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp @@ -16,15 +16,12 @@ namespace grl { namespace vrep { - - - /// Creates a complete vrep plugin object /// usage: /// @code -/// auto kukaPluginPG = std::make_shared(); -/// kukaPluginPG->construct(); -/// while(true) kukaPluginPG->run_one(); +/// auto kukaVrepPluginPG = std::make_shared(); +/// kukaVrepPluginPG->construct(); +/// while(true) kukaVrepPluginPG->run_one(); /// @endcode /// /// @todo this implementation is a bit hacky, redesign it @@ -72,7 +69,6 @@ class KukaVrepPlugin : public std::enable_shared_from_this { std::string > Params; - static const Params defaultParams(){ std::vector jointHandles; @@ -104,7 +100,7 @@ class KukaVrepPlugin : public std::enable_shared_from_this { } /// @todo measuredArmParams are hardcoded, parameterize them - // parameters for measured arm + /// parameters for measured arm static const Params measuredArmParams(){ std::vector jointHandles; @@ -135,351 +131,332 @@ class KukaVrepPlugin : public std::enable_shared_from_this { ); } + /// @todo allow KukaFRIThreadSeparator parameters to be updated + /// @param params a tuple containing all of the parameter strings needed to configure the device. + /// + /// The KukaCommandMode parameters supports the options "FRI" and "JAVA". This configures how commands will + /// be sent to the arm itself. "FRI" mode is via a direct "Fast Robot Interface" "KUKA KONI" + /// ethernet connection which provides substantially higher performance and response time, + /// but is extremely sensitive to delays, and any delay will halt the robot and require a manual reset. + /// "JAVA" mode sends the command to the Java application installed on the KUKA robot, which then submits + /// it to the arm itself to execute. This is a much more forgiving mode of communication, but it is subject to delays. + /// + /// @todo read ms_per_tick from the java interface + KukaVrepPlugin (Params params = defaultParams()) : params_(params) + { + logger_ = spdlog::get("console"); + } - - - - -/// @todo allow KukaFRIThreadSeparator parameters to be updated -/// @param params a tuple containing all of the parameter strings needed to configure the device. -/// -/// The KukaCommandMode parameters supports the options "FRI" and "JAVA". This configures how commands will -/// be sent to the arm itself. "FRI" mode is via a direct "Fast Robot Interface" "KUKA KONI" -/// ethernet connection which provides substantially higher performance and response time, -/// but is extremely sensitive to delays, and any delay will halt the robot and require a manual reset. -/// "JAVA" mode sends the command to the Java application installed on the KUKA robot, which then submits -/// it to the arm itself to execute. This is a much more forgiving mode of communication, but it is subject to delays. -/// -/// @todo read ms_per_tick from the java interface -KukaVrepPlugin (Params params = defaultParams()) - : - params_(params) -{ - logger_ = spdlog::get("console"); -} - -void construct(){construct(params_);} - -/// construct() function completes initialization of the plugin -/// @todo move this into the actual constructor, but need to correctly handle or attach vrep shared libraries for unit tests. -void construct(Params params){ - params_ = params; - // keep driver threads from exiting immediately after creation, because they have work to do! - device_driver_workP_.reset(new boost::asio::io_service::work(device_driver_io_service)); - kukaDriverP_=std::make_shared(std::make_tuple( - - std::get(params), - std::get(params), - std::get(params), - std::get(params), - std::get(params), - std::get(params), - std::get(params), - std::get(params), - std::get(params), - std::get(params), - std::get(params) - - - )); - kukaDriverP_->construct(); - // Default to joint servo mode for commanding motion - kukaDriverP_->set(flatbuffer::ArmState::MoveArmJointServo); - std::cout << "KUKA COMMAND MODE: " << std::get(params) << "\n"; - initHandles(); -} - -/// start recording the kuka state data in memory -/// return true on success, false on failure -bool start_recording() -{ - if(kukaDriverP_.get() != nullptr) { - return kukaDriverP_->start_recording(); + void construct(){construct(params_);} + + /// construct() function completes initialization of the plugin + /// @todo move this into the actual constructor, but need to correctly handle or attach vrep shared libraries for unit tests. + void construct(Params params){ + std::cout<< "Start KukaVrepPlugin->construct()..." << std::endl; + params_ = params; + // keep driver threads from exiting immediately after creation, because they have work to do! + device_driver_workP_.reset(new boost::asio::io_service::work(device_driver_io_service)); + kukaDriverP_=std::make_shared(std::make_tuple( + + std::get(params), + std::get(params), + std::get(params), + std::get(params), + std::get(params), + std::get(params), + std::get(params), + std::get(params), + std::get(params), + std::get(params), + std::get(params) + )); + kukaDriverP_->construct(); + // Default to joint servo mode for commanding motion + kukaDriverP_->set(flatbuffer::ArmState::MoveArmJointServo); + std::cout << "KUKA COMMAND MODE: " << std::get(params) << "\n"; + initHandles(); } - return false; -} -/// stop recording the kuka state data in memory -/// return true on success, false on failure -bool stop_recording() -{ - if(kukaDriverP_.get() != nullptr) { - return kukaDriverP_->stop_recording(); + + /// start recording the kuka state data in memory + /// return true on success, false on failure + bool start_recording() + { + if(kukaDriverP_.get() != nullptr) { + return kukaDriverP_->start_recording(); + } + return false; } - return false; + /// stop recording the kuka state data in memory + /// return true on success, false on failure + bool stop_recording() + { + if(kukaDriverP_.get() != nullptr) { + return kukaDriverP_->stop_recording(); + } + return false; -} -bool save_recording(std::string filename = std::string()) { - if(kukaDriverP_.get() != nullptr) { - return kukaDriverP_->save_recording(filename); } - return false; -} -void clear_recording() -{ - if(kukaDriverP_.get() != nullptr) { - kukaDriverP_->clear_recording(); + bool save_recording(std::string filename = std::string()) { + if(kukaDriverP_.get() != nullptr) { + return kukaDriverP_->save_recording(filename); + } + return false; } -} - -bool is_recording() -{ if(kukaDriverP_.get() != nullptr) { - return kukaDriverP_->is_recording(); + void clear_recording() + { + if(kukaDriverP_.get() != nullptr) { + kukaDriverP_->clear_recording(); + } } - return false; -} - -void run_one(){ - - if(!allHandlesSet) BOOST_THROW_EXCEPTION(std::runtime_error("KukaVrepPlugin: Handles have not been initialized, cannot run updates.")); - getRealKukaAngles(); - bool isError = getStateFromVrep(); // true if there is an error - allHandlesSet = !isError; - /// @todo re-enable simulation feedback based on actual kuka state - syncVrepAndKuka(); -} - - -~KukaVrepPlugin(){ - device_driver_workP_.reset(); - if(driver_threadP){ - device_driver_io_service.stop(); - driver_threadP->join(); + bool is_recording() + { if(kukaDriverP_.get() != nullptr) { + return kukaDriverP_->is_recording(); + } + return false; } -} - -private: - - - -void initHandles() { - - vrepRobotArmDriverP_ = std::make_shared - ( - std::make_tuple( - std::get (params_), - std::get (params_), - std::get (params_), - std::get (params_), - std::get (params_), - std::get (params_) - ) - ); - - vrepRobotArmDriverP_->construct(); - - measuredParams_ = measuredArmParams(); - vrepMeasuredRobotArmDriverP_ = std::make_shared - ( - std::make_tuple( - std::get (measuredParams_), - std::get (measuredParams_), - std::get (measuredParams_), - std::get (measuredParams_), - std::get (measuredParams_), - std::get (measuredParams_) - ) - ); - vrepMeasuredRobotArmDriverP_->construct(); + void run_one(){ + if(!allHandlesSet) BOOST_THROW_EXCEPTION(std::runtime_error("KukaVrepPlugin: Handles have not been initialized, cannot run updates.")); + getRealKukaAngles(); + bool isError = getStateFromVrep(); // true if there is an error + allHandlesSet = !isError; + /// @todo re-enable simulation feedback based on actual kuka state + syncVrepAndKuka(); + } - /// @todo remove this assumption - allHandlesSet = true; -} -void getRealKukaAngles() { - /// @todo TODO(ahundt) m_haveReceivedRealData = true is a DANGEROUS HACK!!!! REMOVE ME AFTER DEBUGGING - m_haveReceivedRealData = true; + ~KukaVrepPlugin(){ + device_driver_workP_.reset(); + if(driver_threadP){ + device_driver_io_service.stop(); + driver_threadP->join(); + } + } + private: + + void initHandles() { + + vrepRobotArmDriverP_ = std::make_shared + ( + std::make_tuple( + std::get (params_), + std::get (params_), + std::get (params_), + std::get (params_), + std::get (params_), + std::get (params_) + ) + ); + + vrepRobotArmDriverP_->construct(); + measuredParams_ = measuredArmParams(); + vrepMeasuredRobotArmDriverP_ = std::make_shared + ( + std::make_tuple( + std::get (measuredParams_), + std::get (measuredParams_), + std::get (measuredParams_), + std::get (measuredParams_), + std::get (measuredParams_), + std::get (measuredParams_) + ) + ); + vrepMeasuredRobotArmDriverP_->construct(); + /// @todo remove this assumption + allHandlesSet = true; + } -} + void getRealKukaAngles() { + /// @todo TODO(ahundt) m_haveReceivedRealData = true is a DANGEROUS HACK!!!! REMOVE ME AFTER DEBUGGING + m_haveReceivedRealData = true; + } -void syncVrepAndKuka(){ + void syncVrepAndKuka(){ + bool debug = true; if(!allHandlesSet || !m_haveReceivedRealData) return; - /// @todo make this handled by template driver implementations/extensions - kukaDriverP_->set(simulationTimeStep_,time_duration_command_tag()); + kukaDriverP_->set(simulationTimeStep_, time_duration_command_tag()); kukaDriverP_->set( simJointPosition, grl::revolute_joint_angle_open_chain_command_tag()); - if(0) kukaDriverP_->set( simJointForce , grl::revolute_joint_torque_open_chain_command_tag()); - - + if(debug) kukaDriverP_->set( simJointForce, grl::revolute_joint_torque_open_chain_command_tag()); kukaDriverP_->run_one(); // We have the real kuka state read from the device now // update real joint angle data realJointPosition.clear(); kukaDriverP_->get(std::back_inserter(realJointPosition), grl::revolute_joint_angle_open_chain_state_tag()); - realJointForce.clear(); kukaDriverP_->get(std::back_inserter(realJointForce), grl::revolute_joint_torque_open_chain_state_tag()); - realExternalJointTorque.clear(); kukaDriverP_->get(std::back_inserter(realExternalJointTorque), grl::revolute_joint_torque_external_open_chain_state_tag()); - realExternalForce.clear(); kukaDriverP_->get(std::back_inserter(realExternalForce), grl::cartesian_external_force_tag()); - if(0){ - // debug output - std::cout << "Measured Torque: "; - std::cout << std::setw(6); - for (float t:realJointForce) { - std::cout << t << " "; + if(debug){ + // debug output + std::cout << "Measured Torque: "; + std::cout << std::setw(6); + for (float t:realJointForce) { + std::cout << t << " "; + } + std::cout << '\n'; + + std::cout << "External Torque: "; + std::cout << std::setw(6); + for (float t:realExternalJointTorque) { + std::cout << t << " "; + } + std::cout << '\n'; + + std::cout << "Measured Position: "; + for (float t:realJointPosition) { + std::cout << t << " "; + } + std::cout << '\n'; } - std::cout << '\n'; - std::cout << "External Torque: "; - std::cout << std::setw(6); - for (float t:realExternalJointTorque) { - std::cout << t << " "; - } - std::cout << '\n'; + if(!allHandlesSet || !vrepMeasuredRobotArmDriverP_) return; + + if (realJointPosition.size() > 0) { // if there are valid measured states + VrepRobotArmDriver::State measuredArmState; + std::get(measuredArmState) = realJointPosition; + std::get(measuredArmState) = realJointForce; + std::get(measuredArmState) = realExternalJointTorque; - std::cout << "Measured Position: "; - for (float t:realJointPosition) { - std::cout << t << " "; + vrepMeasuredRobotArmDriverP_->setState(measuredArmState); } - std::cout << '\n'; + } - if(!allHandlesSet || !vrepMeasuredRobotArmDriverP_) return; + /// @todo if there aren't real limits set via the kuka model already then implement me + void setArmLimits(){ + //simSetJointInterval(simInt objectHandle,simBool cyclic,const simFloat* interval); //Sets the interval parameters of a joint (i.e. range values) + //simSetJointMode(simInt jointHandle,simInt jointMode,simInt options); //Sets the operation mode of a joint. Might have as side-effect the change of additional properties of the joint - if (realJointPosition.size() > 0) { // if there are valid measured states - VrepRobotArmDriver::State measuredArmState; - std::get(measuredArmState) = realJointPosition; - std::get(measuredArmState) = realJointForce; - std::get(measuredArmState) = realExternalJointTorque; + } - vrepMeasuredRobotArmDriverP_->setState(measuredArmState); + /// @return isError - true if there is an error, false if there is no Error + bool getStateFromVrep(){ + if(!allHandlesSet || !vrepRobotArmDriverP_) return false; + + VrepRobotArmDriver::State armState; + + bool isError = vrepRobotArmDriverP_->getState(armState); + + if(isError) return isError; + + simJointPosition = std::get (armState); + simJointForce = std::get (armState); + simJointTargetPosition = std::get(armState); + simJointTransformationMatrix = std::get (armState); + + + /// need to provide tick time in double seconds and get from vrep API call + simulationTimeStep_ = simGetSimulationTimeStep()*1000; + + // for (int i=0 ; i < KUKA::LBRState::NUM_DOF ; i++) + // { + // simGetJointPosition(jointHandle[i],&simJointPosition[i]); //retrieves the intrinsic position of a joint (Angle for revolute joint) + // simGetJointForce(jointHandle[i],&simJointForce[i]); //retrieves the force or torque applied to a joint along/about its active axis. This function retrieves meaningful information only if the joint is prismatic or revolute, and is dynamically enabled. + // simGetJointTargetPosition(jointHandle[i],&simJointTargetPosition[i]); //retrieves the target position of a joint + // simGetJointMatrix(jointHandle[i],&simJointTransformationMatrix[i]); //retrieves the intrinsic transformation matrix of a joint (the transformation caused by the joint movement) + // + // } + // + // BOOST_LOG_TRIVIAL(info) << "simJointPostition = " << simJointPosition << std::endl; + // BOOST_LOG_TRIVIAL(info) << "simJointForce = " << simJointForce << std::endl; + // BOOST_LOG_TRIVIAL(info) << "simJointTargetPostition = " << simJointTargetPosition << std::endl; + // BOOST_LOG_TRIVIAL(info) << "simJointTransformationMatrix = " << simJointTransformationMatrix << std::endl; + // + // float simTipPosition[3]; + // float simTipOrientation[3]; + // + // simGetObjectPosition(target, targetBase, simTipPosition); + // simGetObjectOrientation(target, targetBase, simTipOrientation); + // + // for (int i = 0 ; i < 3 ; i++) + // { + // BOOST_LOG_TRIVIAL(info) << "simTipPosition[" << i << "] = " << simTipPosition[i] << std::endl; + // BOOST_LOG_TRIVIAL(info) << "simTipOrientation[" << i << "] = " << simTipOrientation[i] << std::endl; + // + // } + // Send updated position to the real arm based on simulation + return false; } -} + /// @todo reenable this component + bool updateVrepFromKuka() { + // Step 1 + //////////////////////////////////////////////////// + // call the functions here and just print joint angles out + // or display something on the screens + + /////////////////// + // call our object to get the latest real kuka state + // then use the functions below to set the simulation state + // to match + + /////////////////// assuming given real joint position (angles), forces, target position and target velocity + + + // setting the simulation variables to data from real robot (here they have been assumed given) + + for (int i=0 ; i < 7 ; i++) + { + //simSetJointPosition(jointHandle[i],realJointPosition[i]); //Sets the intrinsic position of a joint. May have no effect depending on the joint mode + + + //simSetJointTargetPosition(jointHandle[i],realJointTargetPosition[i]); //Sets the target position of a joint if the joint is in torque/force mode (also make sure that the joint's motor and position control are enabled + + //simSetJointForce(jointHandle[i],realJointForce[i]); //Sets the maximum force or torque that a joint can exert. This function has no effect when the joint is not dynamically enabled + + + //simSetJointTargetVelocity(jointHandle[i],realJointTargetVelocity[i]); //Sets the intrinsic target velocity of a non-spherical joint. This command makes only sense when the joint mode is: (a) motion mode: the joint's motion handling feature must be enabled (simHandleJoint must be called (is called by default in the main script), and the joint motion properties must be set in the joint settings dialog), (b) torque/force mode: the dynamics functionality and the joint motor have to be enabled (position control should however be disabled) + } + return false; + } -/// @todo if there aren't real limits set via the kuka model already then implement me -void setArmLimits(){ - //simSetJointInterval(simInt objectHandle,simBool cyclic,const simFloat* interval); //Sets the interval parameters of a joint (i.e. range values) - //simSetJointMode(simInt jointHandle,simInt jointMode,simInt options); //Sets the operation mode of a joint. Might have as side-effect the change of additional properties of the joint - -} - -/// @return isError - true if there is an error, false if there is no Error -bool getStateFromVrep(){ - if(!allHandlesSet || !vrepRobotArmDriverP_) return false; - - VrepRobotArmDriver::State armState; - - bool isError = vrepRobotArmDriverP_->getState(armState); - - if(isError) return isError; - - simJointPosition = std::get (armState); - simJointForce = std::get (armState); - simJointTargetPosition = std::get(armState); - simJointTransformationMatrix = std::get (armState); - - - /// need to provide tick time in double seconds and get from vrep API call - simulationTimeStep_ = simGetSimulationTimeStep()*1000; - -// for (int i=0 ; i < KUKA::LBRState::NUM_DOF ; i++) -// { -// simGetJointPosition(jointHandle[i],&simJointPosition[i]); //retrieves the intrinsic position of a joint (Angle for revolute joint) -// simGetJointForce(jointHandle[i],&simJointForce[i]); //retrieves the force or torque applied to a joint along/about its active axis. This function retrieves meaningful information only if the joint is prismatic or revolute, and is dynamically enabled. -// simGetJointTargetPosition(jointHandle[i],&simJointTargetPosition[i]); //retrieves the target position of a joint -// simGetJointMatrix(jointHandle[i],&simJointTransformationMatrix[i]); //retrieves the intrinsic transformation matrix of a joint (the transformation caused by the joint movement) -// -// } -// -// BOOST_LOG_TRIVIAL(info) << "simJointPostition = " << simJointPosition << std::endl; -// BOOST_LOG_TRIVIAL(info) << "simJointForce = " << simJointForce << std::endl; -// BOOST_LOG_TRIVIAL(info) << "simJointTargetPostition = " << simJointTargetPosition << std::endl; -// BOOST_LOG_TRIVIAL(info) << "simJointTransformationMatrix = " << simJointTransformationMatrix << std::endl; -// -// float simTipPosition[3]; -// float simTipOrientation[3]; -// -// simGetObjectPosition(target, targetBase, simTipPosition); -// simGetObjectOrientation(target, targetBase, simTipOrientation); -// -// for (int i = 0 ; i < 3 ; i++) -// { -// BOOST_LOG_TRIVIAL(info) << "simTipPosition[" << i << "] = " << simTipPosition[i] << std::endl; -// BOOST_LOG_TRIVIAL(info) << "simTipOrientation[" << i << "] = " << simTipOrientation[i] << std::endl; -// -// } - // Send updated position to the real arm based on simulation - return false; -} - -/// @todo reenable this component -bool updateVrepFromKuka() { - // Step 1 - //////////////////////////////////////////////////// - // call the functions here and just print joint angles out - // or display something on the screens - - /////////////////// - // call our object to get the latest real kuka state - // then use the functions below to set the simulation state - // to match - - /////////////////// assuming given real joint position (angles), forces, target position and target velocity - - - // setting the simulation variables to data from real robot (here they have been assumed given) - - for (int i=0 ; i < 7 ; i++) - { - //simSetJointPosition(jointHandle[i],realJointPosition[i]); //Sets the intrinsic position of a joint. May have no effect depending on the joint mode - - - //simSetJointTargetPosition(jointHandle[i],realJointTargetPosition[i]); //Sets the target position of a joint if the joint is in torque/force mode (also make sure that the joint's motor and position control are enabled - - //simSetJointForce(jointHandle[i],realJointForce[i]); //Sets the maximum force or torque that a joint can exert. This function has no effect when the joint is not dynamically enabled - - - //simSetJointTargetVelocity(jointHandle[i],realJointTargetVelocity[i]); //Sets the intrinsic target velocity of a non-spherical joint. This command makes only sense when the joint mode is: (a) motion mode: the joint's motion handling feature must be enabled (simHandleJoint must be called (is called by default in the main script), and the joint motion properties must be set in the joint settings dialog), (b) torque/force mode: the dynamics functionality and the joint motor have to be enabled (position control should however be disabled) - } - return false; -} - -volatile bool allHandlesSet = false; -volatile bool m_haveReceivedRealData = false; - -double simulationTimeStep_; // ms - -boost::asio::io_service device_driver_io_service; -std::unique_ptr device_driver_workP_; -std::unique_ptr driver_threadP; -std::shared_ptr kukaDriverP_; -std::shared_ptr vrepRobotArmDriverP_; -std::shared_ptr vrepMeasuredRobotArmDriverP_; -//boost::asio::deadline_timer sendToJavaDelay; - -/// @todo replace all these simJoint elements with simple VrepRobotArmDriver::State -std::vector simJointPosition = {0.0,0.0,0.0,0.0,0.0,0.0,0.0}; -// std::vector simJointVelocity = {0.0,0.0,0.0,0.0,0.0,0.0,0.0}; no velocity yet -std::vector simJointForce = {0.0,0.0,0.0,0.0,0.0,0.0,0.0}; -std::vector simJointTargetPosition = {0.0,0.0,0.0,0.0,0.0,0.0,0.0}; -VrepRobotArmDriver::TransformationMatrices simJointTransformationMatrix; - -/// @note loss of precision! kuka sends double values, if you write custom code don't use these float values. Vrep uses floats internally which is why they are used here. -std::vector realJointPosition = { 0, 0, 0, 0, 0, 0, 0 }; -// does not exist -// std::vector realJointTargetPosition = { 0, 0, 0, 0, 0, 0, 0 }; -std::vector realJointForce = { 0, 0, 0, 0, 0, 0, 0 }; -// does not exist yet -//std::vector realJointTargetVelocity = { 0, 0, 0, 0, 0, 0, 0 }; -std::vector realExternalJointTorque = { 0, 0, 0, 0, 0, 0, 0}; -std::vector realExternalForce = { 0, 0, 0, 0, 0, 0}; -private: -Params params_; -Params measuredParams_; -/// for use in FRI clientdata mode -std::shared_ptr friData_; -std::shared_ptr logger_; + volatile bool allHandlesSet = false; + volatile bool m_haveReceivedRealData = false; + + double simulationTimeStep_; // ms + + boost::asio::io_service device_driver_io_service; + // CThe work class is used to inform the io_service when work starts and finishes. + // This ensures that the io_service object's run() function will not exit while work is underway, + // and that it does exit when there is no unfinished work remaining. + std::unique_ptr device_driver_workP_; + std::unique_ptr driver_threadP; + std::shared_ptr kukaDriverP_; + std::shared_ptr vrepRobotArmDriverP_; + std::shared_ptr vrepMeasuredRobotArmDriverP_; + //boost::asio::deadline_timer sendToJavaDelay; + + /// @todo replace all these simJoint elements with simple VrepRobotArmDriver::State + std::vector simJointPosition = {0.0,0.0,0.0,0.0,0.0,0.0,0.0}; + // std::vector simJointVelocity = {0.0,0.0,0.0,0.0,0.0,0.0,0.0}; no velocity yet + std::vector simJointForce = {0.0,0.0,0.0,0.0,0.0,0.0,0.0}; + std::vector simJointTargetPosition = {0.0,0.0,0.0,0.0,0.0,0.0,0.0}; + VrepRobotArmDriver::TransformationMatrices simJointTransformationMatrix; + + /// @note loss of precision! kuka sends double values, if you write custom code don't use these float values. Vrep uses floats internally which is why they are used here. + std::vector realJointPosition = { 0, 0, 0, 0, 0, 0, 0 }; + // does not exist + // std::vector realJointTargetPosition = { 0, 0, 0, 0, 0, 0, 0 }; + std::vector realJointForce = { 0, 0, 0, 0, 0, 0, 0 }; + // does not exist yet + //std::vector realJointTargetVelocity = { 0, 0, 0, 0, 0, 0, 0 }; + std::vector realExternalJointTorque = { 0, 0, 0, 0, 0, 0, 0}; + std::vector realExternalForce = { 0, 0, 0, 0, 0, 0}; + private: + Params params_; + Params measuredParams_; + /// for use in FRI clientdata mode + std::shared_ptr friData_; + std::shared_ptr logger_; }; diff --git a/include/grl/vrep/VrepRobotArmDriver.hpp b/include/grl/vrep/VrepRobotArmDriver.hpp index b360ba6..6e6451d 100644 --- a/include/grl/vrep/VrepRobotArmDriver.hpp +++ b/include/grl/vrep/VrepRobotArmDriver.hpp @@ -39,19 +39,19 @@ namespace grl { namespace vrep { linkNames.push_back("LBR_iiwa_14_R820_link8"); return linkNames; } - + /// @todo TODO(ahundt) HACK joint to link and joint to respondable only works for v-rep provided scene heirarchy names, modify full class so these workarounds aren't needed. /// @see jointToLink std::vector jointToLinkRespondable(std::vector jointNames) { auto linkNames = jointToLink(jointNames); - + std::vector linkNames_resp; - + int i = 1; for(std::string linkName : linkNames) { - /// @todo TODO(ahundt) link 1 isn't respondable because it is anchored to the ground, but should there be an empty string so the indexes are consistent or skip it entirely? (currently skipping) + /// @todo TODO(ahundt) link 1 isn't respondable because it is anchored to the ground, but should there be an empty string so the indexes are consistent or skip it entirely? (currently skipping) if(i!=1) { boost::algorithm::replace_last(linkName,"link" + boost::lexical_cast(i),"link" + boost::lexical_cast(i) + "_resp"); @@ -59,14 +59,14 @@ namespace grl { namespace vrep { } ++i; } - + return linkNames_resp; } /// @brief C++ interface for any open chain V-REP robot arm /// -/// VrepRobotArmDriver makes it easy to specify and interact with the -/// joints in a V-REP defined robot arm in a consistent manner. +/// VrepRobotArmDriver makes it easy to specify and interact with the +/// joints in a V-REP defined robot arm in a consistent manner. /// /// @todo add support for links, particularly the respondable aspects, see LBR_iiwa_14_R820_joint1_resp in RoboneSimulation.ttt /// @todo write generic getters and setters for this object like in KukaFRIalgorithm.hpp and the member functions of KukaFRIdriver, KukaJAVAdriver @@ -81,7 +81,7 @@ class VrepRobotArmDriver : public std::enable_shared_from_this, std::string, @@ -90,7 +90,7 @@ class VrepRobotArmDriver : public std::enable_shared_from_this Params; - + typedef std::tuple< std::vector, int, @@ -99,19 +99,19 @@ class VrepRobotArmDriver : public std::enable_shared_from_this VrepHandleParams; - + static const Params defaultParams() { std::vector jointNames{ "LBR_iiwa_14_R820_joint1" , // Joint1Handle, - "LBR_iiwa_14_R820_joint2" , // Joint2Handle, - "LBR_iiwa_14_R820_joint3" , // Joint3Handle, - "LBR_iiwa_14_R820_joint4" , // Joint4Handle, - "LBR_iiwa_14_R820_joint5" , // Joint5Handle, - "LBR_iiwa_14_R820_joint6" , // Joint6Handle, + "LBR_iiwa_14_R820_joint2" , // Joint2Handle, + "LBR_iiwa_14_R820_joint3" , // Joint3Handle, + "LBR_iiwa_14_R820_joint4" , // Joint4Handle, + "LBR_iiwa_14_R820_joint5" , // Joint5Handle, + "LBR_iiwa_14_R820_joint6" , // Joint6Handle, "LBR_iiwa_14_R820_joint7" // Joint7Handle, }; - + return std::make_tuple( jointNames , // JointNames "RobotFlangeTip" , // RobotFlangeTipName, @@ -121,7 +121,7 @@ class VrepRobotArmDriver : public std::enable_shared_from_this jointNames{ @@ -133,7 +133,7 @@ class VrepRobotArmDriver : public std::enable_shared_from_this JointScalar; - + /// @see http://www.coppeliarobotics.com/helpFiles/en/apiFunctions.htm#simGetJointMatrix for data layout information typedef std::array TransformationMatrix; typedef std::vector TransformationMatrices; - + typedef std::tuple< JointScalar, // jointPosition // JointScalar // JointVelocity // no velocity yet @@ -178,19 +176,19 @@ class VrepRobotArmDriver : public std::enable_shared_from_this State; - - + + VrepRobotArmDriver(Params params = defaultParams()) : params_(params) { } - + /// @todo create a function that calls simGetObjectHandle and throws an exception when it fails /// @warning getting the ik group is optional, so it does not throw an exception void construct() { std::vector jointHandle; getHandleFromParam(params_,std::back_inserter(jointHandle)); - handleParams_ = + handleParams_ = std::make_tuple( std::move(jointHandle) //Obtain Joint Handles ,getHandleFromParam (params_) //Obtain RobotTip handle @@ -199,7 +197,7 @@ void construct() { ,getHandleFromParam (params_) ,simGetIkGroupHandle(std::get (params_).c_str()) ); - + /// @todo TODO(ahundt) move these functions/member variables into params_ object, take as parameters from lua! linkNames = jointToLink(std::get(params_)); getHandles(linkNames, std::back_inserter(linkHandles)); @@ -217,33 +215,33 @@ void construct() { bool getState(State& state){ if(!allHandlesSet) return false; const std::vector& jointHandle = std::get(handleParams_); - + std::get (state).resize(jointHandle.size()); std::get (state).resize(jointHandle.size()); std::get (state).resize(jointHandle.size()); std::get (state).resize(jointHandle.size()); std::get (state).resize(jointHandle.size()); std::get (state).resize(jointHandle.size()); - + enum limit { lower ,upper ,numLimits }; - + simBool isCyclic; float jointAngleInterval[2]; // min,max double inf = std::numeric_limits::infinity(); - + for (std::size_t i=0 ; i < jointHandle.size() ; i++) - { + { int currentJointHandle = jointHandle[i]; simGetJointPosition(currentJointHandle,&std::get(state)[i]); //retrieves the intrinsic position of a joint (Angle for revolute joint) simGetJointForce(currentJointHandle,&std::get(state)[i]); //retrieves the force or torque applied to a joint along/about its active axis. This function retrieves meaningful information only if the joint is prismatic or revolute, and is dynamically enabled. simGetJointTargetPosition(currentJointHandle,&std::get(state)[i]); //retrieves the target position of a joint simGetJointMatrix(currentJointHandle,&std::get(state)[i][0]); //retrieves the intrinsic transformation matrix of a joint (the transformation caused by the joint movement) simGetJointInterval(currentJointHandle,&isCyclic,jointAngleInterval); - + /// @todo TODO(ahundt) is always setting infinity if it is cyclic the right thing to do? if(isCyclic) { @@ -257,18 +255,18 @@ bool getState(State& state){ } } - + // BOOST_LOG_TRIVIAL(info) << "simJointPostition = " << simJointPosition << std::endl; // BOOST_LOG_TRIVIAL(info) << "simJointForce = " << simJointForce << std::endl; // BOOST_LOG_TRIVIAL(info) << "simJointTargetPostition = " << simJointTargetPosition << std::endl; // BOOST_LOG_TRIVIAL(info) << "simJointTransformationMatrix = " << simJointTransformationMatrix << std::endl; -// +// // float simTipPosition[3]; // float simTipOrientation[3]; // // simGetObjectPosition(target, targetBase, simTipPosition); // simGetObjectOrientation(target, targetBase, simTipOrientation); -// +// // for (int i = 0 ; i < 3 ; i++) // { // BOOST_LOG_TRIVIAL(info) << "simTipPosition[" << i << "] = " << simTipPosition[i] << std::endl; @@ -305,31 +303,31 @@ bool setState(State& state) { for (int i=0 ; i < 7 ; i++) { simSetJointPosition(jointHandle[i],realJointPosition[i]); //Sets the intrinsic position of a joint. May have no effect depending on the joint mode - - + + //simSetJointTargetPosition(jointHandle[i],realJointTargetPosition[i]); //Sets the target position of a joint if the joint is in torque/force mode (also make sure that the joint's motor and position control are enabled - + //simSetJointForce(jointHandle[i],realJointForce[i]); //Sets the maximum force or torque that a joint can exert. This function has no effect when the joint is not dynamically enabled - - + + //simSetJointTargetVelocity(jointHandle[i],realJointTargetVelocity[i]); //Sets the intrinsic target velocity of a non-spherical joint. This command makes only sense when the joint mode is: (a) motion mode: the joint's motion handling feature must be enabled (simHandleJoint must be called (is called by default in the main script), and the joint motion properties must be set in the joint settings dialog), (b) torque/force mode: the dynamics functionality and the joint motor have to be enabled (position control should however be disabled) } - + if (externalHandlesSet) { - + } - + else { - + for (int i=0 ; i < 7 ; i++) { std::string torqueString = boost::lexical_cast(externalJointForce[i]); char * externalTorqueBytes = new char[torqueString.length()+1]; std::strcpy(externalTorqueBytes, torqueString.c_str()); - + simAddObjectCustomData(jointHandle[i], externalTorqueHandle, externalTorqueBytes, torqueString.length()+1); } } - + return true; } diff --git a/include/thirdparty/cartographer/include/cartographer/common/time.h b/include/thirdparty/cartographer/include/cartographer/common/time.h index 31c4778..62446a7 100644 --- a/include/thirdparty/cartographer/include/cartographer/common/time.h +++ b/include/thirdparty/cartographer/include/cartographer/common/time.h @@ -37,15 +37,22 @@ struct UniversalTimeScaleClock { using time_point = std::chrono::time_point; static constexpr bool is_steady = true; - static time_point now() noexcept - { + static time_point now() noexcept + { using namespace std::chrono; + // std::chrono::time_point p1 = std::chrono::system_clock::now(); + // std::chrono::seconds sec(kUtsEpochOffsetFromUnixEpochInSeconds); + // int64 microsecsinceepoch = std::chrono::duration_cast(p1.time_since_epoch()).count(); + // int64 microsec = std::chrono::microseconds(sec).count()*10; + // std::cout << "microseconds since epoch: " << microsecsinceepoch << '\n'; + // std::cout << "microseconds since the Epoch which is January 1, 1 at the start of day in UTC: " + // << microsecsinceepoch - microsec << '\n'; return time_point - ( - duration_cast(system_clock::now().time_since_epoch()) - - seconds(kUtsEpochOffsetFromUnixEpochInSeconds) - ); - } + ( + duration_cast(system_clock::now().time_since_epoch()) - + seconds(kUtsEpochOffsetFromUnixEpochInSeconds) + ); + } }; // Represents Universal Time Scale durations and timestamps which are 64-bit diff --git a/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp b/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp index ed4912b..eabe9c1 100755 --- a/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp +++ b/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp @@ -42,8 +42,8 @@ LIBRARY vrepLib; // the V-REP library that we will dynamically load and bind #define LUA_KUKA_LBR_IIWA_START_COMMAND "simExtKukaLBRiiwaStart" - -std::shared_ptr kukaPluginPG; +/// PG means pointer global +std::shared_ptr kukaVrepPluginPG; std::shared_ptr loggerPG; /// Recording will begin when the simulation starts running, and log files will be saved every time it stops running. bool recordWhileSimulationIsRunningG = false; @@ -75,8 +75,9 @@ void LUA_SIM_EXT_KUKA_LBR_IIWA_START(SLuaCallBack* p) // return Lua Table or arrays containing position, torque, torque minus motor force, timestamp, FRI state try { - if (!kukaPluginPG) { - loggerPG->error("Starting KUKA LBR iiwa plugin connection to Kuka iiwa\n" ); + if (!kukaVrepPluginPG) { + loggerPG->error("simExtKukaLBRiiwaStart: Starting KUKA LBR iiwa plugin connection to Kuka iiwa\n" ); + std::cout<< "Start simExtKukaLBRiiwaStart()..." << std::endl; CLuaFunctionData data; @@ -105,8 +106,7 @@ void LUA_SIM_EXT_KUKA_LBR_IIWA_START(SLuaCallBack* p) std::string KukaMonitorMode (inData->at(14).stringData[0]); std::string IKGroupName (inData->at(15).stringData[0]); - - kukaPluginPG=std::make_shared( + kukaVrepPluginPG = std::make_shared( std::make_tuple( JointHandles , RobotFlangeTipHandle , @@ -126,17 +126,14 @@ void LUA_SIM_EXT_KUKA_LBR_IIWA_START(SLuaCallBack* p) IKGroupName ) ); - kukaPluginPG->construct(); + kukaVrepPluginPG->construct(); } else { /// @todo report an error? // use default params - kukaPluginPG=std::make_shared(); - kukaPluginPG->construct(); + kukaVrepPluginPG=std::make_shared(); + kukaVrepPluginPG->construct(); } - - - } } catch (const boost::exception& e){ @@ -144,19 +141,19 @@ void LUA_SIM_EXT_KUKA_LBR_IIWA_START(SLuaCallBack* p) std::string initerr("v_repExtKukaLBRiiwa plugin encountered the following error and will disable itself:\n" + boost::diagnostic_information(e)); simAddStatusbarMessage( initerr.c_str()); loggerPG->error( initerr ); - kukaPluginPG.reset(); + kukaVrepPluginPG.reset(); } catch (const std::exception& e){ // log the error and print it to the screen, don't release the exception std::string initerr("v_repExtKukaLBRiiwa plugin encountered the following error and will disable itself:\n" + boost::diagnostic_information(e)); simAddStatusbarMessage( initerr.c_str()); loggerPG->error( initerr ); - kukaPluginPG.reset(); + kukaVrepPluginPG.reset(); } catch (...){ // log the error and print it to the screen, don't release the exception std::string initerr("v_repExtKukaLBRiiwa plugin encountered an unknown error and will disable itself. Please debug this issue! file and line:" + std::string(__FILE__) + " " + boost::lexical_cast(__LINE__) + "\n"); simAddStatusbarMessage( initerr.c_str()); loggerPG->error( initerr ); - kukaPluginPG.reset(); + kukaVrepPluginPG.reset(); } } @@ -170,12 +167,12 @@ void LUA_SIM_EXT_KUKA_LBR_IIWA_START_RECORDING(SLuaCallBack *p) { CLuaFunctionData D; bool success = false; - if (kukaPluginPG) + if (kukaVrepPluginPG) { std::string log_message("Starting the recording of KUKAiiwa state data in memory.\n"); simAddStatusbarMessage(log_message.c_str()); loggerPG->info(log_message); - success = kukaPluginPG->start_recording(); + success = kukaVrepPluginPG->start_recording(); } D.pushOutData(CLuaFunctionDataItem(success)); D.writeDataToLua(p); @@ -185,12 +182,12 @@ void LUA_SIM_EXT_KUKA_LBR_IIWA_STOP_RECORDING(SLuaCallBack *p) { CLuaFunctionData D; bool success = false; - if (kukaPluginPG) + if (kukaVrepPluginPG) { std::string log_message("Stoping the recording of KUKAiiwa state data in memory.\n"); simAddStatusbarMessage(log_message.c_str()); loggerPG->info(log_message); - success = kukaPluginPG->stop_recording(); + success = kukaVrepPluginPG->stop_recording(); } D.pushOutData(CLuaFunctionDataItem(success)); D.writeDataToLua(p); @@ -217,12 +214,12 @@ void LUA_SIM_EXT_KUKA_LBR_IIWA_SAVE_RECORDING(SLuaCallBack *p) { std::vector *inData = D.getInDataPtr(); std::string filename_lua_param(inData->at(0).stringData[0]); - if (kukaPluginPG) + if (kukaVrepPluginPG) { std::string log_message("Saving the recording of KUKAiiwa state data in memory.\n"); simAddStatusbarMessage(log_message.c_str()); loggerPG->info(log_message); - success = kukaPluginPG->save_recording(); + success = kukaVrepPluginPG->save_recording(); } D.pushOutData(CLuaFunctionDataItem(success)); D.writeDataToLua(p); @@ -236,12 +233,12 @@ void LUA_SIM_EXT_KUKA_LBR_IIWA_CLEAR_RECORDING(SLuaCallBack *p) { CLuaFunctionData D; bool success = false; - if (kukaPluginPG) + if (kukaVrepPluginPG) { std::string log_message("Clearing the recording of KUKAiiwa state data in memory.\n"); simAddStatusbarMessage(log_message.c_str()); loggerPG->info(log_message); - kukaPluginPG->clear_recording(); + kukaVrepPluginPG->clear_recording(); success = true; } D.pushOutData(CLuaFunctionDataItem(success)); @@ -382,7 +379,7 @@ VREP_DLLEXPORT void v_repEnd() // close out as necessary //////////////////// - kukaPluginPG.reset(); + kukaVrepPluginPG.reset(); unloadVrepLibrary(vrepLib); // release the library } @@ -437,7 +434,7 @@ VREP_DLLEXPORT void* v_repMessage(int message,int* auxiliaryData,void* customDat ///////////// if (simGetSimulationState() != sim_simulation_advancing_abouttostop) //checks if the simulation is still running { - //if(kukaPluginPG) loggerPG->error("current simulation time:" << simGetSimulationTime() << std::endl ); // gets simulation time point + //if(kukaVrepPluginPG) loggerPG->error("current simulation time:" << simGetSimulationTime() << std::endl ); // gets simulation time point } // make sure it is "right" (what does that mean?) @@ -447,31 +444,31 @@ VREP_DLLEXPORT void* v_repMessage(int message,int* auxiliaryData,void* customDat // Use handles that were found at the "start" of this simulation running // next few Lines get the joint angles, torque, etc from the simulation - if (kukaPluginPG)// && kukaPluginPG->allHandlesSet == true // allHandlesSet now handled internally + if (kukaVrepPluginPG)// && kukaVrepPluginPG->allHandlesSet == true // allHandlesSet now handled internally { try { // run one loop synchronizing the arm and plugin - kukaPluginPG->run_one(); + kukaVrepPluginPG->run_one(); } catch (const boost::exception& e){ // log the error and print it to the screen, don't release the exception std::string initerr("v_repExtKukaLBRiiwa plugin encountered the following error and will disable itself:\n" + boost::diagnostic_information(e)); simAddStatusbarMessage( initerr.c_str()); loggerPG->error( initerr ); - kukaPluginPG.reset(); + kukaVrepPluginPG.reset(); } catch (const std::exception& e){ // log the error and print it to the screen, don't release the exception std::string initerr("v_repExtKukaLBRiiwa plugin encountered the following error and will disable itself:\n" + boost::diagnostic_information(e)); simAddStatusbarMessage( initerr.c_str()); loggerPG->error( initerr ); - kukaPluginPG.reset(); + kukaVrepPluginPG.reset(); } catch (...){ // log the error and print it to the screen, don't release the exception std::string initerr("v_repExtKukaLBRiiwa plugin encountered an unknown error and will disable itself. Please debug this issue! file and line:" + std::string(__FILE__) + " " + boost::lexical_cast(__LINE__) + "\n"); simAddStatusbarMessage( initerr.c_str()); loggerPG->error( initerr ); - kukaPluginPG.reset(); + kukaVrepPluginPG.reset(); } } @@ -494,18 +491,18 @@ VREP_DLLEXPORT void* v_repMessage(int message,int* auxiliaryData,void* customDat // try { // loggerPG->error("Starting KUKA LBR iiwa plugin connection to Kuka iiwa\n" ); -// kukaPluginPG = std::make_shared(); -// kukaPluginPG->construct(); -// //kukaPluginPG->run_one(); // for debugging purposes only -// //kukaPluginPG.reset(); // for debugging purposes only +// kukaVrepPluginPG = std::make_shared(); +// kukaVrepPluginPG->construct(); +// //kukaVrepPluginPG->run_one(); // for debugging purposes only +// //kukaVrepPluginPG.reset(); // for debugging purposes only // } catch (boost::exception& e){ // // log the error and print it to the screen, don't release the exception // std::string initerr("v_repExtKukaLBRiiwa plugin initialization error:\n" + boost::diagnostic_information(e)); // simAddStatusbarMessage( initerr.c_str()); // loggerPG->error( initerr ); // } - if(kukaPluginPG && recordWhileSimulationIsRunningG) { - kukaPluginPG->start_recording(); + if(kukaVrepPluginPG && recordWhileSimulationIsRunningG) { + kukaVrepPluginPG->start_recording(); } } @@ -517,12 +514,12 @@ VREP_DLLEXPORT void* v_repMessage(int message,int* auxiliaryData,void* customDat // close out as necessary //////////////////// loggerPG->error("Ending KUKA LBR iiwa plugin connection to Kuka iiwa\n" ); - if(kukaPluginPG && recordWhileSimulationIsRunningG && kukaPluginPG->is_recording()) { - kukaPluginPG->save_recording(); - kukaPluginPG->stop_recording(); + if(kukaVrepPluginPG && recordWhileSimulationIsRunningG && kukaVrepPluginPG->is_recording()) { + kukaVrepPluginPG->save_recording(); + kukaVrepPluginPG->stop_recording(); } - kukaPluginPG->clear_recording(); - kukaPluginPG.reset(); + kukaVrepPluginPG->clear_recording(); + kukaVrepPluginPG.reset(); } diff --git a/test/KukaLBRiiwaVrepPluginTest.cpp b/test/KukaLBRiiwaVrepPluginTest.cpp index 8f2fa57..5594d40 100644 --- a/test/KukaLBRiiwaVrepPluginTest.cpp +++ b/test/KukaLBRiiwaVrepPluginTest.cpp @@ -20,8 +20,8 @@ auto plugin = std::make_shared(); //// uncomment to test error output // try { // BOOST_LOG_TRIVIAL(info) << "Starting KUKA LBR iiwa plugin connection to Kuka iiwa\n"; -// auto kukaPluginPG = std::make_shared(); -// kukaPluginPG->construct(); +// auto kukaVrepPluginPG = std::make_shared(); +// kukaVrepPluginPG->construct(); // } catch (boost::exception& e){ // // log the error and print it to the screen, don't release the exception // BOOST_LOG_TRIVIAL(error) << boost::diagnostic_information(e); diff --git a/test/fusionTrackTest.cpp b/test/fusionTrackTest.cpp index 46b6a01..0b9fd22 100644 --- a/test/fusionTrackTest.cpp +++ b/test/fusionTrackTest.cpp @@ -35,8 +35,8 @@ BOOST_AUTO_TEST_CASE(initialization) //// uncomment to test error output // try { // BOOST_LOG_TRIVIAL(info) << "Starting KUKA LBR iiwa plugin connection to Kuka iiwa\n"; - // auto kukaPluginPG = std::make_shared(); - // kukaPluginPG->construct(); + // auto kukaVrepPluginPG = std::make_shared(); + // kukaVrepPluginPG->construct(); // } catch (boost::exception& e){ // // log the error and print it to the screen, don't release the exception // BOOST_LOG_TRIVIAL(error) << boost::diagnostic_information(e); @@ -93,7 +93,7 @@ BOOST_AUTO_TEST_CASE(runRepeatedly) flatbuffers::Offset LogKUKAiiwaFusionTrack = grl::toFlatBuffer(fbb, states); // Finish a buffer with a given root object fbb.Finish(LogKUKAiiwaFusionTrack); - + // print byte data for debugging: //flatbuffers::SaveFile("test.flik", fbb.GetBufferPointer(), fbb.GetSize()); std::string filename = "test.flik"; From a6ee79eb9648bb8d61650ecd9bc961260e84f447 Mon Sep 17 00:00:00 2001 From: Chunting Date: Fri, 19 Jan 2018 20:32:15 -0500 Subject: [PATCH 035/102] Don't use externalTorque in this version, it only --- include/grl/kuka/KukaDriver.hpp | 1 + include/grl/kuka/KukaFRIdriver.hpp | 3 +-- include/grl/kuka/KukaJAVAdriver.hpp | 4 ++++ include/grl/vrep/VrepRobotArmDriver.hpp | 2 +- src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp | 7 ++----- 5 files changed, 9 insertions(+), 8 deletions(-) diff --git a/include/grl/kuka/KukaDriver.hpp b/include/grl/kuka/KukaDriver.hpp index 461cb77..8eb5522 100644 --- a/include/grl/kuka/KukaDriver.hpp +++ b/include/grl/kuka/KukaDriver.hpp @@ -141,6 +141,7 @@ namespace grl { namespace robot { namespace arm { throw; } } + std::cout<< "End KukaDriver->construct()..." << std::endl; } const Params & getParams(){ diff --git a/include/grl/kuka/KukaFRIdriver.hpp b/include/grl/kuka/KukaFRIdriver.hpp index 32da97e..a871a3a 100644 --- a/include/grl/kuka/KukaFRIdriver.hpp +++ b/include/grl/kuka/KukaFRIdriver.hpp @@ -90,8 +90,7 @@ class KukaFRIdriver : public std::enable_shared_from_this< // keep driver threads from exiting immediately after creation, because they // have work to do! // boost::asio::io_service::work: Constructor notifies the io_service that work is starting. - device_driver_workP_.reset( - new boost::asio::io_service::work(device_driver_io_service)); + device_driver_workP_.reset(new boost::asio::io_service::work(device_driver_io_service)); kukaFRIClientDataDriverP_.reset( new grl::robot::arm::KukaFRIClientDataDriver( diff --git a/include/grl/kuka/KukaJAVAdriver.hpp b/include/grl/kuka/KukaJAVAdriver.hpp index 3c63fef..128f2e8 100644 --- a/include/grl/kuka/KukaJAVAdriver.hpp +++ b/include/grl/kuka/KukaJAVAdriver.hpp @@ -159,6 +159,7 @@ class KukaJAVAdriver : public std::enable_shared_from_this { /// @warning getting the ik group is optional, so it does not throw an exception void construct(Params params) { try { logger_ = spdlog::stdout_logger_mt("console"); } catch (spdlog::spdlog_ex ex) { logger_ = spdlog::get("console"); } + std::cout<< "Start KukaJAVAdriver->construct()..." << std::endl; params_ = params; try { @@ -201,6 +202,7 @@ class KukaJAVAdriver : public std::enable_shared_from_this { // set arm to StartArm mode on initalization //set(grl::flatbuffer::ArmState::StartArm); set(grl::flatbuffer::ArmState::MoveArmJointServo); + std::cout<< "End KukaJAVAdriver->construct()..." << std::endl; } catch( boost::exception &e) { e << errmsg_info("KukaLBRiiwaRosPlugin: Unable to connect to UDP Socket from {}{}{}" + std::get (params_) + " to " + @@ -257,6 +259,8 @@ class KukaJAVAdriver : public std::enable_shared_from_this { case flatbuffer::ArmState::MoveArmJointServo: { /// @todo when new + /// armState_ isn't assigned with any values correctly. + auto armPositionBuffer = fbbP->CreateVector(armState_.commandedPosition_goal.data(),armState_.commandedPosition_goal.size()); auto commandedTorque = fbbP->CreateVector(armState_.commandedTorque.data(),armState_.commandedTorque.size()); auto goalJointState = grl::flatbuffer::CreateJointState(*fbbP,armPositionBuffer,0/*no velocity*/,0/*no acceleration*/,commandedTorque); diff --git a/include/grl/vrep/VrepRobotArmDriver.hpp b/include/grl/vrep/VrepRobotArmDriver.hpp index 6e6451d..bb269a8 100644 --- a/include/grl/vrep/VrepRobotArmDriver.hpp +++ b/include/grl/vrep/VrepRobotArmDriver.hpp @@ -319,7 +319,7 @@ bool setState(State& state) { else { - for (int i=0 ; i < 7 ; i++) { + for (int i=0 ; i < externalJointForce.size(); i++) { std::string torqueString = boost::lexical_cast(externalJointForce[i]); char * externalTorqueBytes = new char[torqueString.length()+1]; std::strcpy(externalTorqueBytes, torqueString.c_str()); diff --git a/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp b/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp index eabe9c1..1d1a20c 100755 --- a/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp +++ b/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp @@ -75,9 +75,9 @@ void LUA_SIM_EXT_KUKA_LBR_IIWA_START(SLuaCallBack* p) // return Lua Table or arrays containing position, torque, torque minus motor force, timestamp, FRI state try { + /// if (!kukaVrepPluginPG) { - loggerPG->error("simExtKukaLBRiiwaStart: Starting KUKA LBR iiwa plugin connection to Kuka iiwa\n" ); - std::cout<< "Start simExtKukaLBRiiwaStart()..." << std::endl; + loggerPG->error("simExtKukaLBRiiwaStart: Starting KUKA LBR iiwa plugin connection to Kuka iiwa...\n kukaVrepPluginPG hasn't been initialized...\n" ); CLuaFunctionData data; @@ -424,9 +424,6 @@ VREP_DLLEXPORT void* v_repMessage(int message,int* auxiliaryData,void* customDat { // we actualize plugin objects for changes in the scene refreshDlgFlag=true; // always a good idea to trigger a refresh of this plugin's dialog here } - - - //... ////////////// // PUT MAIN CODE HERE From f7522025555293735187416c9bda09b9cec81d1a Mon Sep 17 00:00:00 2001 From: Chunting Date: Mon, 22 Jan 2018 19:08:48 -0500 Subject: [PATCH 036/102] In Vrep, log data from real robot. --- include/grl/flatbuffer/HelperToFlatbuffer.hpp | 8 +- include/grl/flatbuffer/flatbuffer.hpp | 301 +++++++++--------- include/grl/kuka/KukaDriver.hpp | 2 - include/grl/kuka/KukaFRIClientDataDriver.hpp | 1 - include/grl/kuka/KukaFRIdriver.hpp | 79 +++-- include/grl/kuka/KukaToFlatbuffer.hpp | 17 +- include/grl/sensor/FusionTrack.hpp | 9 +- include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp | 15 +- src/lua/robone.lua | 7 +- .../v_repExtKukaLBRiiwa.cpp | 14 +- test/KukaFRITest.cpp | 1 + 11 files changed, 237 insertions(+), 217 deletions(-) diff --git a/include/grl/flatbuffer/HelperToFlatbuffer.hpp b/include/grl/flatbuffer/HelperToFlatbuffer.hpp index 9d96c53..0fdf307 100644 --- a/include/grl/flatbuffer/HelperToFlatbuffer.hpp +++ b/include/grl/flatbuffer/HelperToFlatbuffer.hpp @@ -1,7 +1,6 @@ #ifndef GRL_HELPER_TO_FLATBUFFER #define GRL_HELPER_TO_FLATBUFFER - #include "grl/flatbuffer/Time_generated.h" #include "grl/flatbuffer/Geometry_generated.h" #include "grl/TimeEvent.hpp" @@ -13,9 +12,11 @@ #include #include + namespace grl { + /// Helper function for both KUKA and FusionTrack grl::flatbuffer::Vector3d toFlatBuffer(const Eigen::Vector3d &pt) { @@ -53,10 +54,6 @@ toFlatBuffer(flatbuffers::FlatBufferBuilder &fbb, const grl::TimeEvent &timeStamp) { flatbuffers::Offset event_name = fbb.CreateString(const_cast(timeStamp.event_name.begin()), stringLength(timeStamp.event_name)); - for(int i = 0; i /* defines FILENAME_MAX */ +// #define WINDOWS /* uncomment this line to use it for windows.*/ +#ifdef WINDOWS +#include +#define GetCurrentDir _getcwd +#else +#include +#define GetCurrentDir getcwd +#endif + #include #include @@ -8,169 +18,172 @@ namespace grl { -// loads a json flatbuffer from a file -bool LoadJSONFlatbuffer(flatbuffers::Parser& parser, std::string schemaPath, std::string jsonPath, std::string include_directory, bool binary = false) -{ - // source: https://github.com/google/flatbuffers/blob/master/samples/sample_text.cpp - - // load FlatBuffer schema (.fbs) and JSON from disk - std::string schemafile; - std::string jsonfile; - bool ok = flatbuffers::LoadFile(schemaPath.c_str(), binary, &schemafile) && - flatbuffers::LoadFile(jsonPath.c_str(), binary, &jsonfile); - if (!ok) { - printf("couldn't load files!\n"); - return nullptr; + std::string GetCurrentWorkingDir( void ) { + char buff[FILENAME_MAX]; + GetCurrentDir( buff, FILENAME_MAX ); + std::string current_working_dir(buff); + return current_working_dir; } - // parse schema first, so we can use it to parse the data after - const char *include_directories[] = { include_directory.c_str(), "flatbuffer", nullptr }; - ok = parser.Parse(schemafile.c_str(), include_directories) && - parser.Parse(jsonfile.c_str(), include_directories); - assert(ok); - return ok; -} - -/// @brief Loads a flatbuffers schema file from disk into a flatbuffers parser. -/// -/// @param include_paths JSON ONLY. This is the list of paths describing where to find the -/// flatbuffers schema files (.fbs) required to write the json file. -/// In particular, LogKUKAiiwaFusionTrack.fbs is requred. If the empty -/// string or an empty list is specified, the current working directory -/// will be checked for LogKUKAiiwaFusionTrack.fbs and the other files -/// it depends on. -/// @param read_binary_schema JSON only. The LogKUKAiiwaFusionTrack.fbs and other schemea files -/// can be saved in ascii plaintext format (the default) and in binary format. -/// Set this option to true if you expect to load the schema files in binary format. -/// @param write_binary_schema If "binary_stream" is false data is written using ifstream's -/// text mode, otherwise data is written with no transcoding. -bool ParseSchemaFile( - flatbuffers::Parser& parser, - std::string fbs_filename, - std::vector includePaths = std::vector(), - bool read_binary_schema=false) -{ - - bool loadfbsfile_ok = true; - std::string schemafile; - if(includePaths.empty()) + // loads a json flatbuffer from a file + bool LoadJSONFlatbuffer(flatbuffers::Parser& parser, std::string schemaPath, std::string jsonPath, std::string include_directory, bool binary = false) { + // source: https://github.com/google/flatbuffers/blob/master/samples/sample_text.cpp + + // load FlatBuffer schema (.fbs) and JSON from disk + std::string schemafile; + std::string jsonfile; + bool ok = flatbuffers::LoadFile(schemaPath.c_str(), binary, &schemafile) && + flatbuffers::LoadFile(jsonPath.c_str(), binary, &jsonfile); + if (!ok) { + printf("couldn't load files!\n"); + return nullptr; + } - std::string current_working_dir; - char buff[2048]; - /// Get the current working directory - getcwd(buff, 2048); - current_working_dir = std::string(buff); - std::cout << "The current working dir: " << current_working_dir << std::endl; - includePaths.push_back(current_working_dir); + // parse schema first, so we can use it to parse the data after + const char *include_directories[] = { include_directory.c_str(), "flatbuffer", nullptr }; + ok = parser.Parse(schemafile.c_str(), include_directories) && + parser.Parse(jsonfile.c_str(), include_directories); + assert(ok); + return ok; } - std::string fbs_fullpath; - // check if the user passed a full path to the fbs file - if(flatbuffers::FileExists(fbs_filename.c_str())){ - fbs_fullpath = fbs_filename; - } - else + /// @brief Loads a flatbuffers schema file from disk into a flatbuffers parser. + /// + /// @param include_paths JSON ONLY. This is the list of paths describing where to find the + /// flatbuffers schema files (.fbs) required to write the json file. + /// In particular, LogKUKAiiwaFusionTrack.fbs is requred. If the empty + /// string or an empty list is specified, the current working directory + /// will be checked for LogKUKAiiwaFusionTrack.fbs and the other files + /// it depends on. + /// @param read_binary_schema JSON only. The LogKUKAiiwaFusionTrack.fbs and other schemea files + /// can be saved in ascii plaintext format (the default) and in binary format. + /// Set this option to true if you expect to load the schema files in binary format. + /// @param write_binary_schema If "binary_stream" is false data is written using ifstream's + /// text mode, otherwise data is written with no transcoding. + bool ParseSchemaFile( + flatbuffers::Parser& parser, + std::string fbs_filename, + std::vector includePaths = std::vector(), + bool read_binary_schema=false) { - std::cout << "a full path wasn't passed, so check all the include paths" << std::endl; - for(auto includePath : includePaths) + + bool loadfbsfile_ok = true; + std::string schemafile; + if(includePaths.empty()) { - std::string fbs_trypath = flatbuffers::ConCatPathFileName(includePath, fbs_filename); - if(flatbuffers::FileExists(fbs_trypath.c_str())) - { - fbs_fullpath = fbs_trypath; - // we found it! no need to keep looping - break; - } + + std::string current_working_dir = GetCurrentWorkingDir(); + std::cout << "The current working dir: " << current_working_dir << std::endl; + includePaths.push_back(current_working_dir); } - } - // if it is empty none of the files actually existed, return false - if(fbs_fullpath.empty()) return false; - if(!flatbuffers::LoadFile(fbs_filename.c_str(), read_binary_schema, &schemafile)) - { - // something is wrong - return false; - } + std::string fbs_fullpath; + // check if the user passed a full path to the fbs file + if(flatbuffers::FileExists(fbs_filename.c_str())){ + fbs_fullpath = fbs_filename; + } + else + { + std::cout << "a full path wasn't passed, so check all the include paths" << std::endl; + for(auto includePath : includePaths) + { + std::string fbs_trypath = flatbuffers::ConCatPathFileName(includePath, fbs_filename); + if(flatbuffers::FileExists(fbs_trypath.c_str())) + { + fbs_fullpath = fbs_trypath; + // we found it! no need to keep looping + break; + } + } + } - // parse fbs schema, so we can use it to parse the data after - // create a list of char* pointers so we can call Parse - std::vector include_directories; - for(int i = 0; i < includePaths.size(); i++){ - include_directories.push_back(includePaths[i].c_str()); - } - include_directories.push_back(nullptr); - - return parser.Parse(schemafile.c_str(), &include_directories[0]); -} - - -/// -/// Save data flatbuffers::DetachedBuffer db into a file binary_file_path, -/// returning true if successful, false otherwise. Prefer reading and -/// writing binary files over json files, because they are much more efficient. -/// -/// @param db flatbuffers detached buffer object to write to disk -/// @param binary_file_path full file path to write the binary log file. -/// Empty string indicates this file should not be saved. -/// Please prefer saving binary files and use the file extension .flik. -/// @param fbs_file_path JSON Only. The file name of the flatbuffers schema definition file (.fbs). -/// If this is empty the JSON file will not be saved. -/// @param json_file_path full file path to write the json file, -/// Only write this file for debugging purposes, it is very expensive. -/// The default empty string indicates this file should not be saved. -/// Please use the file extension .json if saving this file. -/// @param include_paths JSON ONLY. This is the list of paths describing where to find the -/// flatbuffers schema files (.fbs) required to write the json file. -/// In particular, LogKUKAiiwaFusionTrack.fbs is requred. If the empty -/// string or an empty list is specified, the current working directory -/// will be checked for LogKUKAiiwaFusionTrack.fbs and the other files -/// it depends on. -/// @param read_binary_schema JSON only. The LogKUKAiiwaFusionTrack.fbs and other schemea files -/// can be saved in ascii plaintext format (the default) and in binary format. -/// Set this option to true if you expect to load the schema files in binary format. -/// @param write_binary_schema If "binary_stream" is false data is written using ifstream's -/// text mode, otherwise data is written with no transcoding. -bool SaveFlatBufferFile( - const uint8_t* buffer, - std::size_t size, - std::string binary_file_path, - std::string fbs_filename = std::string(), - std::string json_file_path = std::string(), - std::vector includePaths = std::vector(), - bool read_binary_schema=false, - bool write_binary_stream=true) -{ - bool ok = true; - ///////////////////////////////////// - /// Saving BINARY version of file /// - ///////////////////////////////////// - if(!binary_file_path.empty()) - { - ok = ok && flatbuffers::SaveFile(binary_file_path.c_str(), reinterpret_cast(buffer), size, write_binary_stream); + // if it is empty none of the files actually existed, return false + if(fbs_fullpath.empty()) return false; + if(!flatbuffers::LoadFile(fbs_filename.c_str(), read_binary_schema, &schemafile)) + { + // something is wrong + return false; + } + + // parse fbs schema, so we can use it to parse the data after + // create a list of char* pointers so we can call Parse + std::vector include_directories; + for(int i = 0; i < includePaths.size(); i++){ + include_directories.push_back(includePaths[i].c_str()); + } + include_directories.push_back(nullptr); + + return parser.Parse(schemafile.c_str(), &include_directories[0]); } - //////////////////////////////////////////// - /// Load fbs file and generate json file /// - //////////////////////////////////////////// - if(!json_file_path.empty() && !fbs_filename.empty()) + /// + /// Save data flatbuffers::DetachedBuffer db into a file binary_file_path, + /// returning true if successful, false otherwise. Prefer reading and + /// writing binary files over json files, because they are much more efficient. + /// + /// @param db flatbuffers detached buffer object to write to disk + /// @param binary_file_path full file path to write the binary log file. + /// Empty string indicates this file should not be saved. + /// Please prefer saving binary files and use the file extension .flik. + /// @param fbs_file_path JSON Only. The file name of the flatbuffers schema definition file (.fbs). + /// If this is empty the JSON file will not be saved. + /// @param json_file_path full file path to write the json file, + /// Only write this file for debugging purposes, it is very expensive. + /// The default empty string indicates this file should not be saved. + /// Please use the file extension .json if saving this file. + /// @param include_paths JSON ONLY. This is the list of paths describing where to find the + /// flatbuffers schema files (.fbs) required to write the json file. + /// In particular, LogKUKAiiwaFusionTrack.fbs is requred. If the empty + /// string or an empty list is specified, the current working directory + /// will be checked for LogKUKAiiwaFusionTrack.fbs and the other files + /// it depends on. + /// @param read_binary_schema JSON only. The LogKUKAiiwaFusionTrack.fbs and other schemea files + /// can be saved in ascii plaintext format (the default) and in binary format. + /// Set this option to true if you expect to load the schema files in binary format. + /// @param write_binary_schema If "binary_stream" is false data is written using ifstream's + /// text mode, otherwise data is written with no transcoding. + bool SaveFlatBufferFile( + const uint8_t* buffer, + std::size_t size, + std::string binary_file_path, + std::string fbs_filename = std::string(), + std::string json_file_path = std::string(), + std::vector includePaths = std::vector(), + bool read_binary_schema=false, + bool write_binary_stream=true) { - flatbuffers::Parser parser; + bool ok = true; + ///////////////////////////////////// + /// Saving BINARY version of file /// + ///////////////////////////////////// + if(!binary_file_path.empty()) + { + ok = ok && flatbuffers::SaveFile(binary_file_path.c_str(), reinterpret_cast(buffer), size, write_binary_stream); + } + + //////////////////////////////////////////// + /// Load fbs file and generate json file /// + //////////////////////////////////////////// - ok = ok && ParseSchemaFile(parser, fbs_filename, includePaths, read_binary_schema); - std::string jsongen; - // now generate text from the flatbuffer binary + if(!json_file_path.empty() && !fbs_filename.empty()) + { + flatbuffers::Parser parser; + + ok = ok && ParseSchemaFile(parser, fbs_filename, includePaths, read_binary_schema); + std::string jsongen; + // now generate text from the flatbuffer binary - ok = ok && GenerateText(parser, buffer, &jsongen); - // Write the data get from flatbuffer binary to json file on disk. + ok = ok && GenerateText(parser, buffer, &jsongen); + // Write the data get from flatbuffer binary to json file on disk. - std::ofstream out(json_file_path); - out << jsongen.c_str(); - out.close(); + std::ofstream out(json_file_path); + out << jsongen.c_str(); + out.close(); + } + return ok; } - return ok; -} } // namespace grl #endif // GRL_FLATBUFFER_HPP \ No newline at end of file diff --git a/include/grl/kuka/KukaDriver.hpp b/include/grl/kuka/KukaDriver.hpp index 8eb5522..d3ce443 100644 --- a/include/grl/kuka/KukaDriver.hpp +++ b/include/grl/kuka/KukaDriver.hpp @@ -166,7 +166,6 @@ namespace grl { namespace robot { namespace arm { // @todo CHECK FOR REAL DATA BEFORE SENDING COMMANDS //if(!m_haveReceivedRealDataCount) return; bool haveNewData = false; - std::cout<< "Start KukaDriver->run_one()..." << std::endl; /// @todo make this handled by template driver implementations/extensions if(JAVAdriverP_.get() != nullptr) @@ -219,7 +218,6 @@ namespace grl { namespace robot { namespace arm { if( boost::iequals(std::get(params_),std::string("FRI"))) { FRIdriverP_->set(armState_.commandedPosition,revolute_joint_angle_open_chain_command_tag()); - std::cout << "armState_.commandedPosition in KukaDriver(FRI) run_one():" << armState_.commandedPosition << "\n"; } haveNewData = FRIdriverP_->run_one(); diff --git a/include/grl/kuka/KukaFRIClientDataDriver.hpp b/include/grl/kuka/KukaFRIClientDataDriver.hpp index 9eeaa71..d8a9450 100644 --- a/include/grl/kuka/KukaFRIClientDataDriver.hpp +++ b/include/grl/kuka/KukaFRIClientDataDriver.hpp @@ -414,7 +414,6 @@ void update_state(boost::asio::ip::udp::socket &socket, // get a local clock timestamp, then the latest frame from the device, then another timestamp timeEvent.local_request_time = cartographer::common::UniversalTimeScaleClock::now(); - std::cout<< timeEvent.local_request_time << std::endl; receive_bytes_transferred = socket.receive_from( boost::asio::buffer(friData.receiveBuffer, KUKA::FRI::FRI_MONITOR_MSG_MAX_SIZE), diff --git a/include/grl/kuka/KukaFRIdriver.hpp b/include/grl/kuka/KukaFRIdriver.hpp index a871a3a..01f4c9f 100644 --- a/include/grl/kuka/KukaFRIdriver.hpp +++ b/include/grl/kuka/KukaFRIdriver.hpp @@ -84,8 +84,6 @@ class KukaFRIdriver : public std::enable_shared_from_this< /// @warning getting the ik group is optional, so it does not throw an /// exception void construct(Params params) { - std::cout<< "Start KukaFRIdriver->construct()..." << std::endl; - params_ = params; // keep driver threads from exiting immediately after creation, because they // have work to do! @@ -110,7 +108,6 @@ class KukaFRIdriver : public std::enable_shared_from_this< #ifdef HAVE_spdlog loggerP = spdlog::stdout_logger_mt("logs/kukaiiwa_logger.txt"); #endif // HAVE_spdlog - std::cout<< "End KukaFRIdriver->construct()..." << std::endl; } const Params &getParams() { return params_; } @@ -198,7 +195,7 @@ class KukaFRIdriver : public std::enable_shared_from_this< * */ bool run_one() { - std::cout<< "Start KukaFRIdriver->run_one()..." << std::endl; + // note: this one sends *and* receives the joint data! BOOST_VERIFY(kukaFRIClientDataDriverP_.get() != nullptr); /// @todo use runtime calculation of NUM_JOINTS instead of constant @@ -307,17 +304,17 @@ class KukaFRIdriver : public std::enable_shared_from_this< oneKUKAiiwaStateBuffer(); armState.time_event_stamp = time_event_stamp; saveToDisk(); - // m_driverThread.reset(new std::thread(&KukaFRIdriver::save_recording)); + } else { m_attemptedCommunicationConsecutiveFailureCount++; - std::cerr << "No new FRI data available, is an FRI application running " - "on the Kuka arm? \n Total sucessful transfers: " - << this->m_haveReceivedRealDataCount - << "\n Total attempts: " << m_attemptedCommunicationCount - << "\n Consecutive Failures: " - << m_attemptedCommunicationConsecutiveFailureCount - << "\n Consecutive Successes: " - << m_attemptedCommunicationConsecutiveSuccessCount << "\n"; + // std::cerr << "No new FRI data available, is an FRI application running " + // "on the Kuka arm? \n Total sucessful transfers: " + // << this->m_haveReceivedRealDataCount + // << "\n Total attempts: " << m_attemptedCommunicationCount + // << "\n Consecutive Failures: " + // << m_attemptedCommunicationConsecutiveFailureCount + // << "\n Consecutive Successes: " + // << m_attemptedCommunicationConsecutiveSuccessCount << "\n"; m_attemptedCommunicationConsecutiveSuccessCount = 0; /// @todo TODO(ahundt) Add time information from update_state call here for debugging purposes /// @todo TODO(ahundt) should the results of getlatest state even be possible to call @@ -495,7 +492,6 @@ class KukaFRIdriver : public std::enable_shared_from_this< int16_t portOnController = std::stoi(std::string(std::get(params_))); std::string basename = RobotName; //std::get<0>(params); - bool setArmConfiguration_ = true; // set the arm config first time bool max_control_force_stop_ = false; std::vector joint_stiffness_(KUKA::LBRState::NUM_DOF, 0); std::vector joint_damping_(KUKA::LBRState::NUM_DOF, 0); @@ -556,21 +552,33 @@ class KukaFRIdriver : public std::enable_shared_from_this< std::string local_clock_id_str = s_event_name + "/control_computer/clock/steady"; TimeEvent::UnsignedCharArray event_name; - s_event_name.copy(event_name.begin(), std::min(s_event_name.size(), event_name.size())); + std::size_t length = s_event_name.copy(event_name.begin(), std::min(s_event_name.size(), event_name.size())); + event_name[length] = '\0'; time_event_stamp.event_name = event_name; + TimeEvent::UnsignedCharArray device_clock_id; - device_clock_id_str.copy(device_clock_id.begin(), std::min(device_clock_id_str.size(), device_clock_id.size())); + length = device_clock_id_str.copy(device_clock_id.begin(), std::min(device_clock_id_str.size(), device_clock_id.size())); + device_clock_id[length] = '\0'; time_event_stamp.device_clock_id = device_clock_id; + TimeEvent::UnsignedCharArray local_clock_name_arr; - local_clock_id_str.copy(local_clock_name_arr.begin(), std::min(local_clock_id_str.size(),local_clock_name_arr.size())); + length = local_clock_id_str.copy(local_clock_name_arr.begin(), std::min(local_clock_id_str.size(),local_clock_name_arr.size())); + local_clock_name_arr[length] = '\0'; time_event_stamp.local_clock_id = local_clock_name_arr; time_event_stamp.device_time = FRITimeStampToCommonTime(messageMonitorData.timestamp); + + //std::cout<< time_event_stamp.event_name << std::endl << time_event_stamp.device_clock_id << std::endl << time_event_stamp.local_clock_id < kukaiiwaArmConfiguration = 0; + flatbuffers::Offset kukaiiwaArmConfiguration; flatbuffers::Offset friMessageLog = grl::toFlatBuffer( *m_logFileBufferBuilderP, @@ -674,7 +682,8 @@ class KukaFRIdriver : public std::enable_shared_from_this< // Calculate the data later. // Cartesian pose of the flange relative to the base of the arm // grl::flatbuffer::Pose cartesianFlangePose = grl::flatbuffer::Pose(grl::flatbuffer::Vector3d(0, 0, 0), grl::flatbuffer::Quaternion(0,0,0,0)); - grl::flatbuffer::Pose cartesianFlangePose{}; + grl::flatbuffer::Pose cartesianFlangePose = grl::flatbuffer::Pose(); + // flatbuffers::Offset kukaiiwaMonitorState = 0; flatbuffers::Offset kukaiiwaMonitorState = grl::toFlatBuffer( *m_logFileBufferBuilderP, jointStatetab, // flatbuffers::Offset &measuredState, @@ -705,9 +714,9 @@ class KukaFRIdriver : public std::enable_shared_from_this< source, // duration, armState.time_event_stamp, - true, controlState, - true, kukaiiwaArmConfiguration, - true, kukaiiwaMonitorState, + false, controlState, // if false, then don't record the value + false, kukaiiwaArmConfiguration, + false, kukaiiwaMonitorState, false, monitorConfig, friMessageLog); m_KUKAiiwaStateBufferP->push_back(KUKAiiwaState); @@ -717,12 +726,11 @@ class KukaFRIdriver : public std::enable_shared_from_this< bool save_recording(std::string filename = std::string()) { - - loggerP->info("Here is in save_recording/n "); + loggerP->info("Here is in save_recording/n "); if(filename.empty()) { - /// TODO(ahundt) Saving the file twice in one second will overwrite!!!! - filename = current_date_and_time_string() + "_Kukaiiwa.iiwa"; + /// TODO(ahundt) Saving the file twice in one second will overwrite!!!! + filename = current_date_and_time_string() + "_Kukaiiwa.iiwa"; } #ifdef HAVE_spdlog loggerP->info("Save Recording as: ", filename); @@ -730,18 +738,21 @@ class KukaFRIdriver : public std::enable_shared_from_this< std::cout << "Save Recording as: " << filename << std::endl; #endif // HAVE_spdlog auto saveLambdaFunction = [ - save_fbbP = std::move(m_logFileBufferBuilderP) - ,save_KUKAiiwaBufferP = std::move(m_KUKAiiwaStateBufferP) - ,filename + save_fbbP = std::move(m_logFileBufferBuilderP) + ,save_KUKAiiwaBufferP = std::move(m_KUKAiiwaStateBufferP) + ,filename #ifdef HAVE_spdlog - ,lambdaLoggerP = loggerP + ,lambdaLoggerP = loggerP #endif // HAVE_spdlog ]() mutable { + std::cout<< "Save recording data from KukaLBRiiwa to disk..." << std::endl; + std::string currentWorkingDir = grl::GetCurrentWorkingDir(); + std::cout<< currentWorkingDir<<"/"<(save_fbbP->GetBufferPointer()), save_fbbP->GetSize(), write_binary_stream); - assert(success); + // assert(success); /// TODO(ahFusionTrackLogAndTrackundt) replace cout with proper spdlog and vrep banner notification #ifdef HAVE_spdlog lambdaLoggerP->info("filename: ", filename, " verifier success: ", success); @@ -750,7 +761,7 @@ class KukaFRIdriver : public std::enable_shared_from_this< #endif // HAVE_spdlog }; - // save the recording to a file in a separate thread, memory will be freed up when file finishes saving + // save the recording to a file in a separate thread, memory will be freed up when file finishes saving std::shared_ptr saveLogThread(std::make_shared(saveLambdaFunction)); m_saveRecordingThreads.push_back(saveLogThread); // flatbuffersbuilder does not yet exist @@ -798,7 +809,7 @@ void saveToDisk() { const std::size_t MegaByte = 1024*1024; // If we write too large a flatbuffer - const std::size_t single_buffer_limit_bytes = 0.1*MegaByte; + const std::size_t single_buffer_limit_bytes = 1*MegaByte; // run the primary update loop in a separate thread bool saveFileNow = false; @@ -808,11 +819,11 @@ void saveToDisk() { // There is a flatbuffers file size limit of 2GB, but we use a conservative 512MB int buffsize = m_logFileBufferBuilderP->GetSize(); - std::cout << "Buffersize:" << buffsize << std::endl; if( buffsize > single_buffer_limit_bytes) { // save the file if we are over the limit saveFileNow = true; + std::cout << "Buffersize:" << buffsize << std::endl; } } // end recording steps /// TODO(ahundt) Let the user specify the filenames, or provide a way to check the flatbuffer size and know single_buffer_limit_bytes. diff --git a/include/grl/kuka/KukaToFlatbuffer.hpp b/include/grl/kuka/KukaToFlatbuffer.hpp index a33152c..9a5df8d 100644 --- a/include/grl/kuka/KukaToFlatbuffer.hpp +++ b/include/grl/kuka/KukaToFlatbuffer.hpp @@ -642,20 +642,11 @@ flatbuffers::Offset toFlatBuffer( // get measured joint torque grl::robot::arm::copy(friMonitoringMessage, std::back_inserter(data), grl::revolute_joint_angle_open_chain_command_tag()); flatbuffers::Offset> _commandedJointPosition = fbb.CreateVector(data); - std::cout<<"Command Joint Position:"; - for (int i=0; i<7; i++) { - std::cout<> _commandedTorque = fbb.CreateVector(data); - std::cout<<"Command Joint TOrque:"; - for (int i=0; i<7; i++) { - std::cout< toFlatBuffer( fbb.CreateString(source), _timeEvent, setArmControlState, - armControlState, + setArmControlState? armControlState:0, setArmConfiguration, - armConfiguration, + setArmConfiguration? armConfiguration:0, hasMonitorState, - monitorState, + hasMonitorState ? monitorState:0, hasMonitorConfig, monitorConfig, FRIMessage diff --git a/include/grl/sensor/FusionTrack.hpp b/include/grl/sensor/FusionTrack.hpp index 0407a2f..44755f3 100644 --- a/include/grl/sensor/FusionTrack.hpp +++ b/include/grl/sensor/FusionTrack.hpp @@ -663,15 +663,18 @@ class FusionTrack std::string s_event_name = ss_event_name.str(); std::string device_clock_id_str = s_event_name + m_params.deviceClockID; TimeEvent::UnsignedCharArray event_name; - s_event_name.copy(event_name.begin(), std::min(event_name.size(), s_event_name.size())); + std::size_t length = s_event_name.copy(event_name.begin(), std::min(event_name.size(), s_event_name.size())); + event_name[length] = '\0'; m_event_names.push_back(event_name); TimeEvent::UnsignedCharArray device_clock_id; - device_clock_id_str.copy(device_clock_id.begin(), std::min(device_clock_id_str.size(), device_clock_id.size())); + length = device_clock_id_str.copy(device_clock_id.begin(), std::min(device_clock_id_str.size(), device_clock_id.size())); + device_clock_id[length] = '\0'; m_device_clock_ids.push_back(device_clock_id); TimeEvent::UnsignedCharArray local_clock_name_arr; - m_params.localClockID.copy(local_clock_name_arr.begin(), std::min(local_clock_name_arr.size(), m_params.localClockID.size())); + length = m_params.localClockID.copy(local_clock_name_arr.begin(), std::min(local_clock_name_arr.size(), m_params.localClockID.size())); + local_clock_name_arr[length] = '\0'; m_local_clock_ids.push_back(local_clock_name_arr); } diff --git a/include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp b/include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp index e3c7c9b..bcbabd4 100644 --- a/include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp +++ b/include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp @@ -184,6 +184,7 @@ class KukaVrepPlugin : public std::enable_shared_from_this { if(kukaDriverP_.get() != nullptr) { return kukaDriverP_->start_recording(); } + std::cout << "kukaDriverP_ is nullptr..." << std::endl; return false; } /// stop recording the kuka state data in memory @@ -218,12 +219,12 @@ class KukaVrepPlugin : public std::enable_shared_from_this { void run_one(){ - if(!allHandlesSet) BOOST_THROW_EXCEPTION(std::runtime_error("KukaVrepPlugin: Handles have not been initialized, cannot run updates.")); - getRealKukaAngles(); - bool isError = getStateFromVrep(); // true if there is an error - allHandlesSet = !isError; - /// @todo re-enable simulation feedback based on actual kuka state - syncVrepAndKuka(); + if(!allHandlesSet) BOOST_THROW_EXCEPTION(std::runtime_error("KukaVrepPlugin: Handles have not been initialized, cannot run updates.")); + getRealKukaAngles(); + bool isError = getStateFromVrep(); // true if there is an error + allHandlesSet = !isError; + /// @todo re-enable simulation feedback based on actual kuka state + syncVrepAndKuka(); } @@ -276,7 +277,7 @@ class KukaVrepPlugin : public std::enable_shared_from_this { } void syncVrepAndKuka(){ - bool debug = true; + bool debug = false; if(!allHandlesSet || !m_haveReceivedRealData) return; /// @todo make this handled by template driver implementations/extensions diff --git a/src/lua/robone.lua b/src/lua/robone.lua index bbdd592..dd8271e 100644 --- a/src/lua/robone.lua +++ b/src/lua/robone.lua @@ -5,6 +5,9 @@ --- require "grl" --- 3) call your function! ex: grl.isModuleLoaded('') +--- LRB_iiwa_14_R820#0 represents the real robot; +--- LRB_iiwa_14_R820 represents the robot for simulation + robone = {} require "grl" @@ -434,7 +437,7 @@ robone.startRealArmDriverScript=function() "IK_Group1_iiwa" -- IKGroupName ) - simExtKukaLBRiiwaRecordWhileSimulationIsRunning(false) + simExtKukaLBRiiwaRecordWhileSimulationIsRunning(true) else simDisplayDialog('Error','KukaLBRiiwa plugin was not found. (v_repExtKukaLBRiiwa.dll)&&nSimulation will run without hardware',sim_dlgstyle_ok,true,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1}) end @@ -472,7 +475,7 @@ robone.configureOpticalTracker=function() -------------------------------------------------- -- Move the Tracker -- true enables moving the tracker, false disables it - moveTracker = true + moveTracker = false if (moveTracker) then simAddStatusbarMessage('robone.configureOpticalTracker() + v_repExtAtracsysFusionTrackVrepPlugin: Moving Optical tracker position relative to marker on robot end effector.') simExtAtracsysFusionTrackClearObjects() diff --git a/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp b/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp index 1d1a20c..8c0e4e1 100755 --- a/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp +++ b/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp @@ -84,7 +84,7 @@ void LUA_SIM_EXT_KUKA_LBR_IIWA_START(SLuaCallBack* p) if (data.readDataFromLua(p,inArgs_KUKA_LBR_IIWA_START,inArgs_KUKA_LBR_IIWA_START[0],LUA_KUKA_LBR_IIWA_START_COMMAND)) { - std::vector* inData=data.getInDataPtr(); + std::vector* inData = data.getInDataPtr(); std::vector JointHandles; for (size_t i=0;iat(0).stringData.size();i++) { @@ -270,10 +270,14 @@ void LUA_SIM_EXT_KUKA_LBR_IIWA_RECORD_WHILE_SIMULATION_IS_RUNNING(SLuaCallBack * { std::vector *inData = D.getInDataPtr(); recordWhileSimulationIsRunningG = inData->at(0).boolData[0]; - - std::string log_message = std::string("simExtKukaLBRiiwaRecordWhileSimulationIsRunning: ") + boost::lexical_cast(recordWhileSimulationIsRunningG); - simAddStatusbarMessage(log_message.c_str()); - loggerPG->info(log_message); + std::cout << "Start recording while simulation is running..." << recordWhileSimulationIsRunningG << std::endl; + if (kukaVrepPluginPG && recordWhileSimulationIsRunningG) + { + std::string log_message = std::string("simExtKukaLBRiiwaRecordWhileSimulationIsRunning: ") + boost::lexical_cast(recordWhileSimulationIsRunningG); + simAddStatusbarMessage(log_message.c_str()); + loggerPG->info(log_message); + kukaVrepPluginPG->start_recording(); + } D.pushOutData(CLuaFunctionDataItem(success)); D.writeDataToLua(p); diff --git a/test/KukaFRITest.cpp b/test/KukaFRITest.cpp index 721e68b..0ce09e6 100644 --- a/test/KukaFRITest.cpp +++ b/test/KukaFRITest.cpp @@ -248,6 +248,7 @@ int main(int argc, char* argv[]) { kukaDriverP->set( jointStateToCommand, grl::revolute_joint_angle_open_chain_command_tag()); + kukaDriverP->start_recording(); kukaDriverP->run_one(); } From e986de067303b706685b72a2ba6c63250877e909 Mon Sep 17 00:00:00 2001 From: Chunting Date: Tue, 23 Jan 2018 19:01:09 -0500 Subject: [PATCH 037/102] Clean some codes --- include/grl/kuka/KukaFRIClientDataDriver.hpp | 675 +++++++++---------- include/grl/kuka/KukaFRIalgorithm.hpp | 213 +++--- include/grl/kuka/KukaFRIdriver.hpp | 168 +++-- 3 files changed, 472 insertions(+), 584 deletions(-) diff --git a/include/grl/kuka/KukaFRIClientDataDriver.hpp b/include/grl/kuka/KukaFRIClientDataDriver.hpp index d8a9450..1bf5bc2 100644 --- a/include/grl/kuka/KukaFRIClientDataDriver.hpp +++ b/include/grl/kuka/KukaFRIClientDataDriver.hpp @@ -56,27 +56,25 @@ namespace arm { /// @note encode needs to be updated for each additional supported command type /// and when updating to newer FRI versions void decode(KUKA::FRI::ClientData &friData, std::size_t msg_size) { - // The decoder was given a pointer to the monitoringMessage at initialization - if (!friData.decoder.decode(friData.receiveBuffer, msg_size)) { - BOOST_THROW_EXCEPTION(std::runtime_error("Error decoding received FRI data, the message may be missing or corrupted. This error is most likely due to the application running on the KUKA robot's side of the connection going down or disabling FRI, so check the robot and the JAVA side of the system.")); - } - - // check message type - if (friData.expectedMonitorMsgID != - friData.monitoringMsg.header.messageIdentifier) { - BOOST_THROW_EXCEPTION(std::invalid_argument( - std::string("KukaFRI.hpp: Problem reading buffer, id code: ") + - boost::lexical_cast( - static_cast(friData.monitoringMsg.header.messageIdentifier)) + - std::string(" does not match expected id code: ") + - boost::lexical_cast( - static_cast(friData.expectedMonitorMsgID)) + - std::string("\n"))); - return; - } + // The decoder was given a pointer to the monitoringMessage at initialization + if (!friData.decoder.decode(friData.receiveBuffer, msg_size)) { + BOOST_THROW_EXCEPTION(std::runtime_error("Error decoding received FRI data, the message may be missing or corrupted. This error is most likely due to the application running on the KUKA robot's side of the connection going down or disabling FRI, so check the robot and the JAVA side of the system.")); + } - friData.lastState = - grl::robot::arm::get(friData.monitoringMsg, KUKA::FRI::ESessionState()); + // check message type + if (friData.expectedMonitorMsgID != friData.monitoringMsg.header.messageIdentifier) { + BOOST_THROW_EXCEPTION(std::invalid_argument( + std::string("KukaFRI.hpp: Problem reading buffer, id code: ") + + boost::lexical_cast( + static_cast(friData.monitoringMsg.header.messageIdentifier)) + + std::string(" does not match expected id code: ") + + boost::lexical_cast( + static_cast(friData.expectedMonitorMsgID)) + + std::string("\n"))); + return; + } + // KUKA::FRI::ESessionState + friData.lastState = grl::robot::arm::get(friData.monitoringMsg, KUKA::FRI::ESessionState()); } /// @brief Default LowLevelStepAlgorithmType @@ -85,237 +83,199 @@ void decode(KUKA::FRI::ClientData &friData, std::size_t msg_size) { /// traits". See boost.geometry access and coorinate_type classes for examples. /// Also perhaps make this the outer class which accepts drivers at the template param? struct LinearInterpolation { + enum ParamIndex {JointAngleDest, TimeDurationToDestMS}; + + // A variable-size array container with fixed capacity. + // A static_vector is a sequence that supports random access to elements, constant time insertion and removal of elements at the end, and linear time insertion and removal of elements at the beginning or in the middle. + typedef std::tuple, std::size_t> Params; + // extremely conservative default timeframe to reach destination plus no goal position + static const Params defaultParams() { + boost::container::static_vector nopos; + return std::make_tuple(nopos,10000); + } + /// Default constructor + /// @todo verify this doesn't corrupt the state of the system + LinearInterpolation() { }; + // no action by default + template + void lowLevelTimestep(ArmDataType &, CommandModeType &) { + // need to tag dispatch here + BOOST_VERIFY(false); // not yet supported + } - enum ParamIndex { - JointAngleDest, - TimeDurationToDestMS - }; - - // A variable-size array container with fixed capacity. - // A static_vector is a sequence that supports random access to elements, constant time insertion and removal of elements at the end, and linear time insertion and removal of elements at the beginning or in the middle. - typedef std::tuple,std::size_t> Params; - - // extremely conservative default timeframe to reach destination plus no goal position - static const Params defaultParams() { - boost::container::static_vector nopos; - return std::make_tuple(nopos,10000); - } - /// Default constructor - /// @todo verify this doesn't corrupt the state of the system - LinearInterpolation() { }; - - // no action by default - template - void lowLevelTimestep(ArmDataType &, CommandModeType &) { - // need to tag dispatch here - BOOST_VERIFY(false); // not yet supported - } + template + void lowLevelTimestep(ArmData &friData, revolute_joint_angle_open_chain_command_tag) { + + // no updates if no goal has been set + if(goal_position.size() == 0) return; + // switch (friData_->monitoringMsg.robotInfo.controlMode) { + // case ControlMode_POSITION_CONTROLMODE: + // case ControlMode_JOINT_IMPEDANCE_CONTROLMODE: + + KukaState::joint_state ipoJointPos; + KukaState::joint_state currentJointPos; + KukaState::joint_state currentMinusIPOJointPos; + KukaState::joint_state goalPlusIPOJointPos; + KukaState::joint_state diffToGoal; + KukaState::joint_state amountToMove; + KukaState::joint_state commandToSend; + KukaState::joint_state commandToSendPlusIPOJointPos; + + /// the current "holdposition" joint angles + /// @todo maybe this should be the revolute_joint_angle_interpolated_open_chain_state_tag()? @see + /// kukaFRIalgorithm.hpp + grl::robot::arm::copy(friData.monitoringMsg, + std::back_inserter(currentJointPos), + revolute_joint_angle_open_chain_state_tag()); + grl::robot::arm::copy(friData.monitoringMsg, + std::back_inserter(ipoJointPos), + revolute_joint_angle_interpolated_open_chain_state_tag()); + + boost::transform(currentJointPos, ipoJointPos, std::back_inserter(currentMinusIPOJointPos), std::minus()); + boost::transform(goal_position, ipoJointPos, std::back_inserter(goalPlusIPOJointPos), std::plus()); + + // only move if there is time left to reach the goal + if(goal_position_command_time_duration_remaining > 0) + { + /// single timestep in ms, the time duration between when udp packets are expected to be sent in milliseconds + /// uint32_t sendPeriod in ConnectionInfo; + int thisTimeStepMS(grl::robot::arm::get(friData.monitoringMsg, grl::time_step_tag())); + std::cout << "Send Period: " << thisTimeStepMS << std::endl + << "goal_position_command_time_duration_remaining: " <(thisTimeStepMS) / 1000); + + /// the fraction of the distance to the goal that should be traversed this tick + double fractionOfDistanceToTraverse = static_cast(thisTimeStepMS)/static_cast(goal_position_command_time_duration_remaining); + + // get the angular distance to the goal + // use current time and time to destination to interpolate (scale) goal + // joint position + boost::transform(goal_position, currentJointPos, std::back_inserter(diffToGoal), + [&](double commanded_angle, double current_angle) { + return (commanded_angle - current_angle) * fractionOfDistanceToTraverse; + }); + + + goal_position_command_time_duration_remaining -= thisTimeStepMS; + + /// @todo correctly pass velocity limits from outside, use "copy" fuction in + /// Kuka.hpp, correctly account for differing robot models. This *should* + /// be in KukaFRIdriver at the end of this file. + + // R820 velocity limits + // A1 - 85 °/s == 1.483529864195 rad/s + // A2 - 85 °/s == 1.483529864195 rad/s + // A3 - 100 °/s == 1.745329251994 rad/s + // A4 - 75 °/s == 1.308996938996 rad/s + // A5 - 130 °/s == 2.268928027593 rad/s + // A6 - 135 °/s == 2.356194490192 rad/s + // A1 - 135 °/s == 2.356194490192 rad/s + KukaState::joint_state velocity_limits; + velocity_limits.push_back(1.483529864195*thisTimeStepS); + velocity_limits.push_back(1.483529864195*thisTimeStepS); + velocity_limits.push_back(1.745329251994*thisTimeStepS); + velocity_limits.push_back(1.308996938996*thisTimeStepS); + velocity_limits.push_back(2.268928027593*thisTimeStepS); + velocity_limits.push_back(2.356194490192*thisTimeStepS); + velocity_limits.push_back(2.356194490192*thisTimeStepS); + + // clamp the commanded velocities to below the system limits + // use std::min to ensure commanded change in position remains under the + // maximum possible velocity for a single timestep + boost::transform(diffToGoal, velocity_limits, std::back_inserter(amountToMove), + [&](double diff, double maxvel) { + return boost::math::copysign(std::min(std::abs(diff), maxvel), diff); + }); + // add the current joint position to the amount to move to get the actual position command to send + boost::transform(currentJointPos, amountToMove, std::back_inserter(commandToSend), std::plus()); + boost::transform(commandToSend, ipoJointPos, std::back_inserter(commandToSendPlusIPOJointPos), std::plus()); + // send the command + grl::robot::arm::set(friData.commandMsg, commandToSend, grl::revolute_joint_angle_open_chain_command_tag()); + + std::cout << "commandToSend: " << commandToSend << "\n" << + "currentJointPos: " << currentJointPos << "\n" << + "amountToMove: " << amountToMove << "\n" ; + + /// copy value for debugging, makes viewing in a debugger easier + double ripoJointPos[7]; + double rcurrentJointPos[7]; + double rcurrentMinusIPOJointPos[7]; + double rgoalPlusIPOJointPos[7]; + double rcommandedGoal[7]; + double rdiffToGoal[7]; + double ramountToMove[7]; + double rcommandToSend[7]; + double rvelocity_limits[7]; + double rcommandToSendPlusIPOJointPos[7]; + boost::copy(ipoJointPos, &ripoJointPos[0]); + boost::copy(currentJointPos, &rcurrentJointPos[0]); + boost::copy(goal_position, &rcommandedGoal[0]); + boost::copy(currentMinusIPOJointPos, &rcurrentMinusIPOJointPos[0]); + boost::copy(goal_position, &rcommandedGoal[0]); + boost::copy(goalPlusIPOJointPos, &rgoalPlusIPOJointPos[0]); + + boost::copy(goal_position, &rcommandedGoal[0]); + boost::copy(diffToGoal, &rdiffToGoal[0]); + boost::copy(velocity_limits, &rvelocity_limits[0]); + boost::copy(amountToMove, &ramountToMove[0]); + boost::copy(currentMinusIPOJointPos, &rcurrentMinusIPOJointPos[0]); + boost::copy(commandToSend, &rcommandToSend[0]); + boost::copy(commandToSendPlusIPOJointPos, &rcommandToSendPlusIPOJointPos[0]); - template - void lowLevelTimestep(ArmData &friData, - revolute_joint_angle_open_chain_command_tag) { - - // no updates if no goal has been set - if(goal_position.size() == 0) return; - // switch (friData_->monitoringMsg.robotInfo.controlMode) { - // case ControlMode_POSITION_CONTROLMODE: - // case ControlMode_JOINT_IMPEDANCE_CONTROLMODE: - - KukaState::joint_state ipoJointPos; - KukaState::joint_state currentJointPos; - KukaState::joint_state currentMinusIPOJointPos; - KukaState::joint_state goalPlusIPOJointPos; - KukaState::joint_state diffToGoal; - KukaState::joint_state amountToMove; - KukaState::joint_state commandToSend; - KukaState::joint_state commandToSendPlusIPOJointPos; - - double ripoJointPos[7]; - double rcurrentJointPos[7]; - double rcurrentMinusIPOJointPos[7]; - double rgoalPlusIPOJointPos[7]; - double rcommandedGoal[7]; - double rdiffToGoal[7]; - double ramountToMove[7]; - double rcommandToSend[7]; - double rvelocity_limits[7]; - double rcommandToSendPlusIPOJointPos[7]; - - // the current "holdposition" joint angles - /// @todo maybe this should be the - /// revolute_joint_angle_interpolated_open_chain_state_tag()? @see - /// kukaFRIalgorithm.hpp - grl::robot::arm::copy(friData.monitoringMsg, - std::back_inserter(currentJointPos), - revolute_joint_angle_open_chain_state_tag()); - grl::robot::arm::copy(friData.monitoringMsg, - std::back_inserter(ipoJointPos), - revolute_joint_angle_interpolated_open_chain_state_tag()); - - // copy value for debugging - boost::copy(ipoJointPos, &ripoJointPos[0]); - boost::copy(currentJointPos, &rcurrentJointPos[0]); - boost::copy(goal_position, &rcommandedGoal[0]); - - boost::transform(currentJointPos, ipoJointPos, - std::back_inserter(currentMinusIPOJointPos), std::minus()); - boost::transform(goal_position, ipoJointPos, - std::back_inserter(goalPlusIPOJointPos), std::plus()); - - - boost::copy(currentMinusIPOJointPos, &rcurrentMinusIPOJointPos[0]); - boost::copy(goal_position, &rcommandedGoal[0]); - boost::copy(goalPlusIPOJointPos, &rgoalPlusIPOJointPos[0]); - - // only move if there is time left to reach the goal - if(goal_position_command_time_duration_remaining > 0) - { - // single timestep in ms - int thisTimeStepMS(grl::robot::arm::get(friData.monitoringMsg, grl::time_step_tag())); - double thisTimeStepS = (static_cast(thisTimeStepMS) / 1000); - //double secondsPerTick = std::chrono::duration_cast(thisTimeStep).count(); - - // the fraction of the distance to the goal that should be traversed this - // tick - double fractionOfDistanceToTraverse = - static_cast(thisTimeStepMS) / - static_cast(goal_position_command_time_duration_remaining); - - // makes viewing in a debugger easier - boost::copy(goal_position, &rcommandedGoal[0]); - // get the angular distance to the goal - // use current time and time to destination to interpolate (scale) goal - // joint position - boost::transform(goal_position, currentJointPos, - std::back_inserter(diffToGoal), - [&](double commanded_angle, double current_angle) { - return (commanded_angle - current_angle) * - fractionOfDistanceToTraverse; - }); - boost::copy(diffToGoal, &rdiffToGoal[0]); - - goal_position_command_time_duration_remaining -= thisTimeStepMS; - - /// @todo correctly pass velocity limits from outside, use "copy" fuction in - /// Kuka.hpp, correctly account for differing robot models. This *should* - /// be in KukaFRIdriver at the end of this file. - - // R820 velocity limits - // A1 - 85 °/s == 1.483529864195 rad/s - // A2 - 85 °/s == 1.483529864195 rad/s - // A3 - 100 °/s == 1.745329251994 rad/s - // A4 - 75 °/s == 1.308996938996 rad/s - // A5 - 130 °/s == 2.268928027593 rad/s - // A6 - 135 °/s == 2.356194490192 rad/s - // A1 - 135 °/s == 2.356194490192 rad/s - KukaState::joint_state velocity_limits; - velocity_limits.push_back(1.483529864195*thisTimeStepS); - velocity_limits.push_back(1.483529864195*thisTimeStepS); - velocity_limits.push_back(1.745329251994*thisTimeStepS); - velocity_limits.push_back(1.308996938996*thisTimeStepS); - velocity_limits.push_back(2.268928027593*thisTimeStepS); - velocity_limits.push_back(2.356194490192*thisTimeStepS); - velocity_limits.push_back(2.356194490192*thisTimeStepS); - - boost::copy(velocity_limits, &rvelocity_limits[0]); - // clamp the commanded velocities to below the system limits - // use std::min to ensure commanded change in position remains under the - // maximum possible velocity for a single timestep - boost::transform( - diffToGoal, velocity_limits, std::back_inserter(amountToMove), - [&](double diff, double maxvel) { - return boost::math::copysign(std::min(std::abs(diff), maxvel), diff); - }); - - boost::copy(amountToMove, &ramountToMove[0]); - - - // add the current joint position to the amount to move to get the actual - // position command to send - boost::transform(currentJointPos, amountToMove, - std::back_inserter(commandToSend), std::plus()); - - boost::copy(currentMinusIPOJointPos, &rcurrentMinusIPOJointPos[0]); - boost::copy(commandToSend, &rcommandToSend[0]); - - - boost::transform(commandToSend, ipoJointPos, - std::back_inserter(commandToSendPlusIPOJointPos), std::plus()); - - - boost::copy(commandToSendPlusIPOJointPos, &rcommandToSendPlusIPOJointPos[0]); - // send the command -// grl::robot::arm::set(friData.commandMsg, commandToSend, -// grl::revolute_joint_angle_open_chain_command_tag()); - // send the command - grl::robot::arm::set(friData.commandMsg, commandToSend, - grl::revolute_joint_angle_open_chain_command_tag()); + } } - // break; - } - void setGoal(const Params& params ) { - /// @todo TODO(ahundt) support param tag structs for additional control modes - goal_position_command_time_duration_remaining = std::get(params); - goal_position = std::get(params); + void setGoal(const Params& params ) { + /// @todo TODO(ahundt) support param tag structs for additional control modes + goal_position_command_time_duration_remaining = std::get(params); + goal_position = std::get(params); + } - } + /// @todo look in FRI_Client_SDK_Cpp.zip to see if position must be set for + /// joint torques. Ref files: LBRTorqueSineOverlayClient.cpp, + /// LBRTorqueSineOverlayClient.h, friLBRCommand.cpp, friLBRCommand.h + template + void lowLevelTimestep(ArmData &friData, + revolute_joint_torque_open_chain_command_tag) { + //not yet supported + BOOST_VERIFY(false); + // case ControlMode_JOINT_IMPEDANCE_CONTROLMODE: + // grl::robot::arm::set(friData.commandMsg, armState.commandedTorque, + // grl::revolute_joint_torque_open_chain_command_tag()); + /// @note encode() needs to be updated for each additional supported command + /// type + // break; + } - /// @todo look in FRI_Client_SDK_Cpp.zip to see if position must be set for - /// joint torques. Ref files: LBRTorqueSineOverlayClient.cpp, - /// LBRTorqueSineOverlayClient.h, friLBRCommand.cpp, friLBRCommand.h - template - void lowLevelTimestep(ArmData &friData, - revolute_joint_torque_open_chain_command_tag) { - - //not yet supported - BOOST_VERIFY(false); - // case ControlMode_JOINT_IMPEDANCE_CONTROLMODE: -// grl::robot::arm::set(friData.commandMsg, armState.commandedTorque, -// grl::revolute_joint_torque_open_chain_command_tag()); - - /// @note encode() needs to be updated for each additional supported command - /// type - // break; - } + /// @todo look in FRI_Client_SDK_Cpp.zip to see if position must be set for + /// cartesian wrench. Ref files: LBRWrenchSineOverlayClient.cpp, + /// LBRWrenchSineOverlayClient.h, friLBRCommand.cpp, friLBRCommand.h + template + void lowLevelTimestep(ArmData &friData, cartesian_wrench_command_tag) { + //not yet supported + BOOST_VERIFY(false); + // case ControlMode_CARTESIAN_IMPEDANCE_CONTROLMODE: + // not yet supported + // grl::robot::arm::set(friData.commandMsg, + // armState.commandedCartesianWrenchFeedForward, + // grl::cartesian_wrench_command_tag()); + // break; + } - /// @todo look in FRI_Client_SDK_Cpp.zip to see if position must be set for - /// cartesian wrench. Ref files: LBRWrenchSineOverlayClient.cpp, - /// LBRWrenchSineOverlayClient.h, friLBRCommand.cpp, friLBRCommand.h - template - void lowLevelTimestep(ArmData &friData, cartesian_wrench_command_tag) { - - //not yet supported - BOOST_VERIFY(false); - // case ControlMode_CARTESIAN_IMPEDANCE_CONTROLMODE: - // not yet supported -// grl::robot::arm::set(friData.commandMsg, -// armState.commandedCartesianWrenchFeedForward, -// grl::cartesian_wrench_command_tag()); - // break; - } + /// @todo make this accessible via a nonmember function + bool hasCommandData() { + /// @todo check if duration remaining should be greater than zero or greater + /// than the last tick size + return goal_position_command_time_duration_remaining > 0; + } + private: + // the armstate at initialization of this object + KukaState::joint_state velocity_limits; + KukaState::joint_state goal_position; + double goal_position_command_time_duration_remaining; // milliseconds +}; // End of the structure - /// @todo make this accessible via a nonmember function - bool hasCommandData() { - /// @todo check if duration remaining should be greater than zero or greater - /// than the last tick size - return goal_position_command_time_duration_remaining > 0; - } - // template - // void operator()(ArmData& clientData, - // revolute_joint_angle_open_chain_command_tag){ - // default: - // break; - // } -private: - // the armstate at initialization of this object - KukaState::joint_state velocity_limits; - KukaState::joint_state goal_position; - double goal_position_command_time_duration_remaining; // milliseconds -}; - -/// @brief encode data in the class KUKA::FRI::ClientData into the send buffer -/// for the KUKA FRI. +/// @brief encode data in the class KUKA::FRI::ClientData into the send buffer for the KUKA FRI. /// this preps the information for transport over the network /// /// @note encode needs to be updated for each additional supported command type @@ -324,81 +284,71 @@ template std::size_t encode(LowLevelStepAlgorithmType &step_alg, KUKA::FRI::ClientData &friData, boost::system::error_code &ec) { - // reset send counter - friData.lastSendCounter = 0; - - // set sequence counters - friData.commandMsg.header.sequenceCounter = friData.sequenceCounter++; - friData.commandMsg.header.reflectedSequenceCounter = - friData.monitoringMsg.header.sequenceCounter; - - KUKA::FRI::ESessionState sessionState = - grl::robot::arm::get(friData.monitoringMsg, KUKA::FRI::ESessionState()); - - if ((step_alg.hasCommandData() && - (sessionState == KUKA::FRI::COMMANDING_WAIT || - sessionState == KUKA::FRI::COMMANDING_ACTIVE))) { - KUKA::FRI::EClientCommandMode commandMode = grl::robot::arm::get( - friData.monitoringMsg, KUKA::FRI::EClientCommandMode()); - switch (commandMode) { - case ClientCommandMode_POSITION: - step_alg.lowLevelTimestep(friData, revolute_joint_angle_open_chain_command_tag()); - break; - case ClientCommandMode_WRENCH: - step_alg.lowLevelTimestep(friData, cartesian_wrench_command_tag()); - break; - case ClientCommandMode_TORQUE: - step_alg.lowLevelTimestep(friData, revolute_joint_torque_open_chain_command_tag()); - break; - default: - // this is unhandled at the moment... - BOOST_VERIFY(false); - // BOOST_THROW_EXCEPTION_CURRENT_FUNCTION; - // ClientCommandMode_NO_COMMAND_MODE, anything else that is added in the - // future and unimplemented - /// @todo do nothing if in an unsupported command mode? Or do the same as - /// the next else if step? - break; - } + // reset send counter + friData.lastSendCounter = 0; - } else if (!(friData.commandMsg.has_commandData && - step_alg.hasCommandData() && - (sessionState == KUKA::FRI::COMMANDING_WAIT || - sessionState == KUKA::FRI::COMMANDING_ACTIVE))) { - // copy current measured joint position to commanded position only if we - // *don't* have new command data - - /// @todo should this be different if it is in torque mode? - /// @todo allow copying of data directly between commandmsg and - /// monitoringMsg - std::vector msg; - copy(friData.monitoringMsg, std::back_inserter(msg), - revolute_joint_angle_open_chain_command_tag()); - // copy the previously recorded command over - set(friData.commandMsg, msg, - grl::revolute_joint_angle_open_chain_command_tag()); - } + // set sequence counters + friData.commandMsg.header.sequenceCounter = friData.sequenceCounter++; + friData.commandMsg.header.reflectedSequenceCounter = friData.monitoringMsg.header.sequenceCounter; - int buffersize = KUKA::FRI::FRI_COMMAND_MSG_MAX_SIZE; - if (!friData.encoder.encode(friData.sendBuffer, buffersize)) { - // @todo figure out PB_GET_ERROR, integrate with error_code type supported - // by boost - ec = boost::system::errc::make_error_code(boost::system::errc::bad_message); - return 0; - } + KUKA::FRI::ESessionState sessionState = grl::robot::arm::get(friData.monitoringMsg, KUKA::FRI::ESessionState()); + + if ((step_alg.hasCommandData() && + (sessionState == KUKA::FRI::COMMANDING_WAIT || sessionState == KUKA::FRI::COMMANDING_ACTIVE))) + { + KUKA::FRI::EClientCommandMode commandMode = grl::robot::arm::get(friData.monitoringMsg, KUKA::FRI::EClientCommandMode()); + switch (commandMode) { + case ClientCommandMode_POSITION: + step_alg.lowLevelTimestep(friData, revolute_joint_angle_open_chain_command_tag()); + break; + case ClientCommandMode_WRENCH: + step_alg.lowLevelTimestep(friData, cartesian_wrench_command_tag()); + break; + case ClientCommandMode_TORQUE: + step_alg.lowLevelTimestep(friData, revolute_joint_torque_open_chain_command_tag()); + break; + default: + // this is unhandled at the moment... + BOOST_VERIFY(false); + // BOOST_THROW_EXCEPTION_CURRENT_FUNCTION; + // ClientCommandMode_NO_COMMAND_MODE, anything else that is added in the + // future and unimplemented + /// @todo do nothing if in an unsupported command mode? Or do the same as + /// the next else if step? + break; + } - return buffersize; + } else if (!(friData.commandMsg.has_commandData && step_alg.hasCommandData() && + (sessionState == KUKA::FRI::COMMANDING_WAIT || sessionState == KUKA::FRI::COMMANDING_ACTIVE))) + { + // copy current measured joint position to commanded position only if we + // *don't* have new command data + + /// @todo should this be different if it is in torque mode? + /// @todo allow copying of data directly between commandmsg and + /// monitoringMsg + std::vector msg; + copy(friData.monitoringMsg, std::back_inserter(msg), revolute_joint_angle_open_chain_command_tag()); + // copy the previously recorded command over + set(friData.commandMsg, msg, grl::revolute_joint_angle_open_chain_command_tag()); + } + + int buffersize = KUKA::FRI::FRI_COMMAND_MSG_MAX_SIZE; + if (!friData.encoder.encode(friData.sendBuffer, buffersize)) { + // @todo figure out PB_GET_ERROR, integrate with error_code type supported by boost + ec = boost::system::errc::make_error_code(boost::system::errc::bad_message); + return 0; + } + + return buffersize; } -/// @brief Actually talk over the network to receive an update and send out a -/// new KUKA FRI command +/// @brief Actually talk over the network to receive an update and send out a new KUKA FRI command /// -/// Receives an update, performs the necessary checks, then sends a message if -/// appropriate. +/// Receives an update, performs the necessary checks, then sends a message if appropriate. /// /// @pre socket must already have the endpoint resolved and "connected". While -/// udp is technically stateless the asio socket supports the connection api -/// components for convenience. +/// udp is technically stateless the asio socket supports the connection api components for convenience. template void update_state(boost::asio::ip::udp::socket &socket, LowLevelStepAlgorithmType &step_alg, @@ -410,56 +360,56 @@ void update_state(boost::asio::ip::udp::socket &socket, grl::TimeEvent& timeEvent, boost::asio::ip::udp::endpoint sender_endpoint = boost::asio::ip::udp::endpoint()) { - static const int message_flags = 0; - - // get a local clock timestamp, then the latest frame from the device, then another timestamp - timeEvent.local_request_time = cartographer::common::UniversalTimeScaleClock::now(); - receive_bytes_transferred = socket.receive_from( - boost::asio::buffer(friData.receiveBuffer, - KUKA::FRI::FRI_MONITOR_MSG_MAX_SIZE), - sender_endpoint, message_flags, receive_ec); - timeEvent.local_receive_time = cartographer::common::UniversalTimeScaleClock::now(); - decode(friData, receive_bytes_transferred); - - friData.lastSendCounter++; - // Check whether to send a response - if (friData.lastSendCounter >= - friData.monitoringMsg.connectionInfo.receiveMultiplier) { - send_bytes_transferred = encode(step_alg, friData, send_ec); - if (send_ec) - return; - socket.send(boost::asio::buffer(friData.sendBuffer, send_bytes_transferred), - message_flags, send_ec); - } + static const int message_flags = 0; + + // get a local clock timestamp, then the latest frame from the device, then another timestamp + timeEvent.local_request_time = cartographer::common::UniversalTimeScaleClock::now(); + receive_bytes_transferred = socket.receive_from( + boost::asio::buffer(friData.receiveBuffer, + KUKA::FRI::FRI_MONITOR_MSG_MAX_SIZE), + sender_endpoint, message_flags, receive_ec); + timeEvent.local_receive_time = cartographer::common::UniversalTimeScaleClock::now(); + decode(friData, receive_bytes_transferred); + + friData.lastSendCounter++; + // Check whether to send a response + if (friData.lastSendCounter >= + friData.monitoringMsg.connectionInfo.receiveMultiplier) { + send_bytes_transferred = encode(step_alg, friData, send_ec); + if (send_ec) + return; + socket.send(boost::asio::buffer(friData.sendBuffer, send_bytes_transferred), + message_flags, send_ec); + } } /// @brief don't use this /// @deprecated this is an old implemenation that will be removed in the future void copy(const FRIMonitoringMessage &monitoringMsg, KukaState &state) { - state.clear(); - copy(monitoringMsg, std::back_inserter(state.position), - revolute_joint_angle_open_chain_state_tag()); - copy(monitoringMsg, std::back_inserter(state.torque), - revolute_joint_torque_open_chain_state_tag()); - copy(monitoringMsg, std::back_inserter(state.commandedPosition), - revolute_joint_angle_open_chain_command_tag()); - copy(monitoringMsg, std::back_inserter(state.commandedTorque), - revolute_joint_torque_open_chain_command_tag()); - copy(monitoringMsg, std::back_inserter(state.ipoJointPosition), - revolute_joint_angle_interpolated_open_chain_state_tag()); - state.sessionState = static_cast( - get(monitoringMsg, KUKA::FRI::ESessionState())); - state.connectionQuality = static_cast( - get(monitoringMsg, KUKA::FRI::EConnectionQuality())); - state.safetyState = static_cast( - get(monitoringMsg, KUKA::FRI::ESafetyState())); - state.operationMode = static_cast( - get(monitoringMsg, KUKA::FRI::EOperationMode())); - state.driveState = static_cast( - get(monitoringMsg, KUKA::FRI::EDriveState())); - - /// @todo fill out missing state update steps + state.clear(); + copy(monitoringMsg, std::back_inserter(state.position), + revolute_joint_angle_open_chain_state_tag()); + copy(monitoringMsg, std::back_inserter(state.torque), + revolute_joint_torque_open_chain_state_tag()); + copy(monitoringMsg, std::back_inserter(state.commandedPosition), + revolute_joint_angle_open_chain_command_tag()); + copy(monitoringMsg, std::back_inserter(state.commandedTorque), + revolute_joint_torque_open_chain_command_tag()); + copy(monitoringMsg, std::back_inserter(state.ipoJointPosition), + revolute_joint_angle_interpolated_open_chain_state_tag()); + state.sessionState = static_cast( + get(monitoringMsg, KUKA::FRI::ESessionState())); + state.connectionQuality = static_cast( + get(monitoringMsg, KUKA::FRI::EConnectionQuality())); + state.safetyState = static_cast( + get(monitoringMsg, KUKA::FRI::ESafetyState())); + state.operationMode = static_cast( + get(monitoringMsg, KUKA::FRI::EOperationMode())); + state.driveState = static_cast( + get(monitoringMsg, KUKA::FRI::EDriveState())); + + /// @todo fill out missing state update steps } /// @brief Simple low level driver to communicate over the Kuka iiwa FRI @@ -782,8 +732,7 @@ class KukaFRIClientDataDriver nextClientData.lastState = latestClientData.lastState; nextClientData.sequenceCounter = latestClientData.sequenceCounter; nextClientData.lastSendCounter = latestClientData.lastSendCounter; - nextClientData.expectedMonitorMsgID = - latestClientData.expectedMonitorMsgID; + nextClientData.expectedMonitorMsgID = latestClientData.expectedMonitorMsgID; // copy command from latestCommandBackup to nextState aka // nextClientData @@ -866,16 +815,14 @@ class KukaFRIClientDataDriver /// Initialize valid shared ptr to LatestState object with a valid allocated /// friData. Note that lowLevelAlgorithmParams will remain null! - static LatestState - make_valid_LatestState( +static LatestState make_valid_LatestState( std::shared_ptr lowLevelAlgorithmParams, - std::shared_ptr &friData - ) { - if (!friData) { - friData = make_shared_valid_ClientData(); - } + std::shared_ptr &friData) { + if (!friData) { + friData = make_shared_valid_ClientData(); + } - return make_LatestState(lowLevelAlgorithmParams,friData); + return make_LatestState(lowLevelAlgorithmParams,friData); } static LatestState make_valid_LatestState() { diff --git a/include/grl/kuka/KukaFRIalgorithm.hpp b/include/grl/kuka/KukaFRIalgorithm.hpp index 5b135e3..51d30ad 100644 --- a/include/grl/kuka/KukaFRIalgorithm.hpp +++ b/include/grl/kuka/KukaFRIalgorithm.hpp @@ -28,14 +28,13 @@ namespace grl { namespace robot { - namespace arm { namespace kuka { - // Following from Kuka example program - const int default_port_id = 30200; - struct send_period{}; - struct receive_multiplier{}; + // Following from Kuka example program + const int default_port_id = 30200; + struct send_period{}; + struct receive_multiplier{}; namespace detail { @@ -78,13 +77,13 @@ namespace grl { namespace robot { /// copy measured joint angle to output iterator template void copy(const FRIMonitoringMessage& monitoringMsg, OutputIterator it, revolute_joint_angle_open_chain_state_tag){ - kuka::detail::copyJointState(monitoringMsg.monitorData.measuredJointPosition.value.arg,it,monitoringMsg.monitorData.has_measuredJointPosition); + kuka::detail::copyJointState(monitoringMsg.monitorData.measuredJointPosition.value.arg,it,monitoringMsg.monitorData.has_measuredJointPosition); } /// copy interpolated commanded joint angles template void copy(const FRIMonitoringMessage& monitoringMsg, OutputIterator it,revolute_joint_angle_interpolated_open_chain_state_tag){ - kuka::detail::copyJointState(monitoringMsg.ipoData.jointPosition.value.arg,it,monitoringMsg.ipoData.has_jointPosition); + kuka::detail::copyJointState(monitoringMsg.ipoData.jointPosition.value.arg,it,monitoringMsg.ipoData.has_jointPosition); } /// copy commanded joint angle to output iterator @@ -97,13 +96,13 @@ namespace grl { namespace robot { /// copy measured joint torque to output iterator template void copy(const FRIMonitoringMessage& monitoringMsg, OutputIterator it, revolute_joint_torque_open_chain_state_tag){ - kuka::detail::copyJointState(monitoringMsg.monitorData.measuredTorque.value.arg,it, monitoringMsg.monitorData.has_measuredTorque); + kuka::detail::copyJointState(monitoringMsg.monitorData.measuredTorque.value.arg,it, monitoringMsg.monitorData.has_measuredTorque); } /// copy measured external joint torque to output iterator template void copy(const FRIMonitoringMessage& monitoringMsg, OutputIterator it, revolute_joint_torque_external_open_chain_state_tag){ - kuka::detail::copyJointState(monitoringMsg.monitorData.externalTorque.value.arg,it, monitoringMsg.monitorData.has_externalTorque); + kuka::detail::copyJointState(monitoringMsg.monitorData.externalTorque.value.arg,it, monitoringMsg.monitorData.has_externalTorque); } // only supported for kuka sunrise OS 1.9 @@ -120,23 +119,22 @@ namespace grl { namespace robot { /// (Euler angles A, B, C) of the currently used motion center. template void copy(const FRIMonitoringMessage& monitoringMsg, OutputIterator it, cartesian_external_force_tag){ - kuka::detail::copyCartesianState(monitoringMsg.monitorData.externalForce,it, monitoringMsg.monitorData.has_externalForce); - } + kuka::detail::copyCartesianState(monitoringMsg.monitorData.externalForce,it, monitoringMsg.monitorData.has_externalForce); + } #endif // KUKA_SUNRISE_1_9 /// copy commanded joint torque to output iterator template void copy(const FRIMonitoringMessage& monitoringMsg, OutputIterator it, revolute_joint_torque_open_chain_command_tag){ - kuka::detail::copyJointState(monitoringMsg.monitorData.commandedTorque.value.arg,it, monitoringMsg.monitorData.has_commandedTorque); + kuka::detail::copyJointState(monitoringMsg.monitorData.commandedTorque.value.arg,it, monitoringMsg.monitorData.has_commandedTorque); } - - + /// copy commanded joint angle to output iterator template void copy(const FRICommandMessage& commandMsg, OutputIterator it, revolute_joint_angle_open_chain_command_tag){ if (commandMsg.has_commandData) kuka::detail::copyJointState(commandMsg.commandData.jointPosition.value.arg,it,commandMsg.commandData.has_jointPosition); } - + /// KUKA::FRI::ESafetyState is defined in #include "friClientIf.h" /// @todo consider using another default value, or perhaps boost::optional? KUKA::FRI::ESafetyState get(const FRIMonitoringMessage& monitoringMsg, const KUKA::FRI::ESafetyState) { KUKA::FRI::ESafetyState state = KUKA::FRI::ESafetyState::NORMAL_OPERATION; @@ -147,7 +145,6 @@ namespace grl { namespace robot { return state; } - KUKA::FRI::EOperationMode get(const FRIMonitoringMessage& monitoringMsg, const KUKA::FRI::EOperationMode) { KUKA::FRI::EOperationMode state = KUKA::FRI::EOperationMode::TEST_MODE_1; if (monitoringMsg.has_robotInfo) { @@ -157,7 +154,6 @@ namespace grl { namespace robot { return state; } - KUKA::FRI::EControlMode get(const FRIMonitoringMessage& monitoringMsg, const KUKA::FRI::EControlMode) { KUKA::FRI::EControlMode state = KUKA::FRI::EControlMode::NO_CONTROL; if (monitoringMsg.has_robotInfo) { @@ -167,7 +163,6 @@ namespace grl { namespace robot { return state; } - KUKA::FRI::EClientCommandMode get(const FRIMonitoringMessage& monitoringMsg, const KUKA::FRI::EClientCommandMode) { KUKA::FRI::EClientCommandMode state = KUKA::FRI::EClientCommandMode::NO_COMMAND_MODE; if (monitoringMsg.has_ipoData) { @@ -177,7 +172,6 @@ namespace grl { namespace robot { return state; } - KUKA::FRI::EOverlayType get(const FRIMonitoringMessage& monitoringMsg, const KUKA::FRI::EOverlayType) { KUKA::FRI::EOverlayType state = KUKA::FRI::EOverlayType::NO_OVERLAY; if (monitoringMsg.has_ipoData) { @@ -187,7 +181,6 @@ namespace grl { namespace robot { return state; } - KUKA::FRI::EDriveState get(const FRIMonitoringMessage& _message, const KUKA::FRI::EDriveState) { tRepeatedIntArguments *values = @@ -251,16 +244,14 @@ namespace grl { namespace robot { } return timestamp; } - - - /** - * \brief Set the joint positions for the current interpolation step. - * - * This method is only effective when the client is in a commanding state. - * @param state Object which stores the current state of the robot, including the command to send next - * @param range Array with the new joint positions (in radians) - * @param tag identifier object indicating that revolute joint angle commands should be modified - */ + /** + * \brief Set the joint positions for the current interpolation step. + * + * This method is only effective when the client is in a commanding state. + * @param state Object which stores the current state of the robot, including the command to send next + * @param range Array with the new joint positions (in radians) + * @param tag identifier object indicating that revolute joint angle commands should be modified + */ template static inline void set(FRICommandMessage & state, Range&& range, grl::revolute_joint_angle_open_chain_command_tag) { if(boost::size(range)) @@ -272,17 +263,17 @@ namespace grl { namespace robot { } } - /** - * \brief Set the applied joint torques for the current interpolation step. - * - * This method is only effective when the client is in a commanding state. - * The ControlMode of the robot has to be joint impedance control mode. The - * Client Command Mode has to be torque. - * - * @param state Object which stores the current state of the robot, including the command to send next - * @param torques Array with the applied torque values (in Nm) - * @param tag identifier object indicating that the torqe value command should be modified - */ + /** + * \brief Set the applied joint torques for the current interpolation step. + * + * This method is only effective when the client is in a commanding state. + * The ControlMode of the robot has to be joint impedance control mode. The + * Client Command Mode has to be torque. + * + * @param state Object which stores the current state of the robot, including the command to send next + * @param torques Array with the applied torque values (in Nm) + * @param tag identifier object indicating that the torqe value command should be modified + */ template static inline void set(FRICommandMessage & state, Range&& range, grl::revolute_joint_torque_open_chain_command_tag) { if(boost::size(range)) @@ -295,74 +286,70 @@ namespace grl { namespace robot { } - /** - * \brief Set the applied wrench vector of the current interpolation step. - * - * The wrench vector consists of: - * [F_x, F_y, F_z, tau_A, tau_B, tau_C] - * - * F ... forces (in N) applied along the Cartesian axes of the - * currently used motion center. - * tau ... torques (in Nm) applied along the orientation angles - * (Euler angles A, B, C) of the currently used motion center. - * - * This method is only effective when the client is in a commanding state. - * The ControlMode of the robot has to be Cartesian impedance control mode. The - * Client Command Mode has to be wrench. - * - * @param FRICommandMessage object storing the command data that will be sent to the physical device - * @param range wrench Applied Cartesian wrench vector, in x, y, z, roll, pitch, yaw force measurments. - * @param tag identifier object indicating that the wrench value command should be modified - * - * @todo perhaps support some specific more useful data layouts - * @note copies only the elements that will fit - */ + /** + * \brief Set the applied wrench vector of the current interpolation step. + * + * The wrench vector consists of: + * [F_x, F_y, F_z, tau_A, tau_B, tau_C] + * + * F ... forces (in N) applied along the Cartesian axes of the + * currently used motion center. + * tau ... torques (in Nm) applied along the orientation angles + * (Euler angles A, B, C) of the currently used motion center. + * + * This method is only effective when the client is in a commanding state. + * The ControlMode of the robot has to be Cartesian impedance control mode. The + * Client Command Mode has to be wrench. + * + * @param FRICommandMessage object storing the command data that will be sent to the physical device + * @param range wrench Applied Cartesian wrench vector, in x, y, z, roll, pitch, yaw force measurments. + * @param tag identifier object indicating that the wrench value command should be modified + * + * @todo perhaps support some specific more useful data layouts + * @note copies only the elements that will fit + */ template static inline void set(FRICommandMessage & state, Range&& range, grl::cartesian_wrench_command_tag) { - if(boost::size(range)) - { - state.has_commandData = true; - state.commandData.has_cartesianWrenchFeedForward = true; - std::copy_n(std::begin(range),std::min(boost::size(range),state.commandData.cartesianWrenchFeedForward.element_count), &state.commandData.cartesianWrenchFeedForward.element[0]); - } + if(boost::size(range)) + { + state.has_commandData = true; + state.commandData.has_cartesianWrenchFeedForward = true; + std::copy_n(std::begin(range),std::min(boost::size(range),state.commandData.cartesianWrenchFeedForward.element_count), &state.commandData.cartesianWrenchFeedForward.element[0]); + } } /// set the left destination FRICommandMessage state to be equal to the right source FRICommandMessage state static inline void set(FRICommandMessage & state, const FRICommandMessage& sourceState, grl::command_tag) { - state.has_commandData = sourceState.has_commandData; - - // cartesianWrench - state.commandData.has_cartesianWrenchFeedForward = state.commandData.has_cartesianWrenchFeedForward; - std::copy_n(&state.commandData.cartesianWrenchFeedForward.element[0],std::min(state.commandData.cartesianWrenchFeedForward.element_count,sourceState.commandData.cartesianWrenchFeedForward.element_count), &state.commandData.cartesianWrenchFeedForward.element[0]); + state.has_commandData = sourceState.has_commandData; - // for joint copying - tRepeatedDoubleArguments *dest; - tRepeatedDoubleArguments *source; + // cartesianWrench + state.commandData.has_cartesianWrenchFeedForward = state.commandData.has_cartesianWrenchFeedForward; + std::copy_n(&state.commandData.cartesianWrenchFeedForward.element[0],std::min(state.commandData.cartesianWrenchFeedForward.element_count,sourceState.commandData.cartesianWrenchFeedForward.element_count), &state.commandData.cartesianWrenchFeedForward.element[0]); + // for joint copying + tRepeatedDoubleArguments *dest; + tRepeatedDoubleArguments *source; + // jointTorque + state.commandData.has_jointTorque = state.commandData.has_jointTorque; + dest = (tRepeatedDoubleArguments*)state.commandData.jointTorque.value.arg; + source = (tRepeatedDoubleArguments*)sourceState.commandData.jointTorque.value.arg; + std::copy_n(source->value,std::min(source->size,dest->size), dest->value); - // jointTorque - state.commandData.has_jointTorque = state.commandData.has_jointTorque; - dest = (tRepeatedDoubleArguments*)state.commandData.jointTorque.value.arg; - source = (tRepeatedDoubleArguments*)sourceState.commandData.jointTorque.value.arg; - std::copy_n(source->value,std::min(source->size,dest->size), dest->value); + // jointPosition + state.commandData.has_jointPosition = state.commandData.has_jointPosition; + dest = (tRepeatedDoubleArguments*)state.commandData.jointPosition.value.arg; + source = (tRepeatedDoubleArguments*)sourceState.commandData.jointPosition.value.arg; + std::copy_n(source->value,std::min(source->size,dest->size), dest->value); - // jointPosition - state.commandData.has_jointPosition = state.commandData.has_jointPosition; - dest = (tRepeatedDoubleArguments*)state.commandData.jointPosition.value.arg; - source = (tRepeatedDoubleArguments*)sourceState.commandData.jointPosition.value.arg; - std::copy_n(source->value,std::min(source->size,dest->size), dest->value); - - } - + } static inline void set(FRICommandMessage & state, const FRICommandMessage& sourceState) { - set(state,sourceState, grl::command_tag()); + set(state,sourceState, grl::command_tag()); } - }}} // namespace grl::robot::arm @@ -406,8 +393,6 @@ namespace traits { template struct dimension : boost::mpl::int_ {}; template struct coordinate_type { typedef boost::iterator_range type; }; - - // template struct coordinate_type; // template struct coordinate_type { typedef boost::iterator_range type; }; // template struct coordinate_type { typedef boost::iterator_range type; }; @@ -457,12 +442,6 @@ namespace traits { //static inline void set(nrec::spatial::Coordinate & p, typename nrec::spatial::Coordinate::value_type const& value) { p.operator[](Index) = value; } - - - - - - // // angle // const typename coordinate_type::type // get(FRIMonitoringMessage const& monitoringMsg,revolute_joint_angle_open_chain_state_tag) { @@ -502,24 +481,10 @@ namespace traits { }; - - - - - - - - - - - - template struct tag { typedef grl::device_command_tag type; }; template struct dimension : boost::mpl::int_<1> {}; // each joint is 1 dimensional template struct coordinate_type { typedef boost::iterator_range type; }; - - // template struct coordinate_type; // template struct coordinate_type { typedef boost::iterator_range type; }; // template struct coordinate_type { typedef boost::iterator_range type; }; @@ -536,23 +501,17 @@ namespace traits { // return grl::robot::arm::kuka::detail::get(*static_cast(monitoringMsg.monitorData.commandedJointPosition.value.arg),monitoringMsg.monitorData.has_measuredJointPosition); // } - template - static inline void set(FRICommandMessage & state, Range&& range, grl::revolute_joint_angle_open_chain_command_tag) { - state.has_commandData = true; - state.commandData.has_jointPosition = true; - tRepeatedDoubleArguments *dest = (tRepeatedDoubleArguments*)state.commandData.jointPosition.value.arg; - boost::copy(range, dest->value); - } - - + template + static inline void set(FRICommandMessage & state, Range&& range, grl::revolute_joint_angle_open_chain_command_tag) { + state.has_commandData = true; + state.commandData.has_jointPosition = true; + tRepeatedDoubleArguments *dest = (tRepeatedDoubleArguments*)state.commandData.jointPosition.value.arg; + boost::copy(range, dest->value); + } }; - }}} // boost::geometry::traits - - - #endif diff --git a/include/grl/kuka/KukaFRIdriver.hpp b/include/grl/kuka/KukaFRIdriver.hpp index 01f4c9f..4765bf0 100644 --- a/include/grl/kuka/KukaFRIdriver.hpp +++ b/include/grl/kuka/KukaFRIdriver.hpp @@ -2,6 +2,7 @@ /// If you are new to this code base you are most likely looking for KukaDriver.hpp #ifndef _KUKA_FRI_DRIVER #define _KUKA_FRI_DRIVER +#define FLATBUFFERS_DEBUG_VERIFICATION_FAILURE #include "grl/kuka/KukaFRIClientDataDriver.hpp" //#include "cartographer/common/time.h" @@ -58,10 +59,8 @@ namespace arm { /// a std::shared_ptr named pt to safely generate additional std::shared_ptr instances pt1, pt2, ... /// that all share ownership of t with pt. template -class KukaFRIdriver : public std::enable_shared_from_this< - KukaFRIdriver>, - public KukaUDP { - +class KukaFRIdriver : public std::enable_shared_from_this>, public KukaUDP +{ public: using KukaUDP::ParamIndex; // enum, define some connection parameters, such as ip, port... using KukaUDP::ThreadingRunMode; @@ -70,13 +69,6 @@ class KukaFRIdriver : public std::enable_shared_from_this< KukaFRIdriver(Params params = defaultParams()) : params_(params), m_shouldStop(false) {} - // KukaFRIdriver(boost::asio::io_service& - // device_driver_io_service__,Params params = defaultParams()) - // : - // device_driver_io_service(device_driver_io_service__), - // params_(params) - // {} - void construct() { construct(params_); } /// @todo create a function that calls simGetObjectHandle and throws an @@ -85,8 +77,7 @@ class KukaFRIdriver : public std::enable_shared_from_this< /// exception void construct(Params params) { params_ = params; - // keep driver threads from exiting immediately after creation, because they - // have work to do! + // keep driver threads from exiting immediately after creation, because they have work to do! // boost::asio::io_service::work: Constructor notifies the io_service that work is starting. device_driver_workP_.reset(new boost::asio::io_service::work(device_driver_io_service)); @@ -100,7 +91,6 @@ class KukaFRIdriver : public std::enable_shared_from_this< std::string(std::get(params)), grl::robot::arm::KukaFRIClientDataDriver< LowLevelStepAlgorithmType>::run_automatically)) - ); // flatbuffersbuilder does not yet exist m_logFileBufferBuilderP = std::make_shared(); @@ -114,7 +104,6 @@ class KukaFRIdriver : public std::enable_shared_from_this< ~KukaFRIdriver() { device_driver_workP_.reset(); - if (driver_threadP) { device_driver_io_service.stop(); driver_threadP->join(); @@ -139,15 +128,13 @@ class KukaFRIdriver : public std::enable_shared_from_this< // scale velocity down to a single timestep. In other words multiply each // velocity by the number of seconds in a tick, likely 0.001-0.005 - boost::transform( - maxVel, maxVel.begin(), - std::bind2nd(std::multiplies(), + boost::transform( maxVel, maxVel.begin(), std::bind2nd(std::multiplies(), getSecondsPerTick())); return maxVel; } - struct MicrosecondClock + struct MicrosecondClock { using rep = int64_t; /// 1 microsecond @@ -181,9 +168,7 @@ class KukaFRIdriver : public std::enable_shared_from_this< int64_t nanosecs = friTimeStamp.nanosec; int64_t microseconds = seconds *1000000 + nanosecs/1000; typename MicrosecondClock::time_point fritp = typename MicrosecondClock::time_point(typename MicrosecondClock::duration(microseconds)); - return KukaTimeToCommonTime(fritp); - } /** @@ -213,9 +198,9 @@ class KukaFRIdriver : public std::enable_shared_from_this< armState.velocity_limits.clear(); armState.velocity_limits = getMaxVel(); - // This is the key point where the arm's motion goal command is updated and - // sent to the robot + // This is the key point where the arm's motion goal command is updated and sent to the robot // Set the FRI to the simulated joint positions + // Why set this check? if (this->m_haveReceivedRealDataCount > minimumConsecutiveSuccessesBeforeSendingCommands) { /// @todo TODO(ahundt) Need to eliminate this allocation boost::lock_guard lock(jt_mutex); @@ -228,9 +213,6 @@ class KukaFRIdriver : public std::enable_shared_from_this< /// KukaFRIClientDataDriver /// this is where we used to setup a new FRI command - // std::cout << "commandToSend: " << commandToSend << "\n" << - // "currentJointPos: " << currentJointPos << "\n" << "amountToMove: " << - // amountToMove << "\n" << "maxVel: " << maxvel << "\n"; } else { /// @todo TODO(ahundt) BUG: Need way to pass time to reach specified goal for position control and eliminate this allocation lowLevelStepAlgorithmCommandParamsP.reset(new typename LowLevelStepAlgorithmType::Params()); @@ -258,49 +240,6 @@ class KukaFRIdriver : public std::enable_shared_from_this< this->m_attemptedCommunicationConsecutiveFailureCount = 0; this->m_haveReceivedRealDataCount++; - // We have the real kuka state read from the device now - // update real joint angle data - armState.position.clear(); - grl::robot::arm::copy(friData_->monitoringMsg, - std::back_inserter(armState.position), - grl::revolute_joint_angle_open_chain_state_tag()); - - armState.torque.clear(); - grl::robot::arm::copy(friData_->monitoringMsg, - std::back_inserter(armState.torque), - grl::revolute_joint_torque_open_chain_state_tag()); - - armState.externalTorque.clear(); - grl::robot::arm::copy( - friData_->monitoringMsg, std::back_inserter(armState.externalTorque), - grl::revolute_joint_torque_external_open_chain_state_tag()); - - // only supported for kuka sunrise OS 1.9 - #ifdef KUKA_SUNRISE_1_9 - armState.externalForce.clear(); - grl::robot::arm::copy(friData_->monitoringMsg, - std::back_inserter(armState.externalForce), - grl::cartesian_external_force_tag()); - #endif // KUKA_SUNRISE_1_9 - armState.commandedPosition.clear(); - grl::robot::arm::copy( - friData_->monitoringMsg, std::back_inserter(armState.commandedPosition), - grl::revolute_joint_angle_open_chain_command_tag()); - - armState.commandedTorque.clear(); - grl::robot::arm::copy( - friData_->monitoringMsg, std::back_inserter(armState.commandedTorque), - grl::revolute_joint_torque_open_chain_command_tag()); - - armState.ipoJointPosition.clear(); - grl::robot::arm::copy( - friData_->monitoringMsg, - std::back_inserter(armState.ipoJointPosition), - grl::revolute_joint_angle_interpolated_open_chain_state_tag()); - - armState.sendPeriod = std::chrono::milliseconds( - grl::robot::arm::get(friData_->monitoringMsg, grl::time_step_tag())); - oneKUKAiiwaStateBuffer(); armState.time_event_stamp = time_event_stamp; saveToDisk(); @@ -483,6 +422,53 @@ class KukaFRIdriver : public std::enable_shared_from_this< bool oneKUKAiiwaStateBuffer() { + // We have the real kuka state read from the device now + // update real joint angle data + armState.position.clear(); + grl::robot::arm::copy(friData_->monitoringMsg, + std::back_inserter(armState.position), + grl::revolute_joint_angle_open_chain_state_tag()); + + armState.torque.clear(); + grl::robot::arm::copy(friData_->monitoringMsg, + std::back_inserter(armState.torque), + grl::revolute_joint_torque_open_chain_state_tag()); + + armState.externalTorque.clear(); + grl::robot::arm::copy( + friData_->monitoringMsg, std::back_inserter(armState.externalTorque), + grl::revolute_joint_torque_external_open_chain_state_tag()); + + // only supported for kuka sunrise OS 1.9 + #ifdef KUKA_SUNRISE_1_9 + armState.externalForce.clear(); + grl::robot::arm::copy(friData_->monitoringMsg, + std::back_inserter(armState.externalForce), + grl::cartesian_external_force_tag()); + #endif // KUKA_SUNRISE_1_9 + armState.commandedPosition.clear(); + grl::robot::arm::copy( + friData_->monitoringMsg, std::back_inserter(armState.commandedPosition), + grl::revolute_joint_angle_open_chain_command_tag()); + + armState.commandedTorque.clear(); + grl::robot::arm::copy( + friData_->monitoringMsg, std::back_inserter(armState.commandedTorque), + grl::revolute_joint_torque_open_chain_command_tag()); + + armState.ipoJointPosition.clear(); + grl::robot::arm::copy( + friData_->monitoringMsg, + std::back_inserter(armState.ipoJointPosition), + grl::revolute_joint_angle_interpolated_open_chain_state_tag()); + + armState.sendPeriod = std::chrono::milliseconds( + grl::robot::arm::get(friData_->monitoringMsg, grl::time_step_tag())); + + + + + std::string RobotName = std::string(std::get(params_)); std:: string destination = std::string(std::get(params_)); @@ -490,15 +476,15 @@ class KukaFRIdriver : public std::enable_shared_from_this< int16_t portOnRemote = std::stoi(std::string(std::get(params_))); int16_t portOnController = std::stoi(std::string(std::get(params_))); - std::string basename = RobotName; //std::get<0>(params); + std::string basename = RobotName; - bool max_control_force_stop_ = false; - std::vector joint_stiffness_(KUKA::LBRState::NUM_DOF, 0); - std::vector joint_damping_(KUKA::LBRState::NUM_DOF, 0); - std::vector joint_AccelerationRel_(KUKA::LBRState::NUM_DOF, 0); - std::vector joint_VelocityRel_(KUKA::LBRState::NUM_DOF, 0); - bool updateMinimumTrajectoryExecutionTime = false; - double minimumTrajectoryExecutionTime = 4; + // bool max_control_force_stop_ = false; + // std::vector joint_stiffness_(KUKA::LBRState::NUM_DOF, 0); + // std::vector joint_damping_(KUKA::LBRState::NUM_DOF, 0); + // std::vector joint_AccelerationRel_(KUKA::LBRState::NUM_DOF, 0); + // std::vector joint_VelocityRel_(KUKA::LBRState::NUM_DOF, 0); + // bool updateMinimumTrajectoryExecutionTime = false; + // double minimumTrajectoryExecutionTime = 4; //Cartesian Impedance Values // grl::flatbuffer::Vector3d cart_stiffness_trans_ = grl::flatbuffer::Vector3d(0,0,0); @@ -512,9 +498,9 @@ class KukaFRIdriver : public std::enable_shared_from_this< // grl::flatbuffer::EulerPose cart_max_ctrl_force_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(0,0,0), grl::flatbuffer::EulerRotation(0.,0.,0., grl::flatbuffer::EulerOrder::xyz)); // double nullspace_stiffness_ = 0; // double nullspace_damping_ = 0; - bool updatePortOnRemote = false; + // bool updatePortOnRemote = false; - bool updatePortOnController = false; + // bool updatePortOnController = false; // Resolve the data in FRIMonitoringMessage // Don't match the FRIMessage.pb.h @@ -584,10 +570,7 @@ class KukaFRIdriver : public std::enable_shared_from_this< auto bns = m_logFileBufferBuilderP->CreateString(basename); double duration = boost::chrono::high_resolution_clock::now().time_since_epoch().count(); - - bool OK = true; - int64_t sequenceNumber = 0; copy(monitoringMsg, armState); @@ -605,11 +588,11 @@ class KukaFRIdriver : public std::enable_shared_from_this< flatbuffers::Offset setJointImpedance = 0; // normalized joint accelerations/velocities from 0 to 1 relative to system capabilities // how to get the acceleration of the robot? There is no acceleration information in KukaState (armState). - flatbuffers::Offset setSmartServo = grl::toFlatBuffer(*m_logFileBufferBuilderP, joint_AccelerationRel_, joint_VelocityRel_, updateMinimumTrajectoryExecutionTime, minimumTrajectoryExecutionTime); + flatbuffers::Offset setSmartServo = 0; //grl::toFlatBuffer(*m_logFileBufferBuilderP, joint_AccelerationRel_, joint_VelocityRel_, updateMinimumTrajectoryExecutionTime, minimumTrajectoryExecutionTime); // FRI configuration parameters - flatbuffers::Offset FRIConfig = grl::toFlatBuffer(*m_logFileBufferBuilderP, overlayType, connectionInfo, updatePortOnRemote, portOnRemote, updatePortOnController, portOnController); - std::vector> tools; - std::vector> processData_vec; + flatbuffers::Offset FRIConfig = 0; //grl::toFlatBuffer(*m_logFileBufferBuilderP, overlayType, connectionInfo, updatePortOnRemote, portOnRemote, updatePortOnController, portOnController); + // std::vector> tools; + // std::vector> processData_vec; // @TODO(Chunting) Initialize Pose with 0, we ought to calculate it later with ::Transformation; // Also assign the configuration parameters with random values, we can figure them out later. // for(int i=0; i torqueSensorLimits(KUKA::LBRState::NUM_DOF,0.1); - std::string hardwareVersion("hardvareVersion"); - bool isReadyToMove = true; - bool isMastered = true; + // Set it up in the configuration file + // std::vector torqueSensorLimits(KUKA::LBRState::NUM_DOF,0.1); + // std::string hardwareVersion("hardvareVersion"); + // bool isReadyToMove = true; + // bool isMastered = true; // flatbuffers::Offset monitorConfig = grl::toFlatBuffer( // *m_logFileBufferBuilderP, // hardwareVersion, @@ -712,7 +695,6 @@ class KukaFRIdriver : public std::enable_shared_from_this< RobotName, destination, source, - // duration, armState.time_event_stamp, false, controlState, // if false, then don't record the value false, kukaiiwaArmConfiguration, @@ -746,7 +728,7 @@ class KukaFRIdriver : public std::enable_shared_from_this< #endif // HAVE_spdlog ]() mutable { - std::cout<< "Save recording data from KukaLBRiiwa to disk..." << std::endl; + std::string currentWorkingDir = grl::GetCurrentWorkingDir(); std::cout<< currentWorkingDir<<"/"< Date: Wed, 24 Jan 2018 21:19:39 -0500 Subject: [PATCH 038/102] Read data from binary file --- include/grl/flatbuffer/flatbuffer.hpp | 13 +-- include/grl/kuka/KukaFRIClientDataDriver.hpp | 37 ++++--- include/grl/kuka/KukaFRIalgorithm.hpp | 64 ++++++----- include/grl/kuka/KukaFRIdriver.hpp | 103 ++++-------------- .../grl/kuka/readKukaiiwaFromFlatbuffer.hpp | 49 +++++++++ .../v_repExtAtracsysFusionTrack.cpp | 11 +- test/CMakeLists.txt | 2 + test/KukaFRITest.cpp | 6 +- test/readFRIFBTest.cpp | 11 ++ 9 files changed, 157 insertions(+), 139 deletions(-) create mode 100644 include/grl/kuka/readKukaiiwaFromFlatbuffer.hpp create mode 100644 test/readFRIFBTest.cpp diff --git a/include/grl/flatbuffer/flatbuffer.hpp b/include/grl/flatbuffer/flatbuffer.hpp index 3ca1607..3fac6fc 100644 --- a/include/grl/flatbuffer/flatbuffer.hpp +++ b/include/grl/flatbuffer/flatbuffer.hpp @@ -2,6 +2,7 @@ #define GRL_FLATBUFFER_HPP #include /* defines FILENAME_MAX */ +#include // #define WINDOWS /* uncomment this line to use it for windows.*/ #ifdef WINDOWS #include @@ -36,8 +37,8 @@ namespace grl { bool ok = flatbuffers::LoadFile(schemaPath.c_str(), binary, &schemafile) && flatbuffers::LoadFile(jsonPath.c_str(), binary, &jsonfile); if (!ok) { - printf("couldn't load files!\n"); - return nullptr; + printf("couldn't load files!\n"); + return nullptr; } // parse schema first, so we can use it to parse the data after @@ -74,7 +75,7 @@ namespace grl { { std::string current_working_dir = GetCurrentWorkingDir(); - std::cout << "The current working dir: " << current_working_dir << std::endl; + //std::cout << "The current working dir: " << current_working_dir << std::endl; includePaths.push_back(current_working_dir); } @@ -85,7 +86,7 @@ namespace grl { } else { - std::cout << "a full path wasn't passed, so check all the include paths" << std::endl; + //std::cout << "a full path wasn't passed, so check all the include paths" << std::endl; for(auto includePath : includePaths) { std::string fbs_trypath = flatbuffers::ConCatPathFileName(includePath, fbs_filename); @@ -113,7 +114,6 @@ namespace grl { include_directories.push_back(includePaths[i].c_str()); } include_directories.push_back(nullptr); - return parser.Parse(schemafile.c_str(), &include_directories[0]); } @@ -160,7 +160,7 @@ namespace grl { ///////////////////////////////////// if(!binary_file_path.empty()) { - ok = ok && flatbuffers::SaveFile(binary_file_path.c_str(), reinterpret_cast(buffer), size, write_binary_stream); + ok = ok && flatbuffers::SaveFile(binary_file_path.c_str(), reinterpret_cast(buffer), size, write_binary_stream); } //////////////////////////////////////////// @@ -184,6 +184,5 @@ namespace grl { } return ok; } - } // namespace grl #endif // GRL_FLATBUFFER_HPP \ No newline at end of file diff --git a/include/grl/kuka/KukaFRIClientDataDriver.hpp b/include/grl/kuka/KukaFRIClientDataDriver.hpp index 1bf5bc2..663dd4b 100644 --- a/include/grl/kuka/KukaFRIClientDataDriver.hpp +++ b/include/grl/kuka/KukaFRIClientDataDriver.hpp @@ -73,8 +73,8 @@ void decode(KUKA::FRI::ClientData &friData, std::size_t msg_size) { std::string("\n"))); return; } - // KUKA::FRI::ESessionState - friData.lastState = grl::robot::arm::get(friData.monitoringMsg, KUKA::FRI::ESessionState()); + // ::SessionState + friData.lastState = static_cast(grl::robot::arm::get(friData.monitoringMsg, ::FRISessionState())); } /// @brief Default LowLevelStepAlgorithmType @@ -192,9 +192,9 @@ struct LinearInterpolation { // send the command grl::robot::arm::set(friData.commandMsg, commandToSend, grl::revolute_joint_angle_open_chain_command_tag()); - std::cout << "commandToSend: " << commandToSend << "\n" << - "currentJointPos: " << currentJointPos << "\n" << - "amountToMove: " << amountToMove << "\n" ; + // std::cout << "commandToSend: " << commandToSend << "\n" << + // "currentJointPos: " << currentJointPos << "\n" << + // "amountToMove: " << amountToMove << "\n" ; /// copy value for debugging, makes viewing in a debugger easier double ripoJointPos[7]; @@ -291,12 +291,12 @@ std::size_t encode(LowLevelStepAlgorithmType &step_alg, friData.commandMsg.header.sequenceCounter = friData.sequenceCounter++; friData.commandMsg.header.reflectedSequenceCounter = friData.monitoringMsg.header.sequenceCounter; - KUKA::FRI::ESessionState sessionState = grl::robot::arm::get(friData.monitoringMsg, KUKA::FRI::ESessionState()); + ::FRISessionState sessionState = grl::robot::arm::get(friData.monitoringMsg, ::FRISessionState()); if ((step_alg.hasCommandData() && - (sessionState == KUKA::FRI::COMMANDING_WAIT || sessionState == KUKA::FRI::COMMANDING_ACTIVE))) + (sessionState == ::FRISessionState::FRISessionState_COMMANDING_WAIT || sessionState == ::FRISessionState::FRISessionState_COMMANDING_ACTIVE))) { - KUKA::FRI::EClientCommandMode commandMode = grl::robot::arm::get(friData.monitoringMsg, KUKA::FRI::EClientCommandMode()); + ::ClientCommandMode commandMode = grl::robot::arm::get(friData.monitoringMsg, ::ClientCommandMode()); switch (commandMode) { case ClientCommandMode_POSITION: step_alg.lowLevelTimestep(friData, revolute_joint_angle_open_chain_command_tag()); @@ -319,7 +319,7 @@ std::size_t encode(LowLevelStepAlgorithmType &step_alg, } } else if (!(friData.commandMsg.has_commandData && step_alg.hasCommandData() && - (sessionState == KUKA::FRI::COMMANDING_WAIT || sessionState == KUKA::FRI::COMMANDING_ACTIVE))) + (sessionState == ::FRISessionState::FRISessionState_COMMANDING_WAIT || sessionState == ::FRISessionState::FRISessionState_COMMANDING_ACTIVE))) { // copy current measured joint position to commanded position only if we // *don't* have new command data @@ -392,22 +392,31 @@ void copy(const FRIMonitoringMessage &monitoringMsg, KukaState &state) { revolute_joint_angle_open_chain_state_tag()); copy(monitoringMsg, std::back_inserter(state.torque), revolute_joint_torque_open_chain_state_tag()); + copy(monitoringMsg, std::back_inserter(state.externalTorque), + grl::revolute_joint_torque_external_open_chain_state_tag()); + // only supported for kuka sunrise OS 1.9 + #ifdef KUKA_SUNRISE_1_9 + copy(friData_->monitoringMsg, std::back_inserter(state.externalForce), + grl::cartesian_external_force_tag()); + #endif // KUKA_SUNRISE_1_9 copy(monitoringMsg, std::back_inserter(state.commandedPosition), revolute_joint_angle_open_chain_command_tag()); copy(monitoringMsg, std::back_inserter(state.commandedTorque), revolute_joint_torque_open_chain_command_tag()); copy(monitoringMsg, std::back_inserter(state.ipoJointPosition), revolute_joint_angle_interpolated_open_chain_state_tag()); + state.sendPeriod = std::chrono::milliseconds( + get(monitoringMsg, grl::time_step_tag())); state.sessionState = static_cast( - get(monitoringMsg, KUKA::FRI::ESessionState())); + get(monitoringMsg, ::FRISessionState())); state.connectionQuality = static_cast( - get(monitoringMsg, KUKA::FRI::EConnectionQuality())); + get(monitoringMsg, ::FRISessionState())); state.safetyState = static_cast( - get(monitoringMsg, KUKA::FRI::ESafetyState())); + get(monitoringMsg, ::SafetyState())); state.operationMode = static_cast( - get(monitoringMsg, KUKA::FRI::EOperationMode())); + get(monitoringMsg, ::OperationMode())); state.driveState = static_cast( - get(monitoringMsg, KUKA::FRI::EDriveState())); + get(monitoringMsg, ::DriveState())); /// @todo fill out missing state update steps } diff --git a/include/grl/kuka/KukaFRIalgorithm.hpp b/include/grl/kuka/KukaFRIalgorithm.hpp index 51d30ad..6d18935 100644 --- a/include/grl/kuka/KukaFRIalgorithm.hpp +++ b/include/grl/kuka/KukaFRIalgorithm.hpp @@ -134,54 +134,55 @@ namespace grl { namespace robot { void copy(const FRICommandMessage& commandMsg, OutputIterator it, revolute_joint_angle_open_chain_command_tag){ if (commandMsg.has_commandData) kuka::detail::copyJointState(commandMsg.commandData.jointPosition.value.arg,it,commandMsg.commandData.has_jointPosition); } - /// KUKA::FRI::ESafetyState is defined in #include "friClientIf.h" - /// @todo consider using another default value, or perhaps boost::optional? - KUKA::FRI::ESafetyState get(const FRIMonitoringMessage& monitoringMsg, const KUKA::FRI::ESafetyState) { - KUKA::FRI::ESafetyState state = KUKA::FRI::ESafetyState::NORMAL_OPERATION; + /// ::SafetyState is defined in #include "friClientIf.h" + /// @todo consider using another default value, or perhaps boost::optional<::SafetyState>? + ::SafetyState get(const FRIMonitoringMessage& monitoringMsg, const ::SafetyState) { + ::SafetyState state = ::SafetyState::SafetyState_NORMAL_OPERATION; + if (monitoringMsg.has_robotInfo) { if (monitoringMsg.robotInfo.has_safetyState) - return static_cast(monitoringMsg.robotInfo.safetyState); + return monitoringMsg.robotInfo.safetyState; } return state; } - KUKA::FRI::EOperationMode get(const FRIMonitoringMessage& monitoringMsg, const KUKA::FRI::EOperationMode) { - KUKA::FRI::EOperationMode state = KUKA::FRI::EOperationMode::TEST_MODE_1; + ::OperationMode get(const FRIMonitoringMessage& monitoringMsg, const ::OperationMode) { + ::OperationMode state = ::OperationMode::OperationMode_TEST_MODE_1; if (monitoringMsg.has_robotInfo) { if (monitoringMsg.robotInfo.has_operationMode) - state = static_cast(monitoringMsg.robotInfo.operationMode); + state = monitoringMsg.robotInfo.operationMode; } return state; } - KUKA::FRI::EControlMode get(const FRIMonitoringMessage& monitoringMsg, const KUKA::FRI::EControlMode) { - KUKA::FRI::EControlMode state = KUKA::FRI::EControlMode::NO_CONTROL; + ::ControlMode get(const FRIMonitoringMessage& monitoringMsg, const ::ControlMode) { + ::ControlMode state = ::ControlMode::ControlMode_NO_CONTROLMODE; if (monitoringMsg.has_robotInfo) { if (monitoringMsg.robotInfo.has_controlMode) - state = static_cast(monitoringMsg.robotInfo.controlMode); + state = monitoringMsg.robotInfo.controlMode; } return state; } - KUKA::FRI::EClientCommandMode get(const FRIMonitoringMessage& monitoringMsg, const KUKA::FRI::EClientCommandMode) { - KUKA::FRI::EClientCommandMode state = KUKA::FRI::EClientCommandMode::NO_COMMAND_MODE; + ::ClientCommandMode get(const FRIMonitoringMessage& monitoringMsg, const ::ClientCommandMode) { + ::ClientCommandMode state = ::ClientCommandMode::ClientCommandMode_NO_COMMAND_MODE; if (monitoringMsg.has_ipoData) { if (monitoringMsg.ipoData.has_clientCommandMode) - state = static_cast(monitoringMsg.ipoData.clientCommandMode); + state = monitoringMsg.ipoData.clientCommandMode; } return state; } - KUKA::FRI::EOverlayType get(const FRIMonitoringMessage& monitoringMsg, const KUKA::FRI::EOverlayType) { - KUKA::FRI::EOverlayType state = KUKA::FRI::EOverlayType::NO_OVERLAY; + ::OverlayType get(const FRIMonitoringMessage& monitoringMsg, const ::OverlayType) { + ::OverlayType state = ::OverlayType::OverlayType_NO_OVERLAY; if (monitoringMsg.has_ipoData) { if (monitoringMsg.ipoData.has_overlayType) - state = static_cast(monitoringMsg.ipoData.overlayType); + state = monitoringMsg.ipoData.overlayType; } return state; } - KUKA::FRI::EDriveState get(const FRIMonitoringMessage& _message, const KUKA::FRI::EDriveState) + ::DriveState get(const FRIMonitoringMessage& _message, const ::DriveState) { tRepeatedIntArguments *values = (tRepeatedIntArguments *)_message.robotInfo.driveState.arg; @@ -191,24 +192,24 @@ namespace grl { namespace robot { int state = (int)values->value[i]; if (state != firstState) { - return KUKA::FRI::EDriveState::TRANSITIONING; + return ::DriveState::DriveState_TRANSITIONING; } } - return (KUKA::FRI::EDriveState)firstState; + return (::DriveState)firstState; } - KUKA::FRI::ESessionState get(const FRIMonitoringMessage& monitoringMsg, const KUKA::FRI::ESessionState){ - KUKA::FRI::ESessionState KukaSessionState = KUKA::FRI::ESessionState::IDLE; + ::FRISessionState get(const FRIMonitoringMessage& monitoringMsg, const ::FRISessionState){ + ::FRISessionState KukaSessionState = ::FRISessionState::FRISessionState_IDLE; if (monitoringMsg.has_connectionInfo) { - KukaSessionState = static_cast(monitoringMsg.connectionInfo.sessionState); + KukaSessionState = monitoringMsg.connectionInfo.sessionState; } return KukaSessionState; } - KUKA::FRI::EConnectionQuality get(const FRIMonitoringMessage& monitoringMsg, const KUKA::FRI::EConnectionQuality){ - KUKA::FRI::EConnectionQuality KukaQuality = KUKA::FRI::EConnectionQuality::POOR; + ::FRIConnectionQuality get(const FRIMonitoringMessage& monitoringMsg, const ::FRIConnectionQuality){ + ::FRIConnectionQuality KukaQuality = ::FRIConnectionQuality::FRIConnectionQuality_POOR; if (monitoringMsg.has_connectionInfo) { - KukaQuality = static_cast(monitoringMsg.connectionInfo.quality); + KukaQuality = monitoringMsg.connectionInfo.quality; } return KukaQuality; } @@ -226,7 +227,16 @@ namespace grl { namespace robot { return KukaSendPeriod; } - std::size_t get(const FRIMonitoringMessage& monitoringMsg,const kuka::receive_multiplier){ + std::size_t get(const FRIMonitoringMessage& monitoringMsg,const std::size_t NUM_DOF){ + std::size_t numberofJoints = 7; + if (monitoringMsg.has_robotInfo) { + if (monitoringMsg.robotInfo.has_numberOfJoints) + return monitoringMsg.robotInfo.numberOfJoints; + } + return numberofJoints; + } + + std::size_t get(const FRIMonitoringMessage& monitoringMsg,const kuka::receive_multiplier){ std::size_t KukaReceiveMultiplier = 0; if (monitoringMsg.has_connectionInfo) { if (monitoringMsg.connectionInfo.has_receiveMultiplier) diff --git a/include/grl/kuka/KukaFRIdriver.hpp b/include/grl/kuka/KukaFRIdriver.hpp index 4765bf0..c449142 100644 --- a/include/grl/kuka/KukaFRIdriver.hpp +++ b/include/grl/kuka/KukaFRIdriver.hpp @@ -5,6 +5,8 @@ #define FLATBUFFERS_DEBUG_VERIFICATION_FAILURE #include "grl/kuka/KukaFRIClientDataDriver.hpp" +#include +#include //#include "cartographer/common/time.h" struct KukaState; @@ -130,7 +132,6 @@ class KukaFRIdriver : public std::enable_shared_from_this(), getSecondsPerTick())); - return maxVel; } @@ -157,8 +158,7 @@ class KukaFRIdriver : public std::enable_shared_from_this(Kukatime.time_since_epoch()) + - std::chrono::seconds(cartographer::common::kUtsEpochOffsetFromUnixEpochInSeconds)); + std::chrono::duration_cast(Kukatime.time_since_epoch())); } cartographer::common::Time FRITimeStampToCommonTime(const ::TimeStamp &friTimeStamp) @@ -167,6 +167,10 @@ class KukaFRIdriver : public std::enable_shared_from_this lowLevelStepAlgorithmCommandParamsP; - /// @todo probably only need to set this once armState.velocity_limits.clear(); armState.velocity_limits = getMaxVel(); - // This is the key point where the arm's motion goal command is updated and sent to the robot // Set the FRI to the simulated joint positions // Why set this check? if (this->m_haveReceivedRealDataCount > minimumConsecutiveSuccessesBeforeSendingCommands) { /// @todo TODO(ahundt) Need to eliminate this allocation boost::lock_guard lock(jt_mutex); - boost::container::static_vector jointStateToCommand; boost::copy(armState.commandedPosition,std::back_inserter(jointStateToCommand)); // pass time to reach specified goal for position control @@ -212,7 +211,6 @@ class KukaFRIdriver : public std::enable_shared_from_thism_haveReceivedRealDataCount++; oneKUKAiiwaStateBuffer(); - armState.time_event_stamp = time_event_stamp; + saveToDisk(); } else { @@ -424,57 +422,10 @@ class KukaFRIdriver : public std::enable_shared_from_thismonitoringMsg, - std::back_inserter(armState.position), - grl::revolute_joint_angle_open_chain_state_tag()); - - armState.torque.clear(); - grl::robot::arm::copy(friData_->monitoringMsg, - std::back_inserter(armState.torque), - grl::revolute_joint_torque_open_chain_state_tag()); - - armState.externalTorque.clear(); - grl::robot::arm::copy( - friData_->monitoringMsg, std::back_inserter(armState.externalTorque), - grl::revolute_joint_torque_external_open_chain_state_tag()); - - // only supported for kuka sunrise OS 1.9 - #ifdef KUKA_SUNRISE_1_9 - armState.externalForce.clear(); - grl::robot::arm::copy(friData_->monitoringMsg, - std::back_inserter(armState.externalForce), - grl::cartesian_external_force_tag()); - #endif // KUKA_SUNRISE_1_9 - armState.commandedPosition.clear(); - grl::robot::arm::copy( - friData_->monitoringMsg, std::back_inserter(armState.commandedPosition), - grl::revolute_joint_angle_open_chain_command_tag()); - - armState.commandedTorque.clear(); - grl::robot::arm::copy( - friData_->monitoringMsg, std::back_inserter(armState.commandedTorque), - grl::revolute_joint_torque_open_chain_command_tag()); - - armState.ipoJointPosition.clear(); - grl::robot::arm::copy( - friData_->monitoringMsg, - std::back_inserter(armState.ipoJointPosition), - grl::revolute_joint_angle_interpolated_open_chain_state_tag()); - - armState.sendPeriod = std::chrono::milliseconds( - grl::robot::arm::get(friData_->monitoringMsg, grl::time_step_tag())); - - - - - std::string RobotName = std::string(std::get(params_)); - std:: string destination = std::string(std::get(params_)); std::string source = std::string(std::get(params_)); int16_t portOnRemote = std::stoi(std::string(std::get(params_))); - int16_t portOnController = std::stoi(std::string(std::get(params_))); std::string basename = RobotName; @@ -509,27 +460,16 @@ class KukaFRIdriver : public std::enable_shared_from_thismonitoringMsg; - // RobotInfo - // how to use pb_callback_t driveState in RobotInfo? - ::RobotInfo robotInfo = monitoringMsg.robotInfo; - int NUM_DOF = robotInfo.has_numberOfJoints?robotInfo.numberOfJoints:KUKA::LBRState::NUM_DOF; - ::ControlMode controlMode = robotInfo.controlMode; - ::SafetyState safetyState = robotInfo.safetyState; - ::OperationMode operationMode = robotInfo.operationMode; - - // ConnectionInfo - // how to use uint32_t receiveMultiplier + std::size_t NUM_DOF = grl::robot::arm::get(monitoringMsg, KUKA::LBRState::NUM_DOF);; + ::ControlMode controlMode = grl::robot::arm::get(monitoringMsg, ::ControlMode()); + ::SafetyState safetyState = grl::robot::arm::get(monitoringMsg, ::SafetyState()); + ::OperationMode operationMode = grl::robot::arm::get(monitoringMsg, ::OperationMode()); ::ConnectionInfo connectionInfo = monitoringMsg.connectionInfo; - ::FRISessionState friSessionState = connectionInfo.sessionState; - ::FRIConnectionQuality friConnectionQuality = connectionInfo.quality; - // MessageIpoData - // JointValues jointPosition; double trackingPerformance; - ::MessageIpoData ipoData = monitoringMsg.ipoData; - ::ClientCommandMode clientCommandMode = ipoData.clientCommandMode; - ::OverlayType overlayType = ipoData.overlayType; + ::FRISessionState friSessionState = grl::robot::arm::get(monitoringMsg, ::FRISessionState()); + ::FRIConnectionQuality friConnectionQuality = grl::robot::arm::get(monitoringMsg, ::FRIConnectionQuality()); + ::ClientCommandMode clientCommandMode = grl::robot::arm::get(monitoringMsg, ::ClientCommandMode()); - // No MessageHeader in flatbuffer objects - ::MessageHeader messageHeader = monitoringMsg.header; + ::OverlayType overlayType = grl::robot::arm::get(monitoringMsg, ::OverlayType()); ::MessageMonitorData messageMonitorData = monitoringMsg.monitorData; @@ -552,7 +492,7 @@ class KukaFRIdriver : public std::enable_shared_from_this controlState = grl::toFlatBuffer(*m_logFileBufferBuilderP, basename, sequenceNumber++, duration, armState, armControlMode); // flatbuffers::Offset setCartesianImpedance = grl::toFlatBuffer(*m_logFileBufferBuilderP, cart_stiffness_, cart_damping_, // nullspace_stiffness_, nullspace_damping_, cart_max_path_deviation_, cart_max_ctrl_vel_, cart_max_ctrl_force_, max_control_force_stop_); @@ -675,7 +610,7 @@ class KukaFRIdriver : public std::enable_shared_from_this &jointStateInterpolated, externalState, // flatbuffers::Offset &externalState, friSessionState, - robotInfo.operationMode, + operationMode, cartesianWrench); // Set it up in the configuration file // std::vector torqueSensorLimits(KUKA::LBRState::NUM_DOF,0.1); diff --git a/include/grl/kuka/readKukaiiwaFromFlatbuffer.hpp b/include/grl/kuka/readKukaiiwaFromFlatbuffer.hpp new file mode 100644 index 0000000..28b643a --- /dev/null +++ b/include/grl/kuka/readKukaiiwaFromFlatbuffer.hpp @@ -0,0 +1,49 @@ +#ifndef GRL_READ_KUKAIIWA_FROM_FLATBUFFER +#define GRL_READ_KUKAIIWA_FROM_FLATBUFFER + +#define FLATBUFFERS_DEBUG_VERIFICATION_FAILURE + +#include "flatbuffers/flatbuffers.h" +#include "grl/flatbuffer/JointState_generated.h" +#include "grl/flatbuffer/ArmControlState_generated.h" +#include "grl/flatbuffer/KUKAiiwa_generated.h" +#include "grl/flatbuffer/LinkObject_generated.h" +#include "grl/flatbuffer/Time_generated.h" +#include "grl/flatbuffer/flatbuffer.hpp" + +#include // For printing and file access. +#include +namespace grl { +void getDeviceTime(){ + std::string curWorkingDir = grl::GetCurrentWorkingDir(); + std::cout << "CurrentWorkingDir: " << curWorkingDir << std::endl; + FILE* file = fopen("2018_01_22_19_00_29_Kukaiiwa.iiwa", "rb"); + fseek(file, 0L, SEEK_END); + int length = ftell(file); + fseek(file, 0L, SEEK_SET); + char *data = new char[length]; + fread(data, sizeof(char), length, file); + fclose(file); + auto kukaStates = grl::flatbuffer::GetKUKAiiwaStates(data); + auto states = kukaStates->states(); + auto KUKAiiwaState = states->Get(0); + auto FRIMessage = KUKAiiwaState->FRIMessage(); + auto timeEvent = FRIMessage->timeStamp(); + auto device_time = timeEvent->device_time(); + std::cout<<"Device Time: "<< device_time << std::endl; + + std::size_t state_size = states->size(); + for(int i = 1; iGet(i); + FRIMessage = KUKAiiwaState->FRIMessage(); + timeEvent = FRIMessage->timeStamp(); + auto nextdevice_time = timeEvent->device_time(); + auto diff = nextdevice_time - device_time; + device_time = nextdevice_time; + std::cout<<"Time Diff: "<< diff < *inData = D.getInDataPtr(); recordWhileSimulationIsRunningG = inData->at(0).boolData[0]; - - std::string log_message = std::string("simExtAtracsysFusionTrackRecordWhileSimulationIsRunning: ") + boost::lexical_cast(recordWhileSimulationIsRunningG); - simAddStatusbarMessage(log_message.c_str()); - loggerPG->info(log_message); + if (fusionTrackPG && recordWhileSimulationIsRunningG) + { + std::string log_message = std::string("simExtAtracsysFusionTrackRecordWhileSimulationIsRunning: ") + boost::lexical_cast(recordWhileSimulationIsRunningG); + simAddStatusbarMessage(log_message.c_str()); + loggerPG->info(log_message); + fusionTrackPG->start_recording(); + } D.pushOutData(CLuaFunctionDataItem(success)); D.writeDataToLua(p); diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 340680e..0b56b63 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -95,6 +95,8 @@ if(TARGET KukaFRIClient OR FRI_Client_SDK_Cpp_FOUND) basis_add_executable(kukaiiwaTest.cpp)# ${GRL_FLATBUFFERS_OUTPUTS}) basis_target_link_libraries(kukaiiwaTest ${Boost_LIBRARIES} ${Boost_REGEX_LIBRARY} ${FRI_Client_SDK_Cpp_LIBRARIES} ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} KukaFRIClient ${FLATBUFFERS_STATIC_LIB}) + basis_add_executable(readFRIFBTest.cpp)# ${GRL_FLATBUFFERS_OUTPUTS}) + basis_target_link_libraries(readFRIFBTest ${Boost_LIBRARIES} ${Boost_REGEX_LIBRARY} ${FRI_Client_SDK_Cpp_LIBRARIES} ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} KukaFRIClient ${FLATBUFFERS_STATIC_LIB}) endif() diff --git a/test/KukaFRITest.cpp b/test/KukaFRITest.cpp index 0ce09e6..be1d367 100644 --- a/test/KukaFRITest.cpp +++ b/test/KukaFRITest.cpp @@ -292,7 +292,7 @@ int main(int argc, char* argv[]) // setting howToMove to HowToMove::remain_stationary block causes the robot to simply sit in place, which seems to work correctly. Enabling it causes the joint to rotate. if (howToMove != HowToMove::remain_stationary && - grl::robot::arm::get(friData->monitoringMsg,KUKA::FRI::ESessionState()) == KUKA::FRI::COMMANDING_ACTIVE) + grl::robot::arm::get(friData->monitoringMsg,::FRISessionState()) == ::FRISessionState::FRISessionState_COMMANDING_ACTIVE) { callIfMinPeriodPassed.execution( [&armState,&jointOffset,&delta,&delta_sum,joint_to_move]() { @@ -310,9 +310,9 @@ int main(int argc, char* argv[]) }); } - KUKA::FRI::ESessionState sessionState = grl::robot::arm::get(friData->monitoringMsg,KUKA::FRI::ESessionState()); + ::FRISessionState sessionState = grl::robot::arm::get(friData->monitoringMsg,::FRISessionState()); // copy current joint position to commanded position - if (sessionState == KUKA::FRI::COMMANDING_WAIT || sessionState == KUKA::FRI::COMMANDING_ACTIVE) + if (sessionState == ::FRISessionState::FRISessionState_COMMANDING_WAIT || sessionState == ::FRISessionState::FRISessionState_COMMANDING_ACTIVE) { if (howToMove == HowToMove::relative_position) { // go to a position relative to the current position diff --git a/test/readFRIFBTest.cpp b/test/readFRIFBTest.cpp new file mode 100644 index 0000000..1f53ce1 --- /dev/null +++ b/test/readFRIFBTest.cpp @@ -0,0 +1,11 @@ + +// Library includes +#define FLATBUFFERS_DEBUG_VERIFICATION_FAILURE +#include "grl/kuka/readKukaiiwaFromFlatbuffer.hpp" + + +int main(int argc, char* argv[]) +{ + grl::getDeviceTime(); + return 0; +} From 31c6ed89c7c3a6e12b8edc86c818a425ea97badc Mon Sep 17 00:00:00 2001 From: Chunting Date: Fri, 26 Jan 2018 18:09:05 -0500 Subject: [PATCH 039/102] Enable the recordDataScript() --- src/lua/robone.lua | 26 ++++++++++++++++++++++++-- 1 file changed, 24 insertions(+), 2 deletions(-) diff --git a/src/lua/robone.lua b/src/lua/robone.lua index dd8271e..f989b28 100644 --- a/src/lua/robone.lua +++ b/src/lua/robone.lua @@ -153,7 +153,7 @@ robone.cutBoneScript=function() jointHandles={-1,-1,-1,-1,-1,-1,-1} measuredJointHandles={-1,-1,-1,-1,-1,-1,-1} for i=1,7,1 do - jointHandles[i]=simGetObjectHandle('LBR_iiwa_14_R820_joint'..i) + jointHandles[i]=simGetObjectHandle('LBR_iiwa_14_R820_joint'..i) -- Retrieves an object handle based on its name. measuredJointHandles[i]=simGetObjectHandle('LBR_iiwa_14_R820_joint'..i..'#0') end @@ -505,7 +505,9 @@ robone.configureOpticalTracker=function() -- Start collecting data from the optical tracker simExtAtracsysFusionTrackStart() - simExtAtracsysFusionTrackRecordWhileSimulationIsRunning(false) + -- Test the below one + -- Enable the recordDataScript() to call the method below. + -- simExtAtracsysFusionTrackRecordWhileSimulationIsRunning(true) end @@ -516,6 +518,26 @@ robone.configureOpticalTracker=function() end +------------------------------------------------ +-- Record flatbuffers Data during simulation -- +------------------------------------------------ +robone.recordDataScript=function() + + -- Check if the required plugin is there: + if (grl.isModuleLoaded('KukaLBRiiwa')) then + simExtKukaLBRiiwaRecordWhileSimulationIsRunning(true) + else + simDisplayDialog('Warning','KukaLBRiiwa plugin was not found so data will not be logged.',sim_dlgstyle_ok,true,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1}) + end + + + if (grl.isModuleLoaded('') then + simExtAtracsysFusionTrackRecordWhileSimulationIsRunning(true) + else + simDisplayDialog('Warning','Atracsys plugin was not found so data will not be logged.',sim_dlgstyle_ok,true,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1}) + + end + -------------------------------------------------------------------------- -- Get External Joint Torque Data from KUKA iiwa real arm driver plugin -- -------------------------------------------------------------------------- From cac66097984dff80e2d3b0c9a6d596089666b92b Mon Sep 17 00:00:00 2001 From: Chunting Date: Thu, 1 Feb 2018 15:37:32 -0500 Subject: [PATCH 040/102] Plot timeEvent --- example/fusionTrackExample.cpp | 1 - include/grl/flatbuffer/flatbuffer.hpp | 2 +- include/grl/kuka/KukaDriver.hpp | 25 +- include/grl/kuka/KukaFRIClientDataDriver.hpp | 733 +++++++++--------- include/grl/kuka/KukaFRIdriver.hpp | 18 +- include/grl/kuka/KukaJAVAdriver.hpp | 126 ++- .../grl/kuka/readKukaiiwaFromFlatbuffer.hpp | 200 ++++- include/grl/sensor/FusionTrack.hpp | 3 +- include/grl/sensor/FusionTrackLogAndTrack.hpp | 74 +- .../sensor/readFusionTrackFromFlatbuffer.hpp | 103 +++ .../grl/vrep/InverseKinematicsVrepPlugin.hpp | 236 +++--- include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp | 23 +- .../include/cartographer/common/time.h | 3 +- src/lua/robone.lua | 18 +- test/CMakeLists.txt | 2 +- test/KukaFRITest.cpp | 299 ++++--- test/readFRIFBTest.cpp | 145 +++- 17 files changed, 1190 insertions(+), 821 deletions(-) create mode 100644 include/grl/sensor/readFusionTrackFromFlatbuffer.hpp diff --git a/example/fusionTrackExample.cpp b/example/fusionTrackExample.cpp index ba7e4b3..17f2422 100644 --- a/example/fusionTrackExample.cpp +++ b/example/fusionTrackExample.cpp @@ -141,7 +141,6 @@ int main(int argc, char **argv) { bool success = grl::FinishAndVerifyBuffer(fbb, KUKAiiwaFusionTrackMessage_vector); std::cout << "verifier success " << buffer_num << " : "<< success << std::endl; - // test_binary.fltk should now be named test_binary_0.fltk, test_binary_1.fltk, etc... std::string binary_file_path = binary_file_prefix + std::to_string(buffer_num) + binary_file_suffix; std::string json_file_path = json_file_prefix + std::to_string(buffer_num) + json_file_suffix; std::cout << "Reached single buffer capacity limit of " << static_cast(single_buffer_limit_bytes)/MegaByte << diff --git a/include/grl/flatbuffer/flatbuffer.hpp b/include/grl/flatbuffer/flatbuffer.hpp index 3fac6fc..34b47a5 100644 --- a/include/grl/flatbuffer/flatbuffer.hpp +++ b/include/grl/flatbuffer/flatbuffer.hpp @@ -45,7 +45,7 @@ namespace grl { const char *include_directories[] = { include_directory.c_str(), "flatbuffer", nullptr }; ok = parser.Parse(schemafile.c_str(), include_directories) && parser.Parse(jsonfile.c_str(), include_directories); - assert(ok); + assert(ok &&"LoadJSONFlatbuffer"); return ok; } diff --git a/include/grl/kuka/KukaDriver.hpp b/include/grl/kuka/KukaDriver.hpp index d3ce443..5d3f094 100644 --- a/include/grl/kuka/KukaDriver.hpp +++ b/include/grl/kuka/KukaDriver.hpp @@ -171,7 +171,7 @@ namespace grl { namespace robot { namespace arm { if(JAVAdriverP_.get() != nullptr) { if (debug) { - std::cout << "commandedpos:" << armState_.commandedPosition << "\n"; + std::cout << "commandedpos:" << armState_.commandedPosition << "\n"; } @@ -179,14 +179,14 @@ namespace grl { namespace robot { namespace arm { // Do some configuration if(boost::iequals(std::get(params_),std::string("FRI"))) { - // configure to send commands over FRI interface - JAVAdriverP_->set(flatbuffer::KUKAiiwaInterface::FRI,command_tag()); + // configure to send commands over FRI interface + JAVAdriverP_->set(flatbuffer::KUKAiiwaInterface::FRI,command_tag()); } if(boost::iequals(std::get(params_),std::string("FRI"))) { - // configure to send commands over FRI interface - JAVAdriverP_->set(flatbuffer::KUKAiiwaInterface::FRI,state_tag()); + // configure to send commands over FRI interface + JAVAdriverP_->set(flatbuffer::KUKAiiwaInterface::FRI,state_tag()); } @@ -195,10 +195,10 @@ namespace grl { namespace robot { namespace arm { if( boost::iequals(std::get(params_),std::string("JAVA"))) { - JAVAdriverP_->set(armState_.commandedPosition,revolute_joint_angle_open_chain_command_tag()); + JAVAdriverP_->set(armState_.commandedPosition,revolute_joint_angle_open_chain_command_tag()); - // configure to send commands over JAVA interface - JAVAdriverP_->set(flatbuffer::KUKAiiwaInterface::SmartServo,command_tag()); + // configure to send commands over JAVA interface + JAVAdriverP_->set(flatbuffer::KUKAiiwaInterface::SmartServo,command_tag()); } @@ -207,8 +207,8 @@ namespace grl { namespace robot { namespace arm { if( boost::iequals(std::get(params_),std::string("JAVA"))) { - JAVAdriverP_->get(armState_); - JAVAdriverP_->set(flatbuffer::KUKAiiwaInterface::SmartServo,state_tag()); + JAVAdriverP_->get(armState_); + JAVAdriverP_->set(flatbuffer::KUKAiiwaInterface::SmartServo,state_tag()); } } @@ -224,11 +224,10 @@ namespace grl { namespace robot { namespace arm { if( boost::iequals(std::get(params_),std::string("FRI"))) { - FRIdriverP_->get(armState_); - //JAVAdriverP_->getWrench(armState_); + FRIdriverP_->get(armState_); + //JAVAdriverP_->getWrench(armState_); } } - return haveNewData; } diff --git a/include/grl/kuka/KukaFRIClientDataDriver.hpp b/include/grl/kuka/KukaFRIClientDataDriver.hpp index 663dd4b..8ab60f0 100644 --- a/include/grl/kuka/KukaFRIClientDataDriver.hpp +++ b/include/grl/kuka/KukaFRIClientDataDriver.hpp @@ -50,15 +50,16 @@ namespace grl { namespace robot { namespace arm { -/// @brief internal function to decode KUKA FRI message buffer (using nanopb -/// decoder) for the KUKA FRI +/// @brief internal function to decode KUKA FRI message buffer (using nanopb decoder) for the KUKA FRI /// /// @note encode needs to be updated for each additional supported command type /// and when updating to newer FRI versions void decode(KUKA::FRI::ClientData &friData, std::size_t msg_size) { // The decoder was given a pointer to the monitoringMessage at initialization + // decode() is declared and definied in friMonitoringMessageDecoder.cpp (.h) + // All the stuffs in namespace KUKA::FRI are offered by KUKA side. if (!friData.decoder.decode(friData.receiveBuffer, msg_size)) { - BOOST_THROW_EXCEPTION(std::runtime_error("Error decoding received FRI data, the message may be missing or corrupted. This error is most likely due to the application running on the KUKA robot's side of the connection going down or disabling FRI, so check the robot and the JAVA side of the system.")); + BOOST_THROW_EXCEPTION(std::runtime_error("Error decoding received FRI data, the message may be missing or corrupted. This error is most likely due to the application running on the KUKA robot's side of the connection going down or disabling FRI, so check the robot and the JAVA side of the system.")); } // check message type @@ -99,8 +100,8 @@ struct LinearInterpolation { // no action by default template void lowLevelTimestep(ArmDataType &, CommandModeType &) { - // need to tag dispatch here - BOOST_VERIFY(false); // not yet supported + // need to tag dispatch here + BOOST_VERIFY(false); // not yet supported } template @@ -140,8 +141,8 @@ struct LinearInterpolation { /// single timestep in ms, the time duration between when udp packets are expected to be sent in milliseconds /// uint32_t sendPeriod in ConnectionInfo; int thisTimeStepMS(grl::robot::arm::get(friData.monitoringMsg, grl::time_step_tag())); - std::cout << "Send Period: " << thisTimeStepMS << std::endl - << "goal_position_command_time_duration_remaining: " <(thisTimeStepMS) / 1000); /// the fraction of the distance to the goal that should be traversed this tick @@ -364,22 +365,24 @@ void update_state(boost::asio::ip::udp::socket &socket, // get a local clock timestamp, then the latest frame from the device, then another timestamp timeEvent.local_request_time = cartographer::common::UniversalTimeScaleClock::now(); + //auto start_time = std::chrono::high_resolution_clock::now(); receive_bytes_transferred = socket.receive_from( - boost::asio::buffer(friData.receiveBuffer, - KUKA::FRI::FRI_MONITOR_MSG_MAX_SIZE), + boost::asio::buffer(friData.receiveBuffer, KUKA::FRI::FRI_MONITOR_MSG_MAX_SIZE), sender_endpoint, message_flags, receive_ec); + //auto end_time = std::chrono::high_resolution_clock::now(); + // std::cout << "Time Delay in chrono: "<(end_time - start_time).count() << " microseconds" << std::endl; + timeEvent.local_receive_time = cartographer::common::UniversalTimeScaleClock::now(); + + // std::cout << "Time Delay in UniversalTimeScaleClock: "<= - friData.monitoringMsg.connectionInfo.receiveMultiplier) { - send_bytes_transferred = encode(step_alg, friData, send_ec); - if (send_ec) - return; - socket.send(boost::asio::buffer(friData.sendBuffer, send_bytes_transferred), - message_flags, send_ec); + if (friData.lastSendCounter >= friData.monitoringMsg.connectionInfo.receiveMultiplier) { + send_bytes_transferred = encode(step_alg, friData, send_ec); + if (send_ec) return; + socket.send(boost::asio::buffer(friData.sendBuffer, send_bytes_transferred), message_flags, send_ec); } } @@ -454,162 +457,155 @@ class KukaFRIClientDataDriver using KukaUDP::Params; using KukaUDP::defaultParams; - KukaFRIClientDataDriver(boost::asio::io_service &ios, - Params params = defaultParams()) - : params_(params), m_shouldStop(false), isConnectionEstablished_(false), - io_service_(ios) - //,socketP_(std::move(connect(params, io_service_,sender_endpoint_))) ///< - //@todo this breaks the assumption that the object can be constructed without - // hardware issues being a porblem - { - construct(params); - } - - KukaFRIClientDataDriver(Params params = defaultParams()) - : params_(params), m_shouldStop(false), isConnectionEstablished_(false), - optional_internal_io_service_P(new boost::asio::io_service), - io_service_(*optional_internal_io_service_P) - //,socketP_(std::move(connect(params, io_service_,sender_endpoint_))) ///< - //@todo this breaks the assumption that the object can be constructed without - // hardware issues being a porblem - { - construct(params); - } - - /// Call this to initialize the object after the constructor has been called - void construct(Params params = defaultParams()) { - try { + KukaFRIClientDataDriver(boost::asio::io_service &ios, Params params = defaultParams()) + : params_(params), m_shouldStop(false), isConnectionEstablished_(false), + io_service_(ios) + //,socketP_(std::move(connect(params, io_service_,sender_endpoint_))) ///< + //@todo this breaks the assumption that the object can be constructed without + // hardware issues being a porblem + { + construct(params); + } - /////////// - // initialize all of the states - latestStateForUser_ = make_valid_LatestState(); - spareStates_.emplace_back(std::move(make_valid_LatestState())); - spareStates_.emplace_back(std::move(make_valid_LatestState())); + KukaFRIClientDataDriver(Params params = defaultParams()) + : params_(params), m_shouldStop(false), isConnectionEstablished_(false), + optional_internal_io_service_P(new boost::asio::io_service), + io_service_(*optional_internal_io_service_P) + //,socketP_(std::move(connect(params, io_service_,sender_endpoint_))) ///< + //@todo this breaks the assumption that the object can be constructed without + // hardware issues being a porblem + { + construct(params); + } - // start up the driver thread since the io_service_ is internal only - if (std::get(params)) { - driver_threadP_.reset(new std::thread([&] { update(); })); - } + /// Call this to initialize the object after the constructor has been called + void construct(Params params = defaultParams()) { + try { + // initialize all of the states + latestStateForUser_ = make_valid_LatestState(); + spareStates_.emplace_back(std::move(make_valid_LatestState())); + spareStates_.emplace_back(std::move(make_valid_LatestState())); + + // start up the driver thread since the io_service_ is internal only + if (std::get(params)) { + driver_threadP_.reset(new std::thread([&] { update(); })); + } - } catch (boost::exception &e) { - add_details_to_connection_error(e, params); - throw; + } catch (boost::exception &e) { + add_details_to_connection_error(e, params); + throw; + } } - } - /// @brief blocking call to communicate with the robot continuously - /// @pre construct() should be called before run() - void run() { update(); } - - /// @brief Updates the passed friData shared pointer to a pointer to newly - /// updated data, plus any errors that occurred. - /// - /// We recommend you supply a valid shared_ptr to friData, even if all command - /// elements are set to false. - /// The friData pointer you pass in can contain a command to send to the arm. - /// To update with new state and optional input state, you give up lifetime - /// control of the input, - /// and assume liftime control of the output. - /// - /// This function is designed for single threaded use to quickly receive and - /// send "non-blocking" updates to the robot. - /// It is not thread safe cannot be called simultaneously from multiple - /// threads. - /// - /// - /// @note { An error code is set if update_state is called with no new data - /// available. - /// In this special case, all error codes and bytes_transferred are 0, - /// because - /// there was no new data available for the user. - /// } - /// - /// @warning Do not pound this call continuously in a very tight loop, because - /// then the driver won't be able to acquire the lock and send updates to the - /// robot. - /// - /// @param[in,out] friData supply a new command, receive a new update of the - /// robot state. Pointer is null if no new data is available. - /// - /// @pre If friData!=nullptr it is assumed valid for use and this class will - /// take control of the object. - /// - /// @return isError = false if you have new data, true when there is either an - /// error or no new data - bool update_state(std::shared_ptr &step_alg_params, - std::shared_ptr &friData, - boost::system::error_code &receive_ec, - std::size_t &receive_bytes_transferred, - boost::system::error_code &send_ec, - std::size_t &send_bytes_transferred, - grl::TimeEvent& timeEvent) { - - if (exceptionPtr) { - /// @note this exception most likely came from the update() call running - /// the kuka driver - std::rethrow_exception(exceptionPtr); - } + /// @brief blocking call to communicate with the robot continuously + /// @pre construct() should be called before run() + void run() { update(); } + + /// @brief Updates the passed friData shared pointer to a pointer to newly + /// updated data, plus any errors that occurred. + /// + /// We recommend you supply a valid shared_ptr to friData, even if all command + /// elements are set to false. + /// The friData pointer you pass in can contain a command to send to the arm. + /// To update with new state and optional input state, you give up lifetime + /// control of the input, + /// and assume liftime control of the output. + /// + /// This function is designed for single threaded use to quickly receive and + /// send "non-blocking" updates to the robot. + /// It is not thread safe cannot be called simultaneously from multiple + /// threads. + /// + /// + /// @note { An error code is set if update_state is called with no new data + /// available. + /// In this special case, all error codes and bytes_transferred are 0, + /// because + /// there was no new data available for the user. + /// } + /// + /// @warning Do not pound this call continuously in a very tight loop, because + /// then the driver won't be able to acquire the lock and send updates to the + /// robot. + /// + /// @param[in,out] friData supply a new command, receive a new update of the + /// robot state. Pointer is null if no new data is available. + /// + /// @pre If friData!=nullptr it is assumed valid for use and this class will + /// take control of the object. + /// + /// @return isError = false if you have new data, true when there is either an + /// error or no new data + bool update_state(std::shared_ptr &step_alg_params, + std::shared_ptr &friData, + boost::system::error_code &receive_ec, + std::size_t &receive_bytes_transferred, + boost::system::error_code &send_ec, + std::size_t &send_bytes_transferred, + grl::TimeEvent& timeEvent) { + + if (exceptionPtr) { + /// @note this exception most likely came from the update() call running + /// the kuka driver + std::rethrow_exception(exceptionPtr); + } - bool haveNewData = false; + bool haveNewData = false; - if (!isConnectionEstablished_ || - !std::get(latestStateForUser_)) { - // no new data, so immediately return results accordingly - // Constructs a tuple object whose elements are references to the arguments in args std::tie (Types&... args) , in the same order. - // This allows a set of objects to act as a tuple, which is especially useful to unpack tuple objects. - std::tie(step_alg_params, friData, receive_ec, receive_bytes_transferred, send_ec, - send_bytes_transferred, timeEvent) = make_LatestState(step_alg_params,friData); - return !haveNewData; - } + if (!isConnectionEstablished_ || !std::get(latestStateForUser_)) { + // no new data, so immediately return results accordingly + // Constructs a tuple object whose elements are references to the arguments in args std::tie (Types&... args) , in the same order. + // This allows a set of objects to act as a tuple, which is especially useful to unpack tuple objects. + std::tie(step_alg_params, friData, receive_ec, receive_bytes_transferred, send_ec, + send_bytes_transferred, timeEvent) = make_LatestState(step_alg_params,friData); + return !haveNewData; + } - // ensure we have valid data for future updates - // need to copy this over because friData will be set as an output value - // later - // and allocate/initialize data if null - auto validFriDataLatestState = make_valid_LatestState(step_alg_params,friData); + // ensure we have valid data for future updates + // need to copy this over because friData will be set as an output value later + // and allocate/initialize data if null + auto validFriDataLatestState = make_valid_LatestState(step_alg_params,friData); - // get the latest state from the driver thread - { - boost::lock_guard lock(ptrMutex_); + // get the latest state from the driver thread + { + boost::lock_guard lock(ptrMutex_); - // get the update if one is available - // the user has provided new data to send to the device - if (std::get(validFriDataLatestState) - ->commandMsg.has_commandData) { - std::swap(validFriDataLatestState, newCommandForDriver_); - } - // newCommandForDriver_ is finalized + // get the update if one is available + // the user has provided new data to send to the device + if (std::get(validFriDataLatestState)->commandMsg.has_commandData) { + std::swap(validFriDataLatestState, newCommandForDriver_); + } + // newCommandForDriver_ is finalized - if (spareStates_.size() < spareStates_.capacity() && - std::get(validFriDataLatestState)) { - spareStates_.emplace_back(std::move(validFriDataLatestState)); - } + if (spareStates_.size() < spareStates_.capacity() && + std::get(validFriDataLatestState)) { + spareStates_.emplace_back(std::move(validFriDataLatestState)); + } - if (std::get(latestStateForUser_)) { - // return the latest state to the caller - std::tie(step_alg_params,friData, receive_ec, receive_bytes_transferred, send_ec, - send_bytes_transferred, timeEvent) = std::move(latestStateForUser_); - - haveNewData = true; - } else if (std::get( - validFriDataLatestState)) { - // all storage is full, return the spare data to the user - std::tie(step_alg_params, friData, receive_ec, receive_bytes_transferred, send_ec, - send_bytes_transferred, timeEvent) = validFriDataLatestState; - } - } + if (std::get(latestStateForUser_)) { + // return the latest state to the caller + std::tie(step_alg_params,friData, receive_ec, receive_bytes_transferred, send_ec, + send_bytes_transferred, timeEvent) = std::move(latestStateForUser_); + + haveNewData = true; + } else if (std::get( + validFriDataLatestState)) { + // all storage is full, return the spare data to the user + std::tie(step_alg_params, friData, receive_ec, receive_bytes_transferred, send_ec, + send_bytes_transferred, timeEvent) = validFriDataLatestState; + } + } - // let the user know if we aren't in the best possible state - return !haveNewData || receive_bytes_transferred == 0 || receive_ec || - send_ec; - } + // let the user know if we aren't in the best possible state + return !haveNewData || receive_bytes_transferred == 0 || receive_ec || send_ec; + } - void destruct() { - m_shouldStop = true; - if (driver_threadP_) { - driver_threadP_->join(); + void destruct() { + m_shouldStop = true; + if (driver_threadP_) { + driver_threadP_->join(); + } } - } ~KukaFRIClientDataDriver() { destruct(); } @@ -624,145 +620,135 @@ class KukaFRIClientDataDriver /// @todo consider switching to single producer single consumer queue to avoid /// locking overhead, but keep latency in mind /// https://github.com/facebook/folly/blob/master/folly/docs/ProducerConsumerQueue.md - void update() { +void update() { try { - LowLevelStepAlgorithmType step_alg; - /// nextState is the object currently being loaded with data off the - /// network - /// the driver thread should access this exclusively in update() - LatestState nextState = make_valid_LatestState(); - LatestState latestCommandBackup = make_valid_LatestState(); - - boost::asio::ip::udp::endpoint sender_endpoint; - boost::asio::ip::udp::socket socket( - connect(params_, io_service_, sender_endpoint)); - KukaState kukastate; ///< @todo TODO(ahundt) remove this line when new - /// api works completely since old one is deprecated - - ///////////// - // run the primary update loop in a separate thread - while (!m_shouldStop) { - /// @todo maybe there is a more convienient way to set this that is - /// easier for users? perhaps initializeClientDataForiiwa()? - - // nextState and latestCommandBackup should never be null - BOOST_VERIFY(std::get(nextState)); - BOOST_VERIFY(std::get(latestCommandBackup)); - - // set the flag that must always be there - std::get(nextState) - ->expectedMonitorMsgID = KUKA::LBRState::LBRMONITORMESSAGEID; - - auto lowLevelAlgorithmParamP = std::get(nextState); - - // if there is a valid low level algorithm param command set the new goal - if(lowLevelAlgorithmParamP) step_alg.setGoal(*lowLevelAlgorithmParamP); - - // actually talk over the network to receive an update and send out a - // new command - grl::robot::arm::update_state( - socket, step_alg, - *std::get(nextState), - std::get(nextState), - std::get(nextState), - std::get(nextState), - std::get(nextState), - std::get(nextState)); - - /// @todo use atomics to eliminate the global mutex lock for this object - // lock the mutex to communicate with the user thread - // if it cannot lock, simply send the previous message - // again - if (ptrMutex_.try_lock()) { - - ////////////////////////////////////////////// - // right now this is the state of everything: - ////////////////////////////////////////////// - // - // Always Valid: - // - // nextState: valid, contains the latest update - // latestCommandBackup: should always be valid (though hasCommand - // might be false) - // - // Either Valid or Null: - // latestStateForUser_ : null if the user took data out, valid - // otherwise - // newCommandForDriver_: null if there is no new command data from - // the user, vaild otherwise - - // 1) set the outgoing latest state for the user to pick up - // latestStateForUser_ is finalized - std::swap(latestStateForUser_, nextState); - - // 2) get a new incoming command if available and set incoming command - // variable to null - if (std::get(newCommandForDriver_)) { - // 3) back up the new incoming command - // latestCommandBackup is finalized, newCommandForDriver_ needs to - // be cleared out - std::swap(latestCommandBackup, newCommandForDriver_); - - // nextState may be null - if (!std::get(nextState)) { - nextState = std::move(newCommandForDriver_); - } else if (!(spareStates_.size() == spareStates_.capacity())) { - spareStates_.emplace_back(std::move(newCommandForDriver_)); - } else { - std::get(newCommandForDriver_) - .reset(); + LowLevelStepAlgorithmType step_alg; + /// nextState is the object currently being loaded with data off the network + /// the driver thread should access this exclusively in update() + LatestState nextState = make_valid_LatestState(); + LatestState latestCommandBackup = make_valid_LatestState(); + /// Where is the sender_endpoint initialized? It should be bonded with the sender address and port. + boost::asio::ip::udp::endpoint sender_endpoint; + boost::asio::ip::udp::socket socket(connect(params_, io_service_, sender_endpoint)); + KukaState kukastate; ///< @todo TODO(ahundt) remove this line when new + /// api works completely since old one is deprecated + + ///////////// + // run the primary update loop in a separate thread + while (!m_shouldStop) { + /// @todo maybe there is a more convienient way to set this that is + /// easier for users? perhaps initializeClientDataForiiwa()? + + // nextState and latestCommandBackup should never be null + BOOST_VERIFY(std::get(nextState)); + BOOST_VERIFY(std::get(latestCommandBackup)); + + // set the flag that must always be there + std::get(nextState)->expectedMonitorMsgID = KUKA::LBRState::LBRMONITORMESSAGEID; + + auto lowLevelAlgorithmParamP = std::get(nextState); + + // if there is a valid low level algorithm param command set the new goal + if(lowLevelAlgorithmParamP) step_alg.setGoal(*lowLevelAlgorithmParamP); + + // actually talk over the network to receive an update and send out a new command + grl::robot::arm::update_state( + socket, step_alg, + *std::get(nextState), + std::get(nextState), + std::get(nextState), + std::get(nextState), + std::get(nextState), + std::get(nextState)); + + /// @todo use atomics to eliminate the global mutex lock for this object + // lock the mutex to communicate with the user thread + // if it cannot lock, simply send the previous message + // again + // bool try_lock(): Attempt to obtain ownership for the current thread without blocking. + if (ptrMutex_.try_lock()) { + ////////////////////////////////////////////// + // right now this is the state of everything: + ////////////////////////////////////////////// + // + // Always Valid: + // + // nextState: valid, contains the latest update + // latestCommandBackup: should always be valid (though hasCommand + // might be false) + // + // Either Valid or Null: + // latestStateForUser_ : null if the user took data out, valid otherwise + // newCommandForDriver_: null if there is no new command data from the user, vaild otherwise + + // 1) set the outgoing latest state for the user to pick up + // latestStateForUser_ is finalized + std::swap(latestStateForUser_, nextState); + + // 2) get a new incoming command if available and set incoming command + // variable to null + if (std::get(newCommandForDriver_)) { + // 3) back up the new incoming command + // latestCommandBackup is finalized, newCommandForDriver_ needs to be cleared out + std::swap(latestCommandBackup, newCommandForDriver_); + + // nextState may be null + if (!std::get(nextState)) { + nextState = std::move(newCommandForDriver_); + } else if (!(spareStates_.size() == spareStates_.capacity())) { + spareStates_.emplace_back(std::move(newCommandForDriver_)); + } else { + std::get(newCommandForDriver_).reset(); + } + } + + // finalized: latestStateForUser_, latestCommandBackup, + // newCommandForDriver_ is definitely null + // issues to be resolved: + // nextState: may be null right now, and it should be valid + // newCommandForDriver_: needs to be cleared with 100% certainty + BOOST_VERIFY(spareStates_.size() > 0); + + + if (!std::get(nextState) && + spareStates_.size()) { + // move the last element out and shorten the vector + nextState = std::move(*(spareStates_.end() - 1)); + spareStates_.pop_back(); + } + + BOOST_VERIFY(std::get(nextState)); + + KUKA::FRI::ClientData &nextClientData = + *std::get(nextState); + KUKA::FRI::ClientData &latestClientData = + *std::get(latestStateForUser_); + + // copy essential data from latestStateForUser_ to nextState + nextClientData.lastState = latestClientData.lastState; + nextClientData.sequenceCounter = latestClientData.sequenceCounter; + nextClientData.lastSendCounter = latestClientData.lastSendCounter; + nextClientData.expectedMonitorMsgID = latestClientData.expectedMonitorMsgID; + + // copy command from latestCommandBackup to nextState aka + // nextClientData + KUKA::FRI::ClientData &latestCommandBackupClientData = + *std::get(latestCommandBackup); + set(nextClientData.commandMsg, + latestCommandBackupClientData.commandMsg); + + // if there are no error codes and we have received data, + // then we can consider the connection established! + /// @todo perhaps data should always send too? + if (!std::get(nextState) && + !std::get(nextState) && + std::get(nextState)) { + isConnectionEstablished_ = true; + } + ptrMutex_.unlock(); } - } - - // finalized: latestStateForUser_, latestCommandBackup, - // newCommandForDriver_ is definitely null - // issues to be resolved: - // nextState: may be null right now, and it should be valid - // newCommandForDriver_: needs to be cleared with 100% certainty - BOOST_VERIFY(spareStates_.size() > 0); - - - if (!std::get(nextState) && - spareStates_.size()) { - // move the last element out and shorten the vector - nextState = std::move(*(spareStates_.end() - 1)); - spareStates_.pop_back(); - } - - BOOST_VERIFY(std::get(nextState)); - - KUKA::FRI::ClientData &nextClientData = - *std::get(nextState); - KUKA::FRI::ClientData &latestClientData = - *std::get(latestStateForUser_); - - // copy essential data from latestStateForUser_ to nextState - nextClientData.lastState = latestClientData.lastState; - nextClientData.sequenceCounter = latestClientData.sequenceCounter; - nextClientData.lastSendCounter = latestClientData.lastSendCounter; - nextClientData.expectedMonitorMsgID = latestClientData.expectedMonitorMsgID; - - // copy command from latestCommandBackup to nextState aka - // nextClientData - KUKA::FRI::ClientData &latestCommandBackupClientData = - *std::get(latestCommandBackup); - set(nextClientData.commandMsg, - latestCommandBackupClientData.commandMsg); - - // if there are no error codes and we have received data, - // then we can consider the connection established! - /// @todo perhaps data should always send too? - if (!std::get(nextState) && - !std::get(nextState) && - std::get(nextState)) { - isConnectionEstablished_ = true; - } - - ptrMutex_.unlock(); } - } - } catch (...) { // transport the exception to the main thread in a safe manner exceptionPtr = std::current_exception(); @@ -773,101 +759,102 @@ class KukaFRIClientDataDriver isConnectionEstablished_ = false; } - enum LatestStateIndex { - latest_low_level_algorithm_params, - latest_receive_monitor_state, - latest_receive_ec, - latest_receive_bytes_transferred, - latest_send_ec, - latest_send_bytes_transferred, - latest_time_event_data - }; - - /// this is the object that stores all data for the latest device state - /// including the KUKA defined ClientData object, and a grl defined TimeEvent - /// which stores the time data needed for synchronization. - typedef std::tuple, - std::shared_ptr, - boost::system::error_code, std::size_t, - boost::system::error_code, std::size_t, - grl::TimeEvent> LatestState; - - /// Creates a default LatestState Object - static LatestState - make_LatestState(std::shared_ptr lowLevelAlgorithmParams, - std::shared_ptr &clientData) { - return std::make_tuple(lowLevelAlgorithmParams, - clientData, - boost::system::error_code(), std::size_t(), - boost::system::error_code(), std::size_t(), - grl::TimeEvent()); - } - - /// creates a shared_ptr to KUKA::FRI::ClientData with all command message - /// status explicitly set to false - /// @post std::shared_ptr will be non-null - static std::shared_ptr make_shared_valid_ClientData( - std::shared_ptr &friData) { - if (friData.get() == nullptr) { - friData = std::make_shared(KUKA::LBRState::NUM_DOF); - // there is no commandMessage data on a new object - friData->resetCommandMessage(); + enum LatestStateIndex { + latest_low_level_algorithm_params, + latest_receive_monitor_state, + latest_receive_ec, + latest_receive_bytes_transferred, + latest_send_ec, + latest_send_bytes_transferred, + latest_time_event_data + }; + + /// this is the object that stores all data for the latest device state + /// including the KUKA defined ClientData object, and a grl defined TimeEvent + /// which stores the time data needed for synchronization. + typedef std::tuple, + std::shared_ptr, + boost::system::error_code, std::size_t, + boost::system::error_code, std::size_t, + grl::TimeEvent> LatestState; + + /// Creates a default LatestState Object + /// if there is no new data, then create an empty state. + static LatestState + make_LatestState(std::shared_ptr lowLevelAlgorithmParams, + std::shared_ptr &clientData) { + return std::make_tuple(lowLevelAlgorithmParams, + clientData, + boost::system::error_code(), std::size_t(), + boost::system::error_code(), std::size_t(), + grl::TimeEvent()); } - return friData; - } + /// creates a shared_ptr to KUKA::FRI::ClientData with all command message + /// status explicitly set to false + /// @post std::shared_ptr will be non-null + static std::shared_ptr make_shared_valid_ClientData( + std::shared_ptr &friData) { + if (friData.get() == nullptr) { + friData = std::make_shared(KUKA::LBRState::NUM_DOF); + // there is no commandMessage data on a new object + friData->resetCommandMessage(); + } - static std::shared_ptr make_shared_valid_ClientData() { - std::shared_ptr friData; - return make_shared_valid_ClientData(friData); - } + return friData; + } - /// Initialize valid shared ptr to LatestState object with a valid allocated - /// friData. Note that lowLevelAlgorithmParams will remain null! -static LatestState make_valid_LatestState( - std::shared_ptr lowLevelAlgorithmParams, - std::shared_ptr &friData) { - if (!friData) { - friData = make_shared_valid_ClientData(); - } + static std::shared_ptr make_shared_valid_ClientData() { + std::shared_ptr friData; + return make_shared_valid_ClientData(friData); + } - return make_LatestState(lowLevelAlgorithmParams,friData); - } + /// Initialize valid shared ptr to LatestState object with a valid allocated + /// friData. Note that lowLevelAlgorithmParams will remain null! + static LatestState make_valid_LatestState( + std::shared_ptr lowLevelAlgorithmParams, + std::shared_ptr &friData) { + if (!friData) { + friData = make_shared_valid_ClientData(); + } - static LatestState make_valid_LatestState() { - std::shared_ptr emptyLowLevelAlgParams; - std::shared_ptr friData; - return make_valid_LatestState(emptyLowLevelAlgParams,friData); - } + return make_LatestState(lowLevelAlgorithmParams,friData); + } + + static LatestState make_valid_LatestState() { + std::shared_ptr emptyLowLevelAlgParams; + std::shared_ptr friData; + return make_valid_LatestState(emptyLowLevelAlgParams,friData); + } - Params params_; + Params params_; - /// @todo replace with unique_ptr - /// the latest state we have available to give to the user - LatestState latestStateForUser_; - LatestState newCommandForDriver_; + /// @todo replace with unique_ptr + /// the latest state we have available to give to the user + LatestState latestStateForUser_; + LatestState newCommandForDriver_; - /// should always be valid, never null - boost::container::static_vector spareStates_; + /// should always be valid, never null + boost::container::static_vector spareStates_; - std::atomic m_shouldStop; - std::exception_ptr exceptionPtr; - std::atomic isConnectionEstablished_; + std::atomic m_shouldStop; + std::exception_ptr exceptionPtr; + std::atomic isConnectionEstablished_; - /// may be null, allows the user to choose if they want to provide an - /// io_service - std::unique_ptr optional_internal_io_service_P; + /// may be null, allows the user to choose if they want to provide an + /// io_service + std::unique_ptr optional_internal_io_service_P; - // other things to do somewhere: - // - get list of control points - // - get the control point in the arm base coordinate system - // - load up a configuration file with ip address to send to, etc. - boost::asio::io_service &io_service_; - std::unique_ptr driver_threadP_; - boost::mutex ptrMutex_; + // other things to do somewhere: + // - get list of control points + // - get the control point in the arm base coordinate system + // - load up a configuration file with ip address to send to, etc. + boost::asio::io_service &io_service_; + std::unique_ptr driver_threadP_; + boost::mutex ptrMutex_; - typename LowLevelStepAlgorithmType::Params step_alg_params_; + typename LowLevelStepAlgorithmType::Params step_alg_params_; }; /// End of KukaFRIClientDataDriver } diff --git a/include/grl/kuka/KukaFRIdriver.hpp b/include/grl/kuka/KukaFRIdriver.hpp index c449142..84ba3c8 100644 --- a/include/grl/kuka/KukaFRIdriver.hpp +++ b/include/grl/kuka/KukaFRIdriver.hpp @@ -149,8 +149,7 @@ class KukaFRIdriver : public std::enable_shared_from_this(system_clock::now().time_since_epoch())); + return time_point(duration_cast(system_clock::now().time_since_epoch())); } }; @@ -170,7 +169,7 @@ class KukaFRIdriver : public std::enable_shared_from_thisinfo("Save Recording as: ", filename); + loggerP->info("Save Recording as: {}", filename); #else // HAVE_spdlog std::cout << "Save Recording as: " << filename << std::endl; #endif // HAVE_spdlog @@ -665,14 +664,14 @@ class KukaFRIdriver : public std::enable_shared_from_this(save_fbbP->GetBufferPointer()), save_fbbP->GetSize(), write_binary_stream); // assert(success); /// TODO(ahFusionTrackLogAndTrackundt) replace cout with proper spdlog and vrep banner notification #ifdef HAVE_spdlog - lambdaLoggerP->info("filename: ", filename, " verifier success: ", success); + lambdaLoggerP->info("filename: {}, verifier success: {}", filename, success); #else // HAVE_spdlog std::cout << "filename: " << filename << " verifier success: " << success << std::endl; #endif // HAVE_spdlog @@ -684,7 +683,6 @@ class KukaFRIdriver : public std::enable_shared_from_this(); m_KUKAiiwaStateBufferP = std::make_shared>>(); - std::cout << "End of the program" << std::endl; return true; } @@ -726,7 +724,8 @@ void saveToDisk() { const std::size_t MegaByte = 1024*1024; // If we write too large a flatbuffer - const std::size_t single_buffer_limit_bytes = 1*MegaByte; + const std::size_t single_buffer_limit_bytes = 1024*MegaByte; + const std::size_t single_buffer_limit_states = 1000; // run the primary update loop in a separate thread bool saveFileNow = false; @@ -736,7 +735,8 @@ void saveToDisk() { // There is a flatbuffers file size limit of 2GB, but we use a conservative 512MB int buffsize = m_logFileBufferBuilderP->GetSize(); - if( buffsize > single_buffer_limit_bytes) + int statessize = m_KUKAiiwaStateBufferP->size(); + if( buffsize > single_buffer_limit_bytes || statessize > single_buffer_limit_states) { // save the file if we are over the limit saveFileNow = true; diff --git a/include/grl/kuka/KukaJAVAdriver.hpp b/include/grl/kuka/KukaJAVAdriver.hpp index 128f2e8..e107dba 100644 --- a/include/grl/kuka/KukaJAVAdriver.hpp +++ b/include/grl/kuka/KukaJAVAdriver.hpp @@ -185,7 +185,6 @@ class KukaJAVAdriver : public std::enable_shared_from_this { /// htons: host to network short local_sockaddr.sin_port = htons(port); /// local_sockaddr.sin_addr.s_addr = INADDR_ANY; - /// @todo TODO(ahundt) Consider switching to boost::asio synchronous calls (async has high latency)! /// @todo TODO(ahundt) Need to switch back to an appropriate exception rather than exiting so VREP isn't taken down. /// @todo TODO(ahundt) switch from linux socket to boost::asio::ip::udp::socket, see Kuka.hpp and KukaFRIdriver.hpp for examples, and make use of KukaUDP class. @@ -199,8 +198,8 @@ class KukaJAVAdriver : public std::enable_shared_from_this { FD_ZERO(&dummy_mask); /// if valid socket descriptor then add to socket set FD_SET(socket_local, &mask); - // set arm to StartArm mode on initalization - //set(grl::flatbuffer::ArmState::StartArm); + /// set arm to StartArm mode on initalization + /// set(grl::flatbuffer::ArmState::StartArm); set(grl::flatbuffer::ArmState::MoveArmJointServo); std::cout<< "End KukaJAVAdriver->construct()..." << std::endl; } catch( boost::exception &e) { @@ -258,16 +257,15 @@ class KukaJAVAdriver : public std::enable_shared_from_this { } case flatbuffer::ArmState::MoveArmJointServo: { - /// @todo when new - /// armState_ isn't assigned with any values correctly. - - auto armPositionBuffer = fbbP->CreateVector(armState_.commandedPosition_goal.data(),armState_.commandedPosition_goal.size()); - auto commandedTorque = fbbP->CreateVector(armState_.commandedTorque.data(),armState_.commandedTorque.size()); - auto goalJointState = grl::flatbuffer::CreateJointState(*fbbP,armPositionBuffer,0/*no velocity*/,0/*no acceleration*/,commandedTorque); - auto moveArmJointServo = grl::flatbuffer::CreateMoveArmJointServo(*fbbP,goalJointState); - controlState = flatbuffer::CreateArmControlState(*fbbP,bns,sequenceNumber++,duration,armControlMode_,moveArmJointServo.Union()); - logger_->info("C++ KukaJAVAdriver: sending armposition command: {}{}", armState_.commandedPosition_goal); - break; + /// @todo when new + /// armState_ isn't assigned with any values correctly. + auto armPositionBuffer = fbbP->CreateVector(armState_.commandedPosition_goal.data(),armState_.commandedPosition_goal.size()); + auto commandedTorque = fbbP->CreateVector(armState_.commandedTorque.data(),armState_.commandedTorque.size()); + auto goalJointState = grl::flatbuffer::CreateJointState(*fbbP,armPositionBuffer,0/*no velocity*/,0/*no acceleration*/,commandedTorque); + auto moveArmJointServo = grl::flatbuffer::CreateMoveArmJointServo(*fbbP,goalJointState); + controlState = flatbuffer::CreateArmControlState(*fbbP,bns,sequenceNumber++,duration,armControlMode_,moveArmJointServo.Union()); + logger_->info("C++ KukaJAVAdriver: sending armposition command: {}{}", armState_.commandedPosition_goal); + break; } case flatbuffer::ArmState::TeachArm: { controlState = flatbuffer::CreateArmControlState(*fbbP,bns,sequenceNumber++,duration,armControlMode_,flatbuffer::CreateTeachArm(*fbbP).Union()); @@ -336,7 +334,7 @@ class KukaJAVAdriver : public std::enable_shared_from_this { { angles.push_back(movearm->goal()->position()->Get(i)); } - logger_->info("re-extracted {}{}{}", movearm->goal()->position()->size(), " joint angles: ",angles); + // logger_->info("re-extracted {}{}{}", movearm->goal()->position()->size(), " joint angles: ",angles); } if(debug_) logger_->info("sending packet to KUKA iiwa: len = {}", fbbP->GetSize()); @@ -369,7 +367,7 @@ class KukaJAVAdriver : public std::enable_shared_from_this { { // allocate the buffer, should only happen once if(!java_interface_received_statesP_) { - java_interface_received_statesP_ = std::make_shared>(fbs_tk::Buffer(udp_size_)); + java_interface_received_statesP_ = std::make_shared>(fbs_tk::Buffer(udp_size_)); } if(!java_interface_next_statesP_) { java_interface_next_statesP_ = std::make_shared>(fbs_tk::Buffer(udp_size_)); @@ -394,15 +392,11 @@ class KukaJAVAdriver : public std::enable_shared_from_this { } else { // TODO(ahundt) consider specific error codes for verifier failure vs udp receive failure bool java_state_received_successfully = false; - logger_->error("C++ KukaJAVAdriver Error: flatbuff failed verification. bufOk: {}", java_state_received_successfully); + // logger_->error("C++ KukaJAVAdriver Error: flatbuff failed verification. bufOk: {}", java_state_received_successfully); } } - - } } - - return haveNewData; } @@ -412,17 +406,17 @@ class KukaJAVAdriver : public std::enable_shared_from_this { volatile std::size_t m_attemptedCommunicationConsecutiveSuccessCount = 0; void setPositionControlMode() { - boost::lock_guard lock(jt_mutex); - controlMode_ = grl::flatbuffer::EControlMode::POSITION_CONTROL_MODE; - setArmConfiguration_ = true; + boost::lock_guard lock(jt_mutex); + controlMode_ = grl::flatbuffer::EControlMode::POSITION_CONTROL_MODE; + setArmConfiguration_ = true; } bool setJointImpedanceMode(std::vector joint_stiffnes, std::vectorjoint_damping) { - boost::lock_guard lock(jt_mutex); - //TODO use tags - joint_stiffness_ = joint_stiffnes; - joint_damping_ = joint_damping; - controlMode_ = grl::flatbuffer::EControlMode::JOINT_IMP_CONTROL_MODE; - setArmConfiguration_ = true; + boost::lock_guard lock(jt_mutex); + //TODO use tags + joint_stiffness_ = joint_stiffnes; + joint_damping_ = joint_damping; + controlMode_ = grl::flatbuffer::EControlMode::JOINT_IMP_CONTROL_MODE; + setArmConfiguration_ = true; } // TODO: define custom flatbuffer for Cartesion Quantities void setCartesianImpedanceMode( @@ -474,10 +468,10 @@ class KukaJAVAdriver : public std::enable_shared_from_this { */ template void set(Range&& range, grl::revolute_joint_angle_open_chain_command_tag) { - boost::lock_guard lock(jt_mutex); - armState_.clearCommands(); - boost::copy(range, std::back_inserter(armState_.commandedPosition)); - boost::copy(range, std::back_inserter(armState_.commandedPosition_goal)); + boost::lock_guard lock(jt_mutex); + armState_.clearCommands(); + boost::copy(range, std::back_inserter(armState_.commandedPosition)); + boost::copy(range, std::back_inserter(armState_.commandedPosition_goal)); } /** @@ -492,8 +486,8 @@ class KukaJAVAdriver : public std::enable_shared_from_this { * @brief set the interface over which state is monitored (FRI interface, alternately SmartServo/DirectServo == JAVA interface, ) */ void set(flatbuffer::KUKAiiwaInterface mif, state_tag) { - boost::lock_guard lock(jt_mutex); - commandInterface_ = mif; + boost::lock_guard lock(jt_mutex); + commandInterface_ = mif; } /** @@ -512,8 +506,8 @@ class KukaJAVAdriver : public std::enable_shared_from_this { */ template void set(TimeDuration && duration_to_goal_command, time_duration_command_tag) { - boost::lock_guard lock(jt_mutex); - armState_.goal_position_command_time_duration = duration_to_goal_command; + boost::lock_guard lock(jt_mutex); + armState_.goal_position_command_time_duration = duration_to_goal_command; } @@ -549,40 +543,40 @@ class KukaJAVAdriver : public std::enable_shared_from_this { * @param torques Array with the applied torque values (in Nm) * @param tag identifier object indicating that the torqe value command should be modified */ - template - void set(Range&& range, grl::revolute_joint_torque_open_chain_command_tag) { - boost::lock_guard lock(jt_mutex); - armState_.clearCommands(); - boost::copy(range, armState_.commandedTorque); + template + void set(Range&& range, grl::revolute_joint_torque_open_chain_command_tag) { + boost::lock_guard lock(jt_mutex); + armState_.clearCommands(); + boost::copy(range, armState_.commandedTorque); } - /** - * \brief Set the applied wrench vector of the current interpolation step. - * - * The wrench vector consists of: - * [F_x, F_y, F_z, tau_A, tau_B, tau_C] - * - * F ... forces (in N) applied along the Cartesian axes of the - * currently used motion center. - * tau ... torques (in Nm) applied along the orientation angles - * (Euler angles A, B, C) of the currently used motion center. - * - * This method is only effective when the client is in a commanding state. - * The ControlMode of the robot has to be Cartesian impedance control mode. The - * Client Command Mode has to be wrench. - * - * @param state object storing the command data that will be sent to the physical device - * @param range wrench Applied Cartesian wrench vector, in x, y, z, roll, pitch, yaw force measurments. - * @param tag identifier object indicating that the wrench value command should be modified - * - * @todo perhaps support some specific more useful data layouts - */ + /** + * \brief Set the applied wrench vector of the current interpolation step. + * + * The wrench vector consists of: + * [F_x, F_y, F_z, tau_A, tau_B, tau_C] + * + * F ... forces (in N) applied along the Cartesian axes of the + * currently used motion center. + * tau ... torques (in Nm) applied along the orientation angles + * (Euler angles A, B, C) of the currently used motion center. + * + * This method is only effective when the client is in a commanding state. + * The ControlMode of the robot has to be Cartesian impedance control mode. The + * Client Command Mode has to be wrench. + * + * @param state object storing the command data that will be sent to the physical device + * @param range wrench Applied Cartesian wrench vector, in x, y, z, roll, pitch, yaw force measurments. + * @param tag identifier object indicating that the wrench value command should be modified + * + * @todo perhaps support some specific more useful data layouts + */ template void set(Range&& range, grl::cartesian_wrench_command_tag) { - boost::lock_guard lock(jt_mutex); - armState_.clearCommands(); - std::copy(range,armState_.commandedCartesianWrenchFeedForward); + boost::lock_guard lock(jt_mutex); + armState_.clearCommands(); + std::copy(range,armState_.commandedCartesianWrenchFeedForward); } /// @todo should this exist? is it written correctly? diff --git a/include/grl/kuka/readKukaiiwaFromFlatbuffer.hpp b/include/grl/kuka/readKukaiiwaFromFlatbuffer.hpp index 28b643a..47edb9f 100644 --- a/include/grl/kuka/readKukaiiwaFromFlatbuffer.hpp +++ b/include/grl/kuka/readKukaiiwaFromFlatbuffer.hpp @@ -10,40 +10,186 @@ #include "grl/flatbuffer/LinkObject_generated.h" #include "grl/flatbuffer/Time_generated.h" #include "grl/flatbuffer/flatbuffer.hpp" +#include #include // For printing and file access. #include +#include + +#include +#include +#include +#include +#include +#include namespace grl { -void getDeviceTime(){ - std::string curWorkingDir = grl::GetCurrentWorkingDir(); - std::cout << "CurrentWorkingDir: " << curWorkingDir << std::endl; - FILE* file = fopen("2018_01_22_19_00_29_Kukaiiwa.iiwa", "rb"); - fseek(file, 0L, SEEK_END); - int length = ftell(file); - fseek(file, 0L, SEEK_SET); - char *data = new char[length]; - fread(data, sizeof(char), length, file); - fclose(file); - auto kukaStates = grl::flatbuffer::GetKUKAiiwaStates(data); - auto states = kukaStates->states(); - auto KUKAiiwaState = states->Get(0); - auto FRIMessage = KUKAiiwaState->FRIMessage(); - auto timeEvent = FRIMessage->timeStamp(); - auto device_time = timeEvent->device_time(); - std::cout<<"Device Time: "<< device_time << std::endl; - - std::size_t state_size = states->size(); - for(int i = 1; iGet(i); - FRIMessage = KUKAiiwaState->FRIMessage(); - timeEvent = FRIMessage->timeStamp(); - auto nextdevice_time = timeEvent->device_time(); - auto diff = nextdevice_time - device_time; - device_time = nextdevice_time; - std::cout<<"Time Diff: "<< diff < buffer(length); + std::fread(buffer.data(), sizeof(uint8_t), buffer.size(), file); + std::fclose(file); + return grl::flatbuffer::GetKUKAiiwaStates(&buffer[0]); + } + + std::vector getTimeStamp_Kuka(const fbs_tk::Root &kukaStatesP, std::string time_type){ + + auto states = kukaStatesP->states(); + std::vector timeStamp; + std::size_t state_size = states->size(); + for(int i = 1; iGet(i); + auto FRIMessage = KUKAiiwaState->FRIMessage(); + auto timeEvent = FRIMessage->timeStamp(); + + if(time_type =="device_time"){ + timeStamp.push_back(timeEvent->device_time()); + } else if (time_type == "local_request_time") { + timeStamp.push_back(timeEvent->local_request_time()); + } else if (time_type == "local_receive_time"){ + timeStamp.push_back(timeEvent->local_receive_time()); + } + } + return timeStamp; } + + + /// Return the joint_index th joint angle + std::vector getJointAngles(const grl::flatbuffer::KUKAiiwaStates* kukaStatesP, int joint_index){ + if(kukaStatesP == nullptr) + return std::vector(); + auto states = kukaStatesP->states(); + std::vector jointPosition; + std::size_t state_size = states->size(); + for(int i = 1; iGet(i); + auto FRIMessage = KUKAiiwaState->FRIMessage(); + auto joints_Position = FRIMessage->measuredJointPosition(); // flatbuffers::Vector * + jointPosition.push_back(joints_Position->Get(joint_index)); + } + return jointPosition; + } + /// Return the joint_index th joint angle + std::vector> getAllJointAngles(const grl::flatbuffer::KUKAiiwaStates* kukaStatesP){ + if(kukaStatesP == nullptr) + return std::vector>(); + auto states = kukaStatesP->states(); + + std::vector> allJointPosition; + std::size_t state_size = states->size(); + for(int i = 1; iGet(i); + auto FRIMessage = KUKAiiwaState->FRIMessage(); + auto joints_Position = FRIMessage->measuredJointPosition(); // flatbuffers::Vector * + std::vector oneJointPosition; + for(int joint_index=0; joint_index<7; joint_index++){ + oneJointPosition.push_back(joints_Position->Get(joint_index)); + } + allJointPosition.push_back(oneJointPosition); + } + return allJointPosition; + } + + // void writetoCSVforKuka(std::string binaryfilename, std::string csvfilename) { + // FILE* file = std::fopen(binaryfilename.c_str(), "rb"); + // std::fseek(file, 0L, SEEK_END); // seek to end + // std::size_t length = std::ftell(file); + // std::fseek(file, 0L, SEEK_SET); // seek to start + // std::vector buffer(length); + // std::fread(buffer.data(), sizeof(uint8_t), buffer.size(), file); + // std::fclose(file); + // auto kukaiiwaStates = grl::flatbuffer::GetKUKAiiwaStates(&buffer[0]); + // auto states = kukaiiwaStates->states(); + // std::size_t state_size = states->size(); + // std::cout<< "------Kuka_state_size: "< deviceTime = grl:: getTimeStamp_Kuka(kukaiiwaStates, "device_time"); + // std::vector local_request_time = grl:: getTimeStamp_Kuka(kukaiiwaStates, "local_request_time"); + // std::vector local_receive_time = grl:: getTimeStamp_Kuka(kukaiiwaStates, "local_receive_time"); + + // std::vector jointAngles_0 = grl::getJointAngles(kukaiiwaStates, 0); + // std::vector jointAngles_1 = grl::getJointAngles(kukaiiwaStates, 1); + // std::vector jointAngles_2 = grl::getJointAngles(kukaiiwaStates, 2); + // std::vector jointAngles_3 = grl::getJointAngles(kukaiiwaStates, 3); + // std::vector jointAngles_4 = grl::getJointAngles(kukaiiwaStates, 4); + // std::vector jointAngles_5 = grl::getJointAngles(kukaiiwaStates, 5); + // std::vector jointAngles_6 = grl::getJointAngles(kukaiiwaStates, 6); + + // std::vector> allJointAngles = grl::getAllJointAngles(kukaiiwaStates); + // // if(allJointAngles.size() > 0){ + // // std::cout<< "------States size: "< + void addDatainRow(T first, T last); +}; + +/* + * This Function accepts a range and appends all the elements in the range + * to the last row, seperated by delimeter (Default is comma) + */ +template +void CSVWriter::addDatainRow(T first, T last) +{ + std::fstream file; + // Open the file in truncate mode if first line else in Append Mode + file.open(fileName, std::ios::out | (linesCount ? std::ios::app : std::ios::trunc)); + + // Iterate over the range and add each lement to file seperated by delimeter. + for (; first != last; ) + { + file << *first; + if (++first != last) + file << delimeter; + } + file << "\n"; + linesCount++; + + // Close the file + file.close(); } + } #endif \ No newline at end of file diff --git a/include/grl/sensor/FusionTrack.hpp b/include/grl/sensor/FusionTrack.hpp index 44755f3..c875434 100644 --- a/include/grl/sensor/FusionTrack.hpp +++ b/include/grl/sensor/FusionTrack.hpp @@ -208,8 +208,7 @@ class FusionTrack cartographer::common::Time FusionTrackTimeToCommonTime(typename MicrosecondClock::time_point FTtime) { return cartographer::common::Time( - std::chrono::duration_cast(FTtime.time_since_epoch()) + - std::chrono::seconds(cartographer::common::kUtsEpochOffsetFromUnixEpochInSeconds)); + std::chrono::duration_cast(FTtime.time_since_epoch())); } cartographer::common::Time ImageHeaderToCommonTime(const ::ftkImageHeader &tq) diff --git a/include/grl/sensor/FusionTrackLogAndTrack.hpp b/include/grl/sensor/FusionTrackLogAndTrack.hpp index e4fba59..bd2368d 100644 --- a/include/grl/sensor/FusionTrackLogAndTrack.hpp +++ b/include/grl/sensor/FusionTrackLogAndTrack.hpp @@ -362,11 +362,11 @@ class FusionTrackLogAndTrack : public std::enable_shared_from_thisinfo("Save Recording as: ", filename); -#else // HAVE_spdlog - std::cout << "Save Recording as: " << filename << std::endl; -#endif // HAVE_spdlog + #ifdef HAVE_spdlog + loggerP->info("Save Recording as: {}", filename); + #else // HAVE_spdlog + std::cout << "Save Recording as: {}" << filename << std::endl; + #endif // HAVE_spdlog /// lock mutex before accessing file std::lock_guard lock(m_frameAccess); @@ -393,7 +393,7 @@ class FusionTrackLogAndTrack : public std::enable_shared_from_this(save_fbbP->GetBufferPointer()), save_fbbP->GetSize(), write_binary_stream); /// TODO(ahFusionTrackLogAndTrackundt) replace cout with proper spdlog and vrep banner notification #ifdef HAVE_spdlog - lambdaLoggerP->info("filename: ", filename, " verifier success: ", success); + lambdaLoggerP->info("filename: {}", filename, " verifier success: {}", success); #else // HAVE_spdlog std::cout << "filename: " << filename << " verifier success: " << success << std::endl; #endif // HAVE_spdlog @@ -439,7 +439,8 @@ class FusionTrackLogAndTrack : public std::enable_shared_from_this deviceSerialNumbers; try { @@ -475,33 +476,34 @@ class FusionTrackLogAndTrack : public std::enable_shared_from_this lock(m_frameAccess); if (m_isRecording) { - // convert the buffer into a flatbuffer for recording and add it to the in memory buffer - // @todo TODO(ahundt) if there haven't been problems, delete this todo, but if recording in the driver thread is time consuming move the code to another thread - if (!m_logFileBufferBuilderP) - { - // flatbuffersbuilder does not yet exist - m_logFileBufferBuilderP = std::make_shared(); - m_KUKAiiwaFusionTrackMessageBufferP = - std::make_shared>>(); - } - BOOST_VERIFY(m_logFileBufferBuilderP != nullptr); - BOOST_VERIFY(opticalTrackerP != nullptr); - BOOST_VERIFY(m_nextState != nullptr); - - if(m_nextState->Error != FTK_WAR_NO_FRAME) - { - // only collect frames actually containing new data. - flatbuffers::Offset oneKUKAiiwaFusionTrackMessage = - grl::toFlatBuffer(*m_logFileBufferBuilderP, *opticalTrackerP, *m_nextState); - m_KUKAiiwaFusionTrackMessageBufferP->push_back(oneKUKAiiwaFusionTrackMessage); - } - - // There is a flatbuffers file size limit of 2GB, but we use a conservative 512MB - if(m_logFileBufferBuilderP->GetSize() > single_buffer_limit_bytes) - { - // save the file if we are over the limit - saveFileNow = true; - } + // convert the buffer into a flatbuffer for recording and add it to the in memory buffer + // @todo TODO(ahundt) if there haven't been problems, delete this todo, but if recording in the driver thread is time consuming move the code to another thread + if (!m_logFileBufferBuilderP) + { + // flatbuffersbuilder does not yet exist + m_logFileBufferBuilderP = std::make_shared(); + m_KUKAiiwaFusionTrackMessageBufferP = + std::make_shared>>(); + } + BOOST_VERIFY(m_logFileBufferBuilderP != nullptr); + BOOST_VERIFY(opticalTrackerP != nullptr); + BOOST_VERIFY(m_nextState != nullptr); + + if(m_nextState->Error != FTK_WAR_NO_FRAME) + { + // only collect frames actually containing new data. + flatbuffers::Offset oneKUKAiiwaFusionTrackMessage = + grl::toFlatBuffer(*m_logFileBufferBuilderP, *opticalTrackerP, *m_nextState); + m_KUKAiiwaFusionTrackMessageBufferP->push_back(oneKUKAiiwaFusionTrackMessage); + } + + // There is a flatbuffers file size limit of 2GB, but we use a conservative 512MB + if(m_logFileBufferBuilderP->GetSize() > single_buffer_limit_bytes || + m_KUKAiiwaFusionTrackMessageBufferP->size() > single_buffer_limit_messages) + { + // save the file if we are over the limit + saveFileNow = true; + } } // end recording steps if(m_nextState->Error == FTK_OK) @@ -515,8 +517,8 @@ class FusionTrackLogAndTrack : public std::enable_shared_from_this +#include + +namespace grl { + + const grl::flatbuffer::LogKUKAiiwaFusionTrack* getLogKUKAiiwaFusionTrackFromFlatbuffer(const std::string filename){ + + //std::cout << "CurrentWorkingDir: " << curWorkingDir << std::endl; + if(filename.empty()) return nullptr; + FILE* file = std::fopen(filename.c_str(), "rb"); + if(!file) { + std::perror("File opening failed"); + return nullptr; + } + std::fseek(file, 0L, SEEK_END); // seek to end + std::size_t length = std::ftell(file); + std::fseek(file, 0L, SEEK_SET); // seek to start + std::vector buffer(length); + std::fread(buffer.data(), sizeof(uint8_t), buffer.size(), file); + std::fclose(file); + return grl::flatbuffer::GetLogKUKAiiwaFusionTrack(&buffer[0]); + } + + /// Return the joint_index th joint angle + // std::vector getGeoPositions(const grl::flatbuffer::LogKUKAiiwaFusionTrack* logKUKAiiwaFusionTrackP, int joint_index){ + // if(kukaStatesP == nullptr) + // return std::vector(); + // auto states = kukaStatesP->states(); + // std::vector jointPosition; + // std::size_t state_size = states->size(); + // for(int i = 1; iGet(i); + // auto FRIMessage = KUKAiiwaState->FRIMessage(); + // auto joints_Position = FRIMessage->measuredJointPosition(); // flatbuffers::Vector * + // jointPosition.push_back(joints_Position->Get(joint_index)); + // } + // return jointPosition; + // } + + std::vector getTimeStamp_FusionTrack(const fbs_tk::Root &logKUKAiiwaFusionTrackP, std::string time_type){ + + auto states = logKUKAiiwaFusionTrackP->states(); + std::vector timeStamp; + + std::size_t state_size = states->size(); + //Eigen::VectoXd time_stamp(state_size); + for(int i = 0; iGet(i); + auto timeEvent = kukaiiwaFusionTrackMessage->timeEvent(); + + if(time_type =="device_time"){ + timeStamp.push_back(timeEvent->device_time()); + //time_stamp<device_time(); + } else if (time_type == "local_request_time") { + timeStamp.push_back(timeEvent->local_request_time()); + } else if (time_type == "local_receive_time"){ + timeStamp.push_back(timeEvent->local_receive_time()); + } + } + return timeStamp; + } + + // void writetoCSVforFusionTrack(std::string binaryfilename, std::string csvfilename) { + // FILE* file = std::fopen(binaryfilename.c_str(), "rb"); + // std::fseek(file, 0L, SEEK_END); // seek to end + // std::size_t length = std::ftell(file); + // std::fseek(file, 0L, SEEK_SET); // seek to start + // std::vector buffer(length); + // std::fread(buffer.data(), sizeof(uint8_t), buffer.size(), file); + // std::fclose(file); + // auto logKUKAiiwaFusionTrack = grl::flatbuffer::GetLogKUKAiiwaFusionTrack(&buffer[0]); + // auto states = logKUKAiiwaFusionTrack->states(); + // std::size_t state_size = states->size(); + // std::cout<< "------FusionTrack State Size: "< deviceTime = grl:: getTimeStamp_FusionTrack(logKUKAiiwaFusionTrack, "device_time"); + // std::vector local_request_time = grl:: getTimeStamp_FusionTrack(logKUKAiiwaFusionTrack, "local_request_time"); + // std::vector local_receive_time = grl:: getTimeStamp_FusionTrack(logKUKAiiwaFusionTrack, "local_receive_time"); + + // // create an ofstream for the file output (see the link on streams for more info) + + // std::ofstream fs; + // // create a name for the file output + + // fs.open(csvfilename, std::ofstream::out | std::ofstream::app); + // // write the file headers + // fs << "local_request_time" << ",local_receive_time" << ",Time_Difference" << ",Device_Time" <(futureAngle); @@ -122,12 +122,12 @@ void SetRBDynArmFromVrep( simGetJointPosition(vrepJointHandles[i],&futureAngle); /// @todo TODO(ahundt) warn/error when size!=0 or explicitly handle those cases if(simArmConfig.q[jointIdx].size()>0) simArmConfig.q[jointIdx][0] = futureAngle; - + /// @todo TODO(ahundt) add torque information // simSetJointTargetVelocity(jointHandles_[i],jointAngles_dt[i]/simulationTimeStep); // simSetJointTargetPosition(jointHandles_[i],jointAngles_dt[i]); // simSetJointTargetPosition(jointHandles_[i],futureAngle); - + if(!debug.empty()) { str+=boost::lexical_cast(futureAngle); @@ -159,38 +159,38 @@ class InverseKinematicsVrepPlugin public: //typedef grl::InverseKinematicsController parent_type; - + //using parent_type::currentKinematicsStateP_; //using parent_type::parent_type::InitializeKinematicsState; - + enum VrepVFControllerParamsIndex { DesiredKinematicsObjectName, IKGroupName }; - + typedef std::tuple< std::string // DesiredKinematicsObjectName ,std::string // IKGroupName > Params; - + static Params defaultParams() { return std::make_tuple("RobotMillTipTarget","IK_Group1_iiwa"); } - + /// @todo need to call parent constructor: /*! Constructor */ InverseKinematicsVrepPlugin() { } - + /// @todo TODO(ahundt) accept params for both simulated and measured arms void construct(Params params = defaultParams()){ // get kinematics group name // get number of joints logger_ = spdlog::get("console"); - + // Get the arm that will be used to generate simulated results to command robot // the "base" of this ik is Robotiiwa VrepRobotArmDriverSimulatedP_ = std::make_shared(); @@ -201,10 +201,10 @@ class InverseKinematicsVrepPlugin VrepRobotArmDriverMeasuredP_->construct(); measuredJointHandles_ = VrepRobotArmDriverMeasuredP_->getJointHandles(); measuredJointNames_ = VrepRobotArmDriverMeasuredP_->getJointNames(); - + ikGroupHandle_ = simGetIkGroupHandle(std::get(params).c_str()); simSetExplicitHandling(ikGroupHandle_,1); // enable explicit handling for the ik - + auto armDriverSimulatedParams = VrepRobotArmDriverSimulatedP_->getParams(); // the base frame of the arm ikGroupBaseName_ = (std::get(armDriverSimulatedParams)); @@ -213,7 +213,7 @@ class InverseKinematicsVrepPlugin // the target, or where the tip of the arm should go ikGroupTargetName_ = (std::get(armDriverSimulatedParams)); robotFlangeTipName_ = (std::get(armDriverSimulatedParams)); - + ikGroupBaseHandle_ = grl::vrep::getHandle(ikGroupBaseName_); ikGroupTipHandle_ = grl::vrep::getHandle(ikGroupTipName_); ikGroupTargetHandle_ = grl::vrep::getHandle(ikGroupTargetName_); @@ -223,17 +223,17 @@ class InverseKinematicsVrepPlugin // start by assuming the base is fixed bool isFixed = true; bool isForwardJoint = true; - + { // Initalize the RBDyn arm representation - + jointHandles_ = VrepRobotArmDriverSimulatedP_->getJointHandles(); jointNames_ = VrepRobotArmDriverSimulatedP_->getJointNames(); - + linkNames_ =VrepRobotArmDriverSimulatedP_->getLinkNames(); linkHandles_ =VrepRobotArmDriverSimulatedP_->getLinkHandles(); linkRespondableNames_ = VrepRobotArmDriverSimulatedP_->getLinkRespondableNames(); linkRespondableHandles_ = VrepRobotArmDriverSimulatedP_->getLinkRespondableHandles(); - + // save the initial joint angles for later // and set the arm to the 0 position for initialization of the // optimizer @@ -244,7 +244,7 @@ class InverseKinematicsVrepPlugin simSetJointPosition(handle,0); initialJointAngles.push_back(angle); } - + rbd_bodyNames_.push_back(ikGroupBaseName_); // note: bodyNames are 1 longer than link names, and start with the base! boost::copy(linkNames_, std::back_inserter(rbd_bodyNames_)); @@ -256,19 +256,19 @@ class InverseKinematicsVrepPlugin rbd_jointNames_.push_back(robotFlangeTipName_); rbd_jointNames_.push_back("cutter_joint"); rbd_jointNames_.push_back(ikGroupTipName_); - + rbd_bodyNames_.push_back("cutter_joint"); rbd_bodyNames_.push_back(ikGroupTipName_); getHandles(rbd_bodyNames_,std::back_inserter(bodyHandles_)); getHandles(rbd_jointNames_,std::back_inserter(rbd_jointHandles_)); - + // Note that V-REP specifies full transforms to place objects that rotate joints around the Z axis - + // based on: https://github.com/jrl-umi3218/Tasks/blob/master/tests/arms.h#L34 // and https://github.com/jrl-umi3218/RBDyn/issues/18#issuecomment-257214536 for(std::size_t i = 0; i < rbd_jointNames_.size(); i++) { - + // Note that V-REP specifies full transforms to place objects that rotate joints around the Z axis std::string thisJointName = rbd_jointNames_[i]; rbd::Joint::Type jointType = rbd::Joint::Fixed; @@ -280,18 +280,18 @@ class InverseKinematicsVrepPlugin rbd::Joint j_i(jointType, Eigen::Vector3d::UnitZ(), isForwardJoint, thisJointName); rbd_mbg_.addJoint(j_i); } - + // Note that V-REP specifies full transforms to place objects that rotate joints around the Z axis /// @todo TODO(ahundt) last "joint" RobotMillTip is really fixed... //rbd::Joint j_i(rbd::Joint::Fixed, Eigen::Vector3d::UnitZ(), isForwardJoint, ikGroupTipName_); //rbd::Joint j_i(rbd::Joint::Fixed, Eigen::Vector3d::UnitZ(), isForwardJoint, ikGroupTipName_); //rbd_mbg_.addJoint(j_i); - - + + for(std::size_t i = 0; i < rbd_bodyNames_.size(); i++) { /// @todo TODO(ahundt) extract real inertia and center of mass from V-REP with http://www.coppeliarobotics.com/helpFiles/en/regularApi/simGetShapeMassAndInertia.htm - + double mass = 1.; auto I = Eigen::Matrix3d::Identity(); auto h = Eigen::Vector3d::Zero(); @@ -299,21 +299,21 @@ class InverseKinematicsVrepPlugin sva::RBInertiad rbi_i(mass, h, I); /// @todo TODO(ahundt) add real support for links, particularly the respondable aspects, see LBR_iiwa_14_R820_joint1_resp in RoboneSimulation.ttt // @todo TODO(ahundt) REMEMBER: The first joint is NOT respondable! - + // remember, bodyNames[0] is the ikGroupBaseName, so entity order is // bodyNames[i], joint[i], bodyNames[i+1] std::string bodyName(rbd_bodyNames_[i]); rbd::Body b_i(rbi_i,bodyName); - + rbd_mbg_.addBody(b_i); } - + std::string dummyName0(("Dummy"+ boost::lexical_cast(0+10))); int currentDummy0 = simGetObjectHandle(dummyName0.c_str()); Eigen::Affine3d eto0 = getObjectTransform(rbd_jointHandles_[0],-1); logger_->info("{} \n{}", dummyName0 , eto0.matrix()); setObjectTransform(currentDummy0,-1,eto0); - + std::vector frameHandles; frameHandles.push_back(simGetObjectHandle(ikGroupBaseName_.c_str())); boost::copy(rbd_jointHandles_,std::back_inserter(frameHandles)); @@ -325,59 +325,59 @@ class InverseKinematicsVrepPlugin // this should be the identity matrix because we set the joints to 0! //sva::PTransformd from(getJointPTransform(rbd_jointHandles_[i+1])); sva::PTransformd from(sva::PTransformd::Identity()); - + // Link the PREVIOUSLY created body to the currently created body with the PREVIOUSLY created joint // remember, bodyNames[0] is the ikGroupBaseName, so entity order is // bodyNames[i], joint[i], bodyNames[i+1] std::string prevBody = rbd_bodyNames_[i]; std::string curJoint = rbd_jointNames_[i]; std::string nextBody = rbd_bodyNames_[i+1]; - + rbd_mbg_.linkBodies(prevBody, to, nextBody, from, curJoint); - + } - - + + // note: Tasks takes transforms in the successor (child link) frame, so it is the inverse of v-rep rbd_mbs_.push_back(rbd_mbg_.makeMultiBody(ikGroupBaseName_,isFixed,X_base)); rbd_mbcs_.push_back(rbd::MultiBodyConfig(rbd_mbs_[0])); rbd_mbcs_[0].zero(rbd_mbs_[0]); - + // we only have one robot for the moment so the index of it is 0 const std::size_t simulatedRobotIndex = 0; auto& simArmMultiBody = rbd_mbs_[simulatedRobotIndex]; auto& simArmConfig = rbd_mbcs_[simulatedRobotIndex]; - + // update the simulated arm position for (std::size_t i = 0; i < jointHandles_.size(); ++i) { simSetJointPosition(jointHandles_[i],initialJointAngles[i]); } SetRBDynArmFromVrep(jointNames_,jointHandles_,simArmMultiBody,simArmConfig); - + rbd::forwardKinematics(simArmMultiBody, simArmConfig); rbd::forwardVelocity(simArmMultiBody, simArmConfig); - + // set the preferred position to the initial position // https://github.com/jrl-umi3218/Tasks/blob/15aff94e3e03f6a161a87799ca2cf262b756bd0c/src/QPTasks.h#L426 rbd_preferred_mbcs_.push_back(simArmConfig); - + debugFrames(); - + } - + /// @todo verify object lifetimes - + // add virtual fixtures? need names and number of rows - - + + /// @todo read objective rows from vrep - + /// @todo set vrep explicit handling of IK here, plus unset in destructor of this object - + } - - - + + + /// Set dummy frames named Dummy0 to Dummy19 to the current state of the /// RBDyn and V-REP representations of joints for debugging. void debugFrames(bool print = false) @@ -397,13 +397,13 @@ class InverseKinematicsVrepPlugin eto = getObjectTransform(jointHandles_[i],-1); setObjectTransform(currentDummy2,-1,eto); if(print) logger_->info("{} V-REP World\n{}",dummyName2 , eto.matrix()); - + if(i>0) { Eigen::Affine3d NextJointinPrevFrame(getObjectTransform(jointHandles_[i],jointHandles_[i-1])); if(print) logger_->info("{} V-REP JointInPrevFrame\n{}",dummyName2 , NextJointinPrevFrame.matrix()); } - + bool dummy_world_frame = true; if( dummy_world_frame ) { @@ -418,7 +418,7 @@ class InverseKinematicsVrepPlugin sva::PTransform plinkToSon = simArmConfig.parentToSon[simArmMultiBody.bodyIndexByName(linkNames_[i])]; Eigen::Affine3d linkToSon = PTranformToEigenAffine(plinkToSon); if(print) logger_->info("{} RBDyn ParentLinkToSon\n{}",dummyName, linkToSon.matrix()); - + } else { @@ -430,27 +430,27 @@ class InverseKinematicsVrepPlugin if(print) logger_->info("{} RBDyn\n{}",dummyName , linkWorld.matrix()); setObjectTransform(currentDummy,prevDummy,linkWorld); prevDummy=currentDummy; - + } } - + // Frame "Dummy" with no numbers goes at the tip! Eigen::Affine3d tipTf = PTranformToEigenAffine(simArmConfig.bodyPosW[simArmMultiBody.bodyIndexByName(ikGroupTipName_)]); setObjectTransform(simGetObjectHandle("Dummy"),-1,tipTf); } - - - + + + void testPose(){ - + /// simulation tick time step in float seconds from vrep API call float simulationTimeStep = simGetSimulationTimeStep(); - + // we only have one robot for the moment so the index of it is 0 const std::size_t simulatedRobotIndex = 0; auto& simArmMultiBody = rbd_mbs_[simulatedRobotIndex]; auto& simArmConfig = rbd_mbcs_[simulatedRobotIndex]; - + std::vector vrepJointAngles; double angle = 0.7; for(auto i : jointHandles_) @@ -459,19 +459,19 @@ class InverseKinematicsVrepPlugin simSetJointPosition(i,angle); vrepJointAngles.push_back(angle); } - + /// @todo TODO(ahundt) rethink where/when/how to send command for the joint angles. Return to LUA? Set Directly? Distribute via vrep send message command? - + //////////////////////////////////////////////////// // Set joints to current arm position in simulation SetRBDynArmFromVrep(jointNames_,jointHandles_,simArmMultiBody,simArmConfig); rbd::forwardKinematics(simArmMultiBody, simArmConfig); rbd::forwardVelocity(simArmMultiBody, simArmConfig); - + debugFrames(); } - - + + /// Configures updateKinematics with the goal the kinematics should aim for enum class GoalPosE { realGoalPosition, debugGoalPosition }; /// Configures updateKinematics the algorithm the kinematics should use for solving @@ -484,8 +484,8 @@ class InverseKinematicsVrepPlugin /// simulatedArm is the current simulation, measuredArm /// is based off of data measured from the real physical arm enum class ArmStateSource { simulatedArm, measuredArm }; - - + + /// Runs inverse kinematics or constrained optimization at every simulation time step /// @param runOnce Set runOnce = true to only update kinematics once for debugging purposes. runOnce = false runs this function at every time step. /// @param solveForPosition defines where the arm is aiming for, the moving goal pose object or a stationary debug position to verify the algorithm @@ -505,13 +505,13 @@ class InverseKinematicsVrepPlugin ){ if(runOnce && ranOnce_) return; ranOnce_ = true; - + jointHandles_ = VrepRobotArmDriverSimulatedP_->getJointHandles(); - - + + /////////////////////////////////////////////////////////// // Copy Joint Interval, the range of motion for each joint - + /// @todo TODO(ahundt) change source of current state based on if physical arm is running if(syncSimulatedKinematicsState == ArmStateSource::simulatedArm) { @@ -522,22 +522,22 @@ class InverseKinematicsVrepPlugin VrepRobotArmDriverMeasuredP_->getState(currentArmState_); } - + // lower limits auto & llim = std::get(currentArmState_); std::vector llimits(llim.begin(),llim.end()); - + // upper limits auto & ulim = std::get(currentArmState_); std::vector ulimits(ulim.begin(),ulim.end()); - + // current position auto & currentJointPos = std::get(currentArmState_); std::vector currentJointPosVec(currentJointPos.begin(),currentJointPos.end()); - + /// @todo TODO(ahundt) also copy acceleration velocity, torque, etc - - + + const auto& handleParams = VrepRobotArmDriverSimulatedP_->getVrepHandleParams(); Eigen::Affine3d currentEndEffectorPose = getObjectTransform( @@ -545,30 +545,30 @@ class InverseKinematicsVrepPlugin ,std::get(handleParams) ); auto currentEigenT = currentEndEffectorPose.translation(); - - + + Eigen::Affine3d desiredEndEffectorPose = getObjectTransform( std::get(handleParams) ,std::get(handleParams) ); auto desiredEigenT = desiredEndEffectorPose.translation(); - + ////////////////////// /// @todo TODO(ahundt) move code below here back into separate independent setup and solve functions, move some steps like limits to construct() - + /// simulation tick time step in float seconds from vrep API call float simulationTimeStep = simGetSimulationTimeStep(); - + // we only have one robot for the moment so the index of it is 0 const std::size_t simulatedRobotIndex = 0; auto& simArmMultiBody = rbd_mbs_[simulatedRobotIndex]; auto& simArmConfig = rbd_mbcs_[simulatedRobotIndex]; - - - + + + /// @todo TODO(ahundt) rethink where/when/how to send command for the joint angles. Return to LUA? Set Directly? Distribute via vrep send message command? - + //////////////////////////////////////////////////// // Set joints to current arm position in simulation if(syncSimulatedKinematicsState == ArmStateSource::simulatedArm) @@ -586,7 +586,7 @@ class InverseKinematicsVrepPlugin } rbd::forwardKinematics(simArmMultiBody, simArmConfig); rbd::forwardVelocity(simArmMultiBody, simArmConfig); - + // save the current MultiBodyConfig for comparison after running algorithms rbd_prev_mbcs_ = rbd_mbcs_; // set the preferred position to the current position @@ -595,7 +595,7 @@ class InverseKinematicsVrepPlugin /// @todo TODO(ahundt) make solver object a member variable if possible, initialize in constructor tasks::qp::QPSolver solver; - + //////////////////////////////////////////////////// // Set position goal of the arm sva::PTransformd targetWorldTransform; @@ -604,42 +604,42 @@ class InverseKinematicsVrepPlugin { // go to the real target position targetWorldTransform = getObjectPTransform(ikGroupTargetHandle_); - logger_->info("target translation (vrep format):\n{}", targetWorldTransform.translation()); + // logger_->info("target translation (vrep format):\n{}", targetWorldTransform.translation()); } else { // go to a debugging target position targetWorldTransform = simArmConfig.bodyPosW[simArmMultiBody.bodyIndexByName(ikGroupTipName_)]; targetWorldTransform.translation().z() -= 0.0001; - logger_->info("target translation (rbdyn format):\n", targetWorldTransform.translation()); + // logger_->info("target translation (rbdyn format):\n", targetWorldTransform.translation()); } tasks::qp::PositionTask posTask(rbd_mbs_, simulatedRobotIndex, ikGroupTipName_,targetWorldTransform.translation()); tasks::qp::SetPointTask posTaskSp(rbd_mbs_, simulatedRobotIndex, &posTask, 50., 10.); tasks::qp::OrientationTask oriTask(rbd_mbs_,simulatedRobotIndex, ikGroupTipName_,targetWorldTransform.rotation()); tasks::qp::SetPointTask oriTaskSp(rbd_mbs_, simulatedRobotIndex, &oriTask, 50., 1.); tasks::qp::PostureTask postureTask(rbd_mbs_,simulatedRobotIndex,rbd_preferred_mbcs_[simulatedRobotIndex].q,1,0.01); - + double inf = std::numeric_limits::infinity(); - - + + //////////////////////////////////// // Set joint limits - + // joint limit objects, initialize to current q so entries // will be reasonable, such as empty entries for fixed joints std::vector > lBound = simArmConfig.q; std::vector > uBound = simArmConfig.q; std::vector > lVelBound = simArmConfig.alpha; std::vector > uVelBound = simArmConfig.alpha; - - + + // for all joints for (std::size_t i=0 ; i < rbd_jointHandles_.size()-1; i++) { /// limits must be organized as described in https://github.com/jrl-umi3218/Tasks/issues/10#issuecomment-257793242 std::string jointName = rbd_jointNames_[i]; std::size_t jointIdx = simArmMultiBody.jointIndexByName(jointName); - + /// @todo TODO(ahundt) ulimits aren't the same size as jointHandles_, need velocity limits too // set joint position limits if(boost::iequals(jointName,"cutter_joint")) @@ -654,7 +654,7 @@ class InverseKinematicsVrepPlugin lBound[jointIdx][0] = llimits[i]; uBound[jointIdx][0] = ulimits[i]; } - + // set joint velocity limits if(i logger_; rbd::MultiBodyGraph rbd_mbg_; @@ -785,35 +785,35 @@ class InverseKinematicsVrepPlugin /// because we need additional fixed "joints" so we have transforms /// that match the V-REP scene std::vector rbd_jointHandles_; - - + + //tasks::qp::QPSolver qp_solver_; - + std::vector jointHandles_; ///< @todo move this item back into VrepRobotArmDriver std::vector linkHandles_; std::vector linkRespondableHandles_; std::vector jointNames_; std::vector linkNames_; std::vector linkRespondableNames_; - - + + std::vector measuredJointHandles_; ///< @todo move this item back into VrepRobotArmDriver std::vector measuredJointNames_; - + int ikGroupHandle_; int ikGroupBaseHandle_; int ikGroupTipHandle_; int ikGroupTargetHandle_; - + std::string ikGroupBaseName_; std::string ikGroupTipName_; std::string ikGroupTargetName_; std::string robotFlangeTipName_; // not part of the V-REP ik group - + std::shared_ptr VrepRobotArmDriverSimulatedP_; std::shared_ptr VrepRobotArmDriverMeasuredP_; vrep::VrepRobotArmDriver::State currentArmState_; - + bool ranOnce_ = false; EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; diff --git a/include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp b/include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp index bcbabd4..813d498 100644 --- a/include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp +++ b/include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp @@ -157,18 +157,17 @@ class KukaVrepPlugin : public std::enable_shared_from_this { // keep driver threads from exiting immediately after creation, because they have work to do! device_driver_workP_.reset(new boost::asio::io_service::work(device_driver_io_service)); kukaDriverP_=std::make_shared(std::make_tuple( - - std::get(params), - std::get(params), - std::get(params), - std::get(params), - std::get(params), - std::get(params), - std::get(params), - std::get(params), - std::get(params), - std::get(params), - std::get(params) + std::get(params), + std::get(params), + std::get(params), + std::get(params), + std::get(params), + std::get(params), + std::get(params), + std::get(params), + std::get(params), + std::get(params), + std::get(params) )); kukaDriverP_->construct(); // Default to joint servo mode for commanding motion diff --git a/include/thirdparty/cartographer/include/cartographer/common/time.h b/include/thirdparty/cartographer/include/cartographer/common/time.h index 62446a7..64648a5 100644 --- a/include/thirdparty/cartographer/include/cartographer/common/time.h +++ b/include/thirdparty/cartographer/include/cartographer/common/time.h @@ -49,8 +49,7 @@ struct UniversalTimeScaleClock { // << microsecsinceepoch - microsec << '\n'; return time_point ( - duration_cast(system_clock::now().time_since_epoch()) - - seconds(kUtsEpochOffsetFromUnixEpochInSeconds) + duration_cast(system_clock::now().time_since_epoch()) ); } }; diff --git a/src/lua/robone.lua b/src/lua/robone.lua index f989b28..071ab4e 100644 --- a/src/lua/robone.lua +++ b/src/lua/robone.lua @@ -507,7 +507,7 @@ robone.configureOpticalTracker=function() simExtAtracsysFusionTrackStart() -- Test the below one -- Enable the recordDataScript() to call the method below. - -- simExtAtracsysFusionTrackRecordWhileSimulationIsRunning(true) + simExtAtracsysFusionTrackRecordWhileSimulationIsRunning(true) end @@ -524,19 +524,19 @@ end robone.recordDataScript=function() -- Check if the required plugin is there: - if (grl.isModuleLoaded('KukaLBRiiwa')) then - simExtKukaLBRiiwaRecordWhileSimulationIsRunning(true) - else - simDisplayDialog('Warning','KukaLBRiiwa plugin was not found so data will not be logged.',sim_dlgstyle_ok,true,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1}) - end + if (grl.isModuleLoaded('KukaLBRiiwa')) then + simExtKukaLBRiiwaRecordWhileSimulationIsRunning(true) + else + simDisplayDialog('Warning','KukaLBRiiwa plugin was not found so data will not be logged.',sim_dlgstyle_ok,true,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1}) + end - if (grl.isModuleLoaded('') then + if (not grl.isModuleLoaded('AtracsysFusionTrack')) then simExtAtracsysFusionTrackRecordWhileSimulationIsRunning(true) - else + else simDisplayDialog('Warning','Atracsys plugin was not found so data will not be logged.',sim_dlgstyle_ok,true,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1}) - end +end -------------------------------------------------------------------------- -- Get External Joint Torque Data from KUKA iiwa real arm driver plugin -- diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 0b56b63..db380ea 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -96,7 +96,7 @@ if(TARGET KukaFRIClient OR FRI_Client_SDK_Cpp_FOUND) basis_target_link_libraries(kukaiiwaTest ${Boost_LIBRARIES} ${Boost_REGEX_LIBRARY} ${FRI_Client_SDK_Cpp_LIBRARIES} ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} KukaFRIClient ${FLATBUFFERS_STATIC_LIB}) basis_add_executable(readFRIFBTest.cpp)# ${GRL_FLATBUFFERS_OUTPUTS}) - basis_target_link_libraries(readFRIFBTest ${Boost_LIBRARIES} ${Boost_REGEX_LIBRARY} ${FRI_Client_SDK_Cpp_LIBRARIES} ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} KukaFRIClient ${FLATBUFFERS_STATIC_LIB}) + basis_target_link_libraries(readFRIFBTest ${Boost_LIBRARIES} ${Boost_REGEX_LIBRARY} ${FRI_Client_SDK_Cpp_LIBRARIES} ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} KukaFRIClient ${FLATBUFFERS_STATIC_LIB} -lboost_iostreams -lboost_system -lboost_filesystem) endif() diff --git a/test/KukaFRITest.cpp b/test/KukaFRITest.cpp index be1d367..4c76f0b 100644 --- a/test/KukaFRITest.cpp +++ b/test/KukaFRITest.cpp @@ -49,161 +49,161 @@ enum class DriverToUse int main(int argc, char* argv[]) { - bool debug = true; - int print_every_n = 100; - std::size_t q_size = 4096; //queue size must be power of 2 - spdlog::set_async_mode(q_size); - std::shared_ptr loggerPG; - try { - loggerPG = spdlog::stdout_logger_mt("console"); - } - catch (spdlog::spdlog_ex ex) { - loggerPG = spdlog::get("console"); - } + bool debug = true; + int print_every_n = 100; + std::size_t q_size = 4096; //queue size must be power of 2 + spdlog::set_async_mode(q_size); + std::shared_ptr loggerPG; + try { + loggerPG = spdlog::stdout_logger_mt("console"); + } + catch (spdlog::spdlog_ex ex) { + loggerPG = spdlog::get("console"); + } - grl::periodic<> callIfMinPeriodPassed; - // defines how the robot will move when this test is executed - HowToMove howToMove = HowToMove::absolute_position_with_relative_rotation;//HowToMove::absolute_position; HowToMove::relative_position; - - // change which part of the driver is being tested. - // It is divided into several levels of functionality - // and the java program you need to execute varies based on - // which selection you make. - // - // Run GRL_Driver on the Kuka teach pendant with: - // - // DriverToUse::kuka_driver_high_level_class - // - // Run FRI_HoldsPosition_command on the KUKA teach pendant with: - // - // DriverToUse::low_level_fri_function, - // DriverToUse::low_level_fri_class, - // - DriverToUse driverToUse = DriverToUse::kuka_driver_high_level_class; - - try - { - std::string localhost("192.170.10.100"); - std::string localport("30200"); - std::string remotehost("192.170.10.2"); - std::string remoteport("30200"); + grl::periodic<> callIfMinPeriodPassed; + // defines how the robot will move when this test is executed + HowToMove howToMove = HowToMove::absolute_position_with_relative_rotation;//HowToMove::absolute_position; HowToMove::relative_position; - std::cout << "argc: " << argc << "\n"; - if (argc !=5 && argc !=1) + // change which part of the driver is being tested. + // It is divided into several levels of functionality + // and the java program you need to execute varies based on + // which selection you make. + // + // Run GRL_Driver on the Kuka teach pendant with: + // + // DriverToUse::kuka_driver_high_level_class + // + // Run FRI_HoldsPosition_command on the KUKA teach pendant with: + // + // DriverToUse::low_level_fri_function, + // DriverToUse::low_level_fri_class, + // + DriverToUse driverToUse = DriverToUse::kuka_driver_high_level_class; + + try { - loggerPG->error("Usage: ", argv[0], " \n"); - return 1; - } + std::string localhost("192.170.10.100"); + std::string localport("30200"); + std::string remotehost("192.170.10.2"); + std::string remoteport("30200"); - if(argc ==5){ - localhost = std::string(argv[1]); - localport = std::string(argv[2]); - remotehost = std::string(argv[3]); - remoteport = std::string(argv[4]); - } + std::cout << "argc: " << argc << "\n"; + if (argc !=5 && argc !=1) + { + loggerPG->error("Usage: ", argv[0], " \n"); + return 1; + } - std::cout << "using: " << argv[0] << " " << localhost << " " << localport << " " << remotehost << " " << remoteport << "\n"; - // A single class for an I/O service object. - boost::asio::io_service io_service; + if(argc ==5){ + localhost = std::string(argv[1]); + localport = std::string(argv[2]); + remotehost = std::string(argv[3]); + remoteport = std::string(argv[4]); + } - std::shared_ptr friData(std::make_shared(7)); - /// std::chrono::time_point startTime; - cartographer::common::Time startTime; + std::cout << "using: " << argv[0] << " " << localhost << " " << localport << " " << remotehost << " " << remoteport << "\n"; + // A single class for an I/O service object. + boost::asio::io_service io_service; - BOOST_VERIFY(friData); + std::shared_ptr friData(std::make_shared(7)); + /// std::chrono::time_point startTime; + cartographer::common::Time startTime; - double delta = -0.0005; - double delta_sum = 0; - /// consider moving joint angles based on time - int joint_to_move = 6; - loggerPG->warn("WARNING: THIS PROGRAM WILL ACTUALLY MOVE YOUR KUKA ROBOT!!!", - "YOU COULD INJURE YOURSELF, THE OTHERS AROUND YOU, ", - "AND DAMAGE OR DESTROY YOUR KUKA ROBOT. TAKE APPROPRIATE PRECAUTIONS. YOU ARE RESPONSIBLE. {}{}{}{}{}{}", - "if joint angle delta variable is too large with respect to ", - "the time it takes to go around the loop and change it. ", - "Current delta (radians/update): ", delta, " Joint to move: ", joint_to_move); + BOOST_VERIFY(friData); - std::vector ipoJointPos(7,0); - std::vector jointOffset(7,0); // length 7, value 0 - boost::container::static_vector jointStateToCommand(7,0); + double delta = -0.0005; + double delta_sum = 0; + /// consider moving joint angles based on time + int joint_to_move = 6; + loggerPG->warn("WARNING: THIS PROGRAM WILL ACTUALLY MOVE YOUR KUKA ROBOT!!!", + "YOU COULD INJURE YOURSELF, THE OTHERS AROUND YOU, ", + "AND DAMAGE OR DESTROY YOUR KUKA ROBOT. TAKE APPROPRIATE PRECAUTIONS. YOU ARE RESPONSIBLE. {}{}{}{}{}{}", + "if joint angle delta variable is too large with respect to ", + "the time it takes to go around the loop and change it. ", + "Current delta (radians/update): ", delta, " Joint to move: ", joint_to_move); - // Absolute goal position to travel to in some modes of HowToMove - // Set all 7 joints to go to a position 1 radian from the center - std::vector absoluteGoalPos(7,0.2); + std::vector ipoJointPos(7,0); + std::vector jointOffset(7,0); // length 7, value 0 + boost::container::static_vector jointStateToCommand(7,0); - /// TODO(ahundt) remove deprecated arm state from here and implementation - grl::robot::arm::KukaState armState; - std::unique_ptr lowLevelStepAlgorithmP; + // Absolute goal position to travel to in some modes of HowToMove + // Set all 7 joints to go to a position 1 radian from the center + std::vector absoluteGoalPos(7,0.2); - // Need to tell the system how long in milliseconds it has to reach the goal - // or it will never move! - std::size_t goal_position_command_time_duration = 4; - lowLevelStepAlgorithmP.reset(new grl::robot::arm::LinearInterpolation()); - // RobotModel (options are KUKA_LBR_IIWA_14_R820, KUKA_LBR_IIWA_7_R800) see grl::robot::arm::KukaState::KUKA_LBR_IIWA_14_R820 + /// TODO(ahundt) remove deprecated arm state from here and implementation + grl::robot::arm::KukaState armState; + std::unique_ptr lowLevelStepAlgorithmP; - std::shared_ptr> highLevelDriverClassP; + // Need to tell the system how long in milliseconds it has to reach the goal + // or it will never move! + std::size_t goal_position_command_time_duration = 4; + lowLevelStepAlgorithmP.reset(new grl::robot::arm::LinearInterpolation()); + // RobotModel (options are KUKA_LBR_IIWA_14_R820, KUKA_LBR_IIWA_7_R800) see grl::robot::arm::KukaState::KUKA_LBR_IIWA_14_R820 - if(driverToUse == DriverToUse::low_level_fri_class) - { - /// @todo TODO(ahundt) BUG: Need way to supply time to reach specified goal for position control and eliminate this allocation internally in the kuka driver. See similar comment in KukaFRIDriver.hpp - /// IDEA: PASS A LOW LEVEL STEP ALGORITHM PARAMS OBJECT ON EACH UPDATE AND ONLY ONE INSTANCE OF THE ALGORITHM OBJECT ITSELF - highLevelDriverClassP = std::make_shared>( - io_service, - std::make_tuple("KUKA_LBR_IIWA_14_R820", - localhost, - localport, - remotehost, - remoteport/*,4 ms per tick*/, - grl::robot::arm::KukaFRIClientDataDriver::run_automatically)); - } + std::shared_ptr> highLevelDriverClassP; - std::shared_ptr socketP; + if(driverToUse == DriverToUse::low_level_fri_class) + { + /// @todo TODO(ahundt) BUG: Need way to supply time to reach specified goal for position control and eliminate this allocation internally in the kuka driver. See similar comment in KukaFRIDriver.hpp + /// IDEA: PASS A LOW LEVEL STEP ALGORITHM PARAMS OBJECT ON EACH UPDATE AND ONLY ONE INSTANCE OF THE ALGORITHM OBJECT ITSELF + highLevelDriverClassP = std::make_shared>( + io_service, + std::make_tuple("KUKA_LBR_IIWA_14_R820", + localhost, + localport, + remotehost, + remoteport/*,4 ms per tick*/, + grl::robot::arm::KukaFRIClientDataDriver::run_automatically)); + } - if(driverToUse == DriverToUse::low_level_fri_function) - { + std::shared_ptr socketP; - socketP = std::make_shared(io_service, boost::asio::ip::udp::endpoint(boost::asio::ip::address::from_string(localhost), boost::lexical_cast(localport))); + if(driverToUse == DriverToUse::low_level_fri_function) + { - boost::asio::ip::udp::resolver resolver(io_service); - boost::asio::ip::udp::endpoint endpoint = *resolver.resolve({boost::asio::ip::udp::v4(), remotehost, remoteport}); - socketP->connect(endpoint); + socketP = std::make_shared(io_service, boost::asio::ip::udp::endpoint(boost::asio::ip::address::from_string(localhost), boost::lexical_cast(localport))); - /// @todo maybe there is a more convienient way to set this that is easier for users? perhaps initializeClientDataForiiwa()? - friData->expectedMonitorMsgID = KUKA::LBRState::LBRMONITORMESSAGEID; - } + boost::asio::ip::udp::resolver resolver(io_service); + boost::asio::ip::udp::endpoint endpoint = *resolver.resolve({boost::asio::ip::udp::v4(), remotehost, remoteport}); + socketP->connect(endpoint); - std::shared_ptr kukaDriverP; + /// @todo maybe there is a more convienient way to set this that is easier for users? perhaps initializeClientDataForiiwa()? + friData->expectedMonitorMsgID = KUKA::LBRState::LBRMONITORMESSAGEID; + } - if(driverToUse == DriverToUse::kuka_driver_high_level_class) - { - grl::robot::arm::KukaDriver::Params params = std::make_tuple( - "Robotiiwa" , // RobotName, - "KUKA_LBR_IIWA_14_R820" , // RobotModel (options are KUKA_LBR_IIWA_14_R820, KUKA_LBR_IIWA_7_R800) - "0.0.0.0" , // LocalUDPAddress - "30010" , // LocalUDPPort - "172.31.1.147" , // RemoteUDPAddress - "192.170.10.100" , // LocalHostKukaKoniUDPAddress, - "30200" , // LocalHostKukaKoniUDPPort, - remotehost , // RemoteHostKukaKoniUDPAddress, - remoteport , // RemoteHostKukaKoniUDPPort - "FRI" , // KukaCommandMode (options are FRI, JAVA) - "FRI" // KukaMonitorMode (options are FRI, JAVA) - ); - /// @todo TODO(ahundt) Currently assumes ip address - kukaDriverP = std::make_shared(params); - kukaDriverP->construct(); - // Default to joint servo mode for commanding motion - kukaDriverP->set(grl::flatbuffer::ArmState::MoveArmJointServo); - kukaDriverP->set(goal_position_command_time_duration,grl::time_duration_command_tag()); - std::cout << "KUKA COMMAND MODE: " << std::get(params) << "\n"; - /// kukaDriverP->start_recording(); + std::shared_ptr kukaDriverP; - } - std::size_t num_missed = 0; - std::size_t maximum_consecutive_missed_updates_before_warning = 100000; - std::size_t maximum_consecutive_missed_updates_before_exit = 100000000; - // run until the user kills the program with ctrl+c - for (std::size_t i = 0; !signalStatusG; ++i) { + if(driverToUse == DriverToUse::kuka_driver_high_level_class) + { + grl::robot::arm::KukaDriver::Params params = std::make_tuple( + "Robotiiwa" , // RobotName, + "KUKA_LBR_IIWA_14_R820" , // RobotModel (options are KUKA_LBR_IIWA_14_R820, KUKA_LBR_IIWA_7_R800) + "0.0.0.0" , // LocalUDPAddress + "30010" , // LocalUDPPort + "172.31.1.147" , // RemoteUDPAddress + "192.170.10.100" , // LocalHostKukaKoniUDPAddress, + "30200" , // LocalHostKukaKoniUDPPort, + remotehost , // RemoteHostKukaKoniUDPAddress, + remoteport , // RemoteHostKukaKoniUDPPort + "FRI" , // KukaCommandMode (options are FRI, JAVA) + "FRI" // KukaMonitorMode (options are FRI, JAVA) + ); + /// @todo TODO(ahundt) Currently assumes ip address + kukaDriverP = std::make_shared(params); + kukaDriverP->construct(); + // Default to joint servo mode for commanding motion + kukaDriverP->set(grl::flatbuffer::ArmState::MoveArmJointServo); + kukaDriverP->set(goal_position_command_time_duration,grl::time_duration_command_tag()); + std::cout << "KUKA COMMAND MODE: " << std::get(params) << "\n"; + /// kukaDriverP->start_recording(); + + } + std::size_t num_missed = 0; + std::size_t maximum_consecutive_missed_updates_before_warning = 100000; + std::size_t maximum_consecutive_missed_updates_before_exit = 100000000; + // run until the user kills the program with ctrl+c + for (std::size_t i = 0; !signalStatusG; ++i) { /// Save the interpolated joint position from the previous update as the base for some motions /// The interpolated position is where the JAVA side is commanding, @@ -265,21 +265,20 @@ int main(int argc, char* argv[]) // but we can't process the new data so try updating again immediately. if(!haveNewData && !recv_bytes_transferred) { - if(driverToUse == DriverToUse::low_level_fri_class || - driverToUse == DriverToUse::kuka_driver_high_level_class) - { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - ++num_missed; - if(num_missed > maximum_consecutive_missed_updates_before_warning) { - loggerPG->warn("No new data for ", num_missed, " milliseconds."); - continue; - } else if(num_missed > maximum_consecutive_missed_updates_before_exit) { - loggerPG->warn("No new data for ", num_missed, " milliseconds. Max time limit hit, Exiting..."); - break; - } else { - continue; - } + if(driverToUse == DriverToUse::low_level_fri_class || driverToUse == DriverToUse::kuka_driver_high_level_class) + { + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + ++num_missed; + if(num_missed > maximum_consecutive_missed_updates_before_warning) { + loggerPG->warn("No new data for ", num_missed, " milliseconds."); + continue; + } else if(num_missed > maximum_consecutive_missed_updates_before_exit) { + loggerPG->warn("No new data for ", num_missed, " milliseconds. Max time limit hit, Exiting..."); + break; + } else { + continue; + } } else { num_missed = 0; } @@ -344,8 +343,8 @@ int main(int argc, char* argv[]) " jointOffset: ", jointOffset); } - } - } + } + } catch (boost::exception &e) { std::string errmsg("If you get an error 'std::exception::what: bind: Can't assign requested address', check your network connection.\n\nKukaFRITest Main Test Loop Stopped:\n" + boost::diagnostic_information(e)); diff --git a/test/readFRIFBTest.cpp b/test/readFRIFBTest.cpp index 1f53ce1..3648e22 100644 --- a/test/readFRIFBTest.cpp +++ b/test/readFRIFBTest.cpp @@ -1,11 +1,154 @@ // Library includes #define FLATBUFFERS_DEBUG_VERIFICATION_FAILURE +#include "flatbuffers/util.h" #include "grl/kuka/readKukaiiwaFromFlatbuffer.hpp" +#include "grl/sensor/readFusionTrackFromFlatbuffer.hpp" +#include +#include "grl/flatbuffer/FusionTrack_generated.h" +#include "grl/flatbuffer/Time_generated.h" +#include "grl/flatbuffer/LogKUKAiiwaFusionTrack_generated.h" + +#include +#include +#include +#include + +void writetoCSVforFusionTrackKukaiiwa(std::string KUKAbinaryfilename, std::string FusionTrackbinaryfilename, std::string csvfilename); int main(int argc, char* argv[]) { - grl::getDeviceTime(); + std::string kukaBinaryfile = "/home/chunting/src/V-REP_PRO_EDU_V3_4_0_Linux/2018_01_31_16_11_27_Kukaiiwa.iiwa"; + std::string fusiontrackBinaryfile = "/home/chunting/src/V-REP_PRO_EDU_V3_4_0_Linux/2018_01_31_16_10_58_FusionTrack.flik"; + std::string KukaFusionTrackCSVFile = "/home/chunting/src/V-REP_PRO_EDU_V3_4_0_Linux/TimeEventCombined.csv"; + writetoCSVforFusionTrackKukaiiwa(kukaBinaryfile, fusiontrackBinaryfile, KukaFusionTrackCSVFile); + // fbs_tk::Root KUKAiiwaStatesRoot = fbs_tk::open_root(kukaBinaryfile); + + // auto states = KUKAiiwaStatesRoot->states(); + // std::size_t state_size = states->size(); + // std::cout<< "++++++Kuka_state_size: "< KUKAiiwaStatesRoot = fbs_tk::open_root(KUKAbinaryfilename); + auto KUKA_states = KUKAiiwaStatesRoot->states(); + std::size_t kuka_state_size = KUKA_states->size(); + std::cout<< "------Kuka_state_size: "< kuka_deviceTime = grl:: getTimeStamp_Kuka(KUKAiiwaStatesRoot, "device_time"); + std::vector kuka_local_request_time = grl::getTimeStamp_Kuka(KUKAiiwaStatesRoot, "local_request_time"); + std::vector kuka_local_receive_time = grl::getTimeStamp_Kuka(KUKAiiwaStatesRoot, "local_receive_time"); + fbs_tk::Root FusionTrackStatesRoot = fbs_tk::open_root(FusionTrackbinaryfilename); + + auto FT_states = FusionTrackStatesRoot->states(); + std::size_t FT_state_size = FT_states->size(); + std::cout<< "------FusionTrack State Size: "< FT_deviceTime = grl:: getTimeStamp_FusionTrack(FusionTrackStatesRoot, "device_time"); + std::vector FT_local_request_time = grl::getTimeStamp_FusionTrack(FusionTrackStatesRoot, "local_request_time"); + std::vector FT_local_receive_time = grl::getTimeStamp_FusionTrack(FusionTrackStatesRoot, "local_receive_time"); + std::vector local_request_time; + std::vector local_receive_time_offset_FT; + std::vector local_receive_time_offset_kuka; + std::vector device_time_offset_kuka; + std::vector device_time_offset_FT; + int64_t initial_local_request_time = std::min(kuka_local_request_time[0], FT_local_request_time[0]); + int64_t initial_local_receive_time_kuka = kuka_local_receive_time[0]; + int64_t initial_local_receive_time_FT = FT_local_receive_time[0]; + int64_t initial_device_time_kuka = kuka_deviceTime[0]; + int64_t initial_device_time_FT = FT_deviceTime[0]; + + auto itkuka_local_request_time = kuka_local_request_time.begin(); + auto itFT_local_request_time = FT_local_request_time.begin(); + + auto itkuka_local_request_timeEnd = kuka_local_request_time.end(); + auto itFT_local_request_timeEnd = FT_local_request_time.end(); + + local_request_time.reserve(kuka_state_size+FT_state_size); + local_receive_time_offset_FT.reserve(kuka_state_size+FT_state_size); + local_receive_time_offset_kuka.reserve(kuka_state_size+FT_state_size); + device_time_offset_kuka.reserve(kuka_state_size+FT_state_size); + device_time_offset_FT.reserve(kuka_state_size+FT_state_size); + + int kuka_index = 0; + int FT_index = 0; + + while ( itkuka_local_request_time != itkuka_local_request_timeEnd && + itFT_local_request_time != itFT_local_request_timeEnd ) + { + if ( *itkuka_local_request_time < *itFT_local_request_time ){ + local_request_time.push_back( *itkuka_local_request_time - initial_local_request_time); + local_receive_time_offset_FT.push_back(FT_local_receive_time[FT_index] - initial_local_receive_time_FT); + local_receive_time_offset_kuka.push_back(kuka_local_receive_time[kuka_index] - initial_local_receive_time_kuka); + device_time_offset_kuka.push_back(kuka_deviceTime[kuka_index]-initial_device_time_kuka); + device_time_offset_FT.push_back(FT_deviceTime[FT_index]-initial_device_time_FT); + kuka_index++; + itkuka_local_request_time++; + + + } else if( *itkuka_local_request_time > *itFT_local_request_time ) { + local_request_time.push_back( *itFT_local_request_time - initial_local_request_time ); + local_receive_time_offset_FT.push_back(FT_local_receive_time[FT_index] - initial_local_receive_time_FT); + local_receive_time_offset_kuka.push_back(kuka_local_receive_time[kuka_index] - initial_local_receive_time_kuka); + device_time_offset_kuka.push_back(kuka_deviceTime[kuka_index]-initial_device_time_kuka); + device_time_offset_FT.push_back(FT_deviceTime[FT_index]-initial_device_time_FT); + FT_index++; + itFT_local_request_time++; + } else { + local_request_time.push_back( *itFT_local_request_time - initial_local_request_time ); + local_receive_time_offset_FT.push_back(FT_local_receive_time[FT_index] - initial_local_receive_time_FT); + local_receive_time_offset_kuka.push_back(kuka_local_receive_time[kuka_index] - initial_local_receive_time_kuka); + device_time_offset_kuka.push_back(kuka_deviceTime[kuka_index]-initial_device_time_kuka); + device_time_offset_FT.push_back(FT_deviceTime[FT_index]-initial_device_time_FT); + FT_index++; + kuka_index++; + itkuka_local_request_time++; + itFT_local_request_time++; + } + } + + // copy rest of kuka_local_request_time array + while ( itkuka_local_request_time != itkuka_local_request_timeEnd ){ + FT_index = FT_state_size-1; + local_request_time.push_back( *itkuka_local_request_time - initial_local_request_time); + local_receive_time_offset_FT.push_back(FT_local_receive_time[FT_index] - initial_local_receive_time_FT); + local_receive_time_offset_kuka.push_back(kuka_local_receive_time[kuka_index] - initial_local_receive_time_kuka); + device_time_offset_kuka.push_back(kuka_deviceTime[kuka_index]-initial_device_time_kuka); + device_time_offset_FT.push_back(FT_deviceTime[FT_index]-initial_device_time_FT); + kuka_index++; + itkuka_local_request_time++; + } + + + // copy rest of FT_local_request_time array + while ( itFT_local_request_time != itFT_local_request_timeEnd ) { + kuka_index = kuka_state_size-1; + local_request_time.push_back( *itFT_local_request_time - initial_local_request_time ); + local_receive_time_offset_FT.push_back(FT_local_receive_time[FT_index] - initial_local_receive_time_FT); + local_receive_time_offset_kuka.push_back(kuka_local_receive_time[kuka_index] - initial_local_receive_time_kuka); + device_time_offset_kuka.push_back(kuka_deviceTime[kuka_index]-initial_device_time_kuka); + device_time_offset_FT.push_back(FT_deviceTime[FT_index]-initial_device_time_FT); + FT_index++; + itFT_local_request_time++; + } + // // create an ofstream for the file output (see the link on streams for more info) + std::ofstream fs; + // create a name for the file output + fs.open( csvfilename, std::ofstream::out | std::ofstream::app); + // write the file headers + fs << "local_request_time" << ",local_receive_time_offset_FT" << ",local_receive_time_offset_kuka" <<",device_time_offset_FT" << ",device_time_offset_kuka" << std::endl; + for(int i=0; i Date: Thu, 1 Feb 2018 18:49:53 -0500 Subject: [PATCH 041/102] Modify the TimeEvent --- include/grl/kuka/KukaFRIdriver.hpp | 39 +++++++++++++++++++++++------- 1 file changed, 30 insertions(+), 9 deletions(-) diff --git a/include/grl/kuka/KukaFRIdriver.hpp b/include/grl/kuka/KukaFRIdriver.hpp index 84ba3c8..2956f85 100644 --- a/include/grl/kuka/KukaFRIdriver.hpp +++ b/include/grl/kuka/KukaFRIdriver.hpp @@ -155,6 +155,13 @@ class KukaFRIdriver : public std::enable_shared_from_this(Kukatime.time_since_epoch())); + } + + /// TODO(ahundt) currently assuming the FusionTrack timestamp is from the unix time epoch + cartographer::common::Time KukaTimeToCommonTime( std::chrono::time_point Kukatime) { return cartographer::common::Time( std::chrono::duration_cast(Kukatime.time_since_epoch())); @@ -163,15 +170,29 @@ class KukaFRIdriver : public std::enable_shared_from_this timestamp; + timestamp += std::chrono::seconds(friTimeStamp.sec) + std::chrono::nanoseconds(friTimeStamp.nanosec); + + // std::chrono::seconds sec(friTimeStamp.sec); + // int64_t seconds = std::chrono::seconds(sec).count(); + // std::chrono::nanoseconds nanosec(friTimeStamp.nanosec); + // int64_t nanosecs = friTimeStamp.nanosec; + // int64_t microseconds_sec = std::chrono::microseconds(sec).count(); + + + // int64_t microseconds = microseconds + nanosecs/1000; + // if(INT64_MAX - microseconds_sec < nanosecs/1000) { + // throw std::runtime_error("signed overflow has occured"); + // } + // std::cout<<"secondes: "< Date: Fri, 2 Feb 2018 20:34:41 -0500 Subject: [PATCH 042/102] dump out the time stuff --- include/grl/kuka/KukaDriver.hpp | 4 +- include/grl/kuka/KukaFRIdriver.hpp | 36 +++-- include/grl/kuka/KukaJAVAdriver.hpp | 2 - include/grl/kuka/KukaToFlatbuffer.hpp | 2 +- include/grl/sensor/FusionTrackLogAndTrack.hpp | 4 +- .../grl/sensor/FusionTrackToFlatbuffer.hpp | 1 - include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp | 2 +- src/lua/robone.lua | 9 +- .../v_repExtAtracsysFusionTrack.cpp | 18 ++- .../v_repExtKukaLBRiiwa.cpp | 10 +- test/readFRIFBTest.cpp | 147 +++++++++++------- 11 files changed, 139 insertions(+), 96 deletions(-) diff --git a/include/grl/kuka/KukaDriver.hpp b/include/grl/kuka/KukaDriver.hpp index 5d3f094..f68ed23 100644 --- a/include/grl/kuka/KukaDriver.hpp +++ b/include/grl/kuka/KukaDriver.hpp @@ -97,8 +97,6 @@ namespace grl { namespace robot { namespace arm { /// @todo create a function that calls simGetObjectHandle and throws an exception when it fails /// @warning getting the ik group is optional, so it does not throw an exception void construct(Params params ) { - std::cout<< "Start KukaDriver->construct(), initialize params for both FRI and JAVA driver..." << std::endl; - params_ = params; // keep driver threads from exiting immediately after creation, because they have work to do! //device_driver_workP_.reset(new boost::asio::io_service::work(device_driver_io_service)); @@ -141,7 +139,6 @@ namespace grl { namespace robot { namespace arm { throw; } } - std::cout<< "End KukaDriver->construct()..." << std::endl; } const Params & getParams(){ @@ -264,6 +261,7 @@ namespace grl { namespace robot { namespace arm { } bool is_recording() { if(FRIdriverP_.get() != nullptr) { + // std::cout<< "In KukaDriver is_recording(): " << FRIdriverP_->is_recording() <is_recording(); } return false; diff --git a/include/grl/kuka/KukaFRIdriver.hpp b/include/grl/kuka/KukaFRIdriver.hpp index 2956f85..f83e626 100644 --- a/include/grl/kuka/KukaFRIdriver.hpp +++ b/include/grl/kuka/KukaFRIdriver.hpp @@ -164,7 +164,8 @@ class KukaFRIdriver : public std::enable_shared_from_this Kukatime) { return cartographer::common::Time( - std::chrono::duration_cast(Kukatime.time_since_epoch())); + // std::chrono::duration_cast(Kukatime.time_since_epoch())); + std::chrono::duration_cast(Kukatime.time_since_epoch())); } cartographer::common::Time FRITimeStampToCommonTime(const ::TimeStamp &friTimeStamp) @@ -421,7 +422,9 @@ class KukaFRIdriver : public std::enable_shared_from_thisinfo("Here is in save_recording/n "); if(filename.empty()) { /// TODO(ahundt) Saving the file twice in one second will overwrite!!!! filename = current_date_and_time_string() + "_Kukaiiwa.iiwa"; } #ifdef HAVE_spdlog - loggerP->info("Save Recording as: {}", filename); + loggerP->info("Save Recording as in Kuka: {}", filename); #else // HAVE_spdlog std::cout << "Save Recording as: " << filename << std::endl; #endif // HAVE_spdlog @@ -685,17 +687,21 @@ class KukaFRIdriver : public std::enable_shared_from_this(save_fbbP->GetBufferPointer()), save_fbbP->GetSize(), write_binary_stream); - // assert(success); - /// TODO(ahFusionTrackLogAndTrackundt) replace cout with proper spdlog and vrep banner notification - #ifdef HAVE_spdlog - lambdaLoggerP->info("filename: {}, verifier success: {}", filename, success); - #else // HAVE_spdlog - std::cout << "filename: " << filename << " verifier success: " << success << std::endl; - #endif // HAVE_spdlog + lambdaLoggerP->info("currentWorkingDir ...: {}", currentWorkingDir); + if(save_fbbP != nullptr && save_KUKAiiwaBufferP != nullptr) { + bool success = grl::FinishAndVerifyBuffer(*save_fbbP, *save_KUKAiiwaBufferP); + bool write_binary_stream = true; + success = success && flatbuffers::SaveFile(filename.c_str(), reinterpret_cast(save_fbbP->GetBufferPointer()), save_fbbP->GetSize(), write_binary_stream); + // assert(success); + /// TODO(ahFusionTrackLogAndTrackundt) replace cout with proper spdlog and vrep banner notification + #ifdef HAVE_spdlog + lambdaLoggerP->info("For KUKA filename: {}, verifier success:{}", filename,success); + #else // HAVE_spdlog + std::cout << "filename: " << filename << " verifier success: " << success << std::endl; + #endif // HAVE_spdlog + }else{ + lambdaLoggerP->error("pointer is nullptr..."); + } }; // save the recording to a file in a separate thread, memory will be freed up when file finishes saving @@ -746,7 +752,7 @@ void saveToDisk() const std::size_t MegaByte = 1024*1024; // If we write too large a flatbuffer const std::size_t single_buffer_limit_bytes = 1024*MegaByte; - const std::size_t single_buffer_limit_states = 1000; + const std::size_t single_buffer_limit_states = 1350; // run the primary update loop in a separate thread bool saveFileNow = false; diff --git a/include/grl/kuka/KukaJAVAdriver.hpp b/include/grl/kuka/KukaJAVAdriver.hpp index e107dba..5682ca2 100644 --- a/include/grl/kuka/KukaJAVAdriver.hpp +++ b/include/grl/kuka/KukaJAVAdriver.hpp @@ -159,7 +159,6 @@ class KukaJAVAdriver : public std::enable_shared_from_this { /// @warning getting the ik group is optional, so it does not throw an exception void construct(Params params) { try { logger_ = spdlog::stdout_logger_mt("console"); } catch (spdlog::spdlog_ex ex) { logger_ = spdlog::get("console"); } - std::cout<< "Start KukaJAVAdriver->construct()..." << std::endl; params_ = params; try { @@ -201,7 +200,6 @@ class KukaJAVAdriver : public std::enable_shared_from_this { /// set arm to StartArm mode on initalization /// set(grl::flatbuffer::ArmState::StartArm); set(grl::flatbuffer::ArmState::MoveArmJointServo); - std::cout<< "End KukaJAVAdriver->construct()..." << std::endl; } catch( boost::exception &e) { e << errmsg_info("KukaLBRiiwaRosPlugin: Unable to connect to UDP Socket from {}{}{}" + std::get (params_) + " to " + diff --git a/include/grl/kuka/KukaToFlatbuffer.hpp b/include/grl/kuka/KukaToFlatbuffer.hpp index 9a5df8d..d75615f 100644 --- a/include/grl/kuka/KukaToFlatbuffer.hpp +++ b/include/grl/kuka/KukaToFlatbuffer.hpp @@ -349,7 +349,7 @@ flatbuffers::Offset toFlatBuffer( grl::flatbuffer::CreateShutdownArm(fbb).Union()); } case grl::flatbuffer::ArmState::NONE: { - std::cout << "Waiting for interation mode... (currently NONE)" << std::endl; + //std::cout << "Waiting for interation mode... (currently NONE)" << std::endl; break; } default: diff --git a/include/grl/sensor/FusionTrackLogAndTrack.hpp b/include/grl/sensor/FusionTrackLogAndTrack.hpp index bd2368d..bfcc541 100644 --- a/include/grl/sensor/FusionTrackLogAndTrack.hpp +++ b/include/grl/sensor/FusionTrackLogAndTrack.hpp @@ -393,7 +393,7 @@ class FusionTrackLogAndTrack : public std::enable_shared_from_this(save_fbbP->GetBufferPointer()), save_fbbP->GetSize(), write_binary_stream); /// TODO(ahFusionTrackLogAndTrackundt) replace cout with proper spdlog and vrep banner notification #ifdef HAVE_spdlog - lambdaLoggerP->info("filename: {}", filename, " verifier success: {}", success); + lambdaLoggerP->info("For FT filename: {}, verifier success:{}", filename,success); #else // HAVE_spdlog std::cout << "filename: " << filename << " verifier success: " << success << std::endl; #endif // HAVE_spdlog @@ -440,7 +440,7 @@ class FusionTrackLogAndTrack : public std::enable_shared_from_this deviceSerialNumbers; try { diff --git a/include/grl/sensor/FusionTrackToFlatbuffer.hpp b/include/grl/sensor/FusionTrackToFlatbuffer.hpp index bf144b0..937a0c8 100644 --- a/include/grl/sensor/FusionTrackToFlatbuffer.hpp +++ b/include/grl/sensor/FusionTrackToFlatbuffer.hpp @@ -143,7 +143,6 @@ toFlatBuffer(flatbuffers::FlatBufferBuilder &fbb, _fbb.Finish(_ftkGeometry); auto verifier = flatbuffers::Verifier(_fbb.GetBufferPointer(), _fbb.GetSize()); bool success = verifier.VerifyBuffer(); - std::cout <<" verifier success for ftkGeometry: " << success << std::endl; /////////////////////////////////////////////////////////////////////////// return grl::flatbuffer::CreateftkGeometry( diff --git a/include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp b/include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp index 813d498..7b78c95 100644 --- a/include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp +++ b/include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp @@ -152,7 +152,6 @@ class KukaVrepPlugin : public std::enable_shared_from_this { /// construct() function completes initialization of the plugin /// @todo move this into the actual constructor, but need to correctly handle or attach vrep shared libraries for unit tests. void construct(Params params){ - std::cout<< "Start KukaVrepPlugin->construct()..." << std::endl; params_ = params; // keep driver threads from exiting immediately after creation, because they have work to do! device_driver_workP_.reset(new boost::asio::io_service::work(device_driver_io_service)); @@ -211,6 +210,7 @@ class KukaVrepPlugin : public std::enable_shared_from_this { bool is_recording() { if(kukaDriverP_.get() != nullptr) { + // std::cout<<"is_recording: " <is_recording() <is_recording(); } return false; diff --git a/src/lua/robone.lua b/src/lua/robone.lua index 071ab4e..47db797 100644 --- a/src/lua/robone.lua +++ b/src/lua/robone.lua @@ -525,17 +525,18 @@ robone.recordDataScript=function() -- Check if the required plugin is there: if (grl.isModuleLoaded('KukaLBRiiwa')) then - simExtKukaLBRiiwaRecordWhileSimulationIsRunning(true) + --simExtKukaLBRiiwaRecordWhileSimulationIsRunning(true) else simDisplayDialog('Warning','KukaLBRiiwa plugin was not found so data will not be logged.',sim_dlgstyle_ok,true,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1}) end - if (not grl.isModuleLoaded('AtracsysFusionTrack')) then - simExtAtracsysFusionTrackRecordWhileSimulationIsRunning(true) + if (grl.isModuleLoaded('AtracsysFusionTrack')) then + --simExtAtracsysFusionTrackRecordWhileSimulationIsRunning(true) else simDisplayDialog('Warning','Atracsys plugin was not found so data will not be logged.',sim_dlgstyle_ok,true,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1}) + end end -------------------------------------------------------------------------- @@ -568,4 +569,4 @@ robone.getExternalJointTorque=function(dofs, jointHandles, externalTorqueHandle) return externalJointTorque end -return robone \ No newline at end of file +return robone diff --git a/src/v_repExtAtracsysFusionTrack/v_repExtAtracsysFusionTrack.cpp b/src/v_repExtAtracsysFusionTrack/v_repExtAtracsysFusionTrack.cpp index 7e28210..128026e 100755 --- a/src/v_repExtAtracsysFusionTrack/v_repExtAtracsysFusionTrack.cpp +++ b/src/v_repExtAtracsysFusionTrack/v_repExtAtracsysFusionTrack.cpp @@ -432,12 +432,13 @@ void LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_RECORD_WHILE_SIMULATION_IS_RUNNING(SLuaCa { std::vector *inData = D.getInDataPtr(); recordWhileSimulationIsRunningG = inData->at(0).boolData[0]; + // std::cout << "Start recording while simulation is running for Tracker..." << recordWhileSimulationIsRunningG << std::endl; if (fusionTrackPG && recordWhileSimulationIsRunningG) { std::string log_message = std::string("simExtAtracsysFusionTrackRecordWhileSimulationIsRunning: ") + boost::lexical_cast(recordWhileSimulationIsRunningG); simAddStatusbarMessage(log_message.c_str()); loggerPG->info(log_message); - fusionTrackPG->start_recording(); + success = fusionTrackPG->start_recording(); } D.pushOutData(CLuaFunctionDataItem(success)); @@ -718,9 +719,9 @@ VREP_DLLEXPORT void *v_repMessage(int message, int *auxiliaryData, void *customD // LoggerPG->error( initerr); // } - if(fusionTrackPG && recordWhileSimulationIsRunningG) { - fusionTrackPG->start_recording(); - } + // if(fusionTrackPG && recordWhileSimulationIsRunningG) { + // fusionTrackPG->start_recording(); + // } } if (message == sim_message_eventcallback_simulationended) @@ -730,8 +731,15 @@ VREP_DLLEXPORT void *v_repMessage(int message, int *auxiliaryData, void *customD // SIMULATION STOPS RUNNING HERE // close out as necessary //////////////////// + loggerPG->error("Ending Fusion Tracker plugin connection to Kuka iiwa\n" ); + loggerPG->info(" Atracsys recordWhileSimulationIsRunningG: {}", recordWhileSimulationIsRunningG); + loggerPG->info("fusionTrackPG->is_recording(): {}",fusionTrackPG->is_recording()); + if(fusionTrackPG && recordWhileSimulationIsRunningG && fusionTrackPG->is_recording()) { - fusionTrackPG->save_recording(); + bool success = fusionTrackPG->save_recording(); + if(success) { + loggerPG->info("Vrep quits successfully in FT..." ); + } fusionTrackPG->stop_recording(); } } diff --git a/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp b/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp index 8c0e4e1..e07a152 100755 --- a/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp +++ b/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp @@ -382,7 +382,6 @@ VREP_DLLEXPORT void v_repEnd() // PUT OBJECT RESET CODE HERE // close out as necessary //////////////////// - kukaVrepPluginPG.reset(); unloadVrepLibrary(vrepLib); // release the library @@ -515,8 +514,15 @@ VREP_DLLEXPORT void* v_repMessage(int message,int* auxiliaryData,void* customDat // close out as necessary //////////////////// loggerPG->error("Ending KUKA LBR iiwa plugin connection to Kuka iiwa\n" ); + loggerPG->info("recordWhileSimulationIsRunningG: {}", recordWhileSimulationIsRunningG); + loggerPG->info("kukaVrepPluginPG->is_recording(): {}", kukaVrepPluginPG->is_recording()); + if(kukaVrepPluginPG && recordWhileSimulationIsRunningG && kukaVrepPluginPG->is_recording()) { - kukaVrepPluginPG->save_recording(); + bool success = kukaVrepPluginPG->save_recording(); + if(success) { + loggerPG->info("Vrep quits successfully..." ); + } + kukaVrepPluginPG->stop_recording(); } kukaVrepPluginPG->clear_recording(); diff --git a/test/readFRIFBTest.cpp b/test/readFRIFBTest.cpp index 3648e22..7b7c97a 100644 --- a/test/readFRIFBTest.cpp +++ b/test/readFRIFBTest.cpp @@ -8,6 +8,7 @@ #include "grl/flatbuffer/FusionTrack_generated.h" #include "grl/flatbuffer/Time_generated.h" #include "grl/flatbuffer/LogKUKAiiwaFusionTrack_generated.h" +#include "grl/time.hpp" @@ -19,21 +20,11 @@ void writetoCSVforFusionTrackKukaiiwa(std::string KUKAbinaryfilename, std::string FusionTrackbinaryfilename, std::string csvfilename); int main(int argc, char* argv[]) { - std::string kukaBinaryfile = "/home/chunting/src/V-REP_PRO_EDU_V3_4_0_Linux/2018_01_31_16_11_27_Kukaiiwa.iiwa"; - std::string fusiontrackBinaryfile = "/home/chunting/src/V-REP_PRO_EDU_V3_4_0_Linux/2018_01_31_16_10_58_FusionTrack.flik"; - std::string KukaFusionTrackCSVFile = "/home/chunting/src/V-REP_PRO_EDU_V3_4_0_Linux/TimeEventCombined.csv"; - writetoCSVforFusionTrackKukaiiwa(kukaBinaryfile, fusiontrackBinaryfile, KukaFusionTrackCSVFile); - // fbs_tk::Root KUKAiiwaStatesRoot = fbs_tk::open_root(kukaBinaryfile); - - // auto states = KUKAiiwaStatesRoot->states(); - // std::size_t state_size = states->size(); - // std::cout<< "++++++Kuka_state_size: "< KUKAiiwaStatesRoot = fbs_tk::open_root(KUKAbinaryfilename); auto KUKA_states = KUKAiiwaStatesRoot->states(); std::size_t kuka_state_size = KUKA_states->size(); - std::cout<< "------Kuka_state_size: "< kuka_deviceTime = grl:: getTimeStamp_Kuka(KUKAiiwaStatesRoot, "device_time"); std::vector kuka_local_request_time = grl::getTimeStamp_Kuka(KUKAiiwaStatesRoot, "local_request_time"); std::vector kuka_local_receive_time = grl::getTimeStamp_Kuka(KUKAiiwaStatesRoot, "local_receive_time"); - fbs_tk::Root FusionTrackStatesRoot = fbs_tk::open_root(FusionTrackbinaryfilename); + fbs_tk::Root FusionTrackStatesRoot = fbs_tk::open_root(FusionTrackbinaryfilename); auto FT_states = FusionTrackStatesRoot->states(); std::size_t FT_state_size = FT_states->size(); - std::cout<< "------FusionTrack State Size: "< FT_deviceTime = grl:: getTimeStamp_FusionTrack(FusionTrackStatesRoot, "device_time"); std::vector FT_local_request_time = grl::getTimeStamp_FusionTrack(FusionTrackStatesRoot, "local_request_time"); std::vector FT_local_receive_time = grl::getTimeStamp_FusionTrack(FusionTrackStatesRoot, "local_receive_time"); - std::vector local_request_time; + + std::vector local_request_time_offset; std::vector local_receive_time_offset_FT; std::vector local_receive_time_offset_kuka; std::vector device_time_offset_kuka; std::vector device_time_offset_FT; + std::vector device_type; int64_t initial_local_request_time = std::min(kuka_local_request_time[0], FT_local_request_time[0]); int64_t initial_local_receive_time_kuka = kuka_local_receive_time[0]; int64_t initial_local_receive_time_FT = FT_local_receive_time[0]; @@ -71,38 +64,44 @@ void writetoCSVforFusionTrackKukaiiwa(std::string KUKAbinaryfilename, std::strin auto itkuka_local_request_timeEnd = kuka_local_request_time.end(); auto itFT_local_request_timeEnd = FT_local_request_time.end(); - local_request_time.reserve(kuka_state_size+FT_state_size); - local_receive_time_offset_FT.reserve(kuka_state_size+FT_state_size); - local_receive_time_offset_kuka.reserve(kuka_state_size+FT_state_size); - device_time_offset_kuka.reserve(kuka_state_size+FT_state_size); - device_time_offset_FT.reserve(kuka_state_size+FT_state_size); - + // local_request_time_offset.reserve(kuka_state_size+FT_state_size); + // local_receive_time_offset_FT.reserve(kuka_state_size+FT_state_size); + // local_receive_time_offset_kuka.reserve(kuka_state_size+FT_state_size); + // device_time_offset_kuka.reserve(kuka_state_size+FT_state_size); + // device_time_offset_FT.reserve(kuka_state_size+FT_state_size); + int kuka_index = 0; - int FT_index = 0; + int FT_index = 0; + bool flag = false; + while(*itkuka_local_request_time > *itFT_local_request_time){ + itFT_local_request_time++; + } - while ( itkuka_local_request_time != itkuka_local_request_timeEnd && + while ( itkuka_local_request_time != itkuka_local_request_timeEnd && itFT_local_request_time != itFT_local_request_timeEnd ) { if ( *itkuka_local_request_time < *itFT_local_request_time ){ - local_request_time.push_back( *itkuka_local_request_time - initial_local_request_time); + local_request_time_offset.push_back( *itkuka_local_request_time - initial_local_request_time); local_receive_time_offset_FT.push_back(FT_local_receive_time[FT_index] - initial_local_receive_time_FT); local_receive_time_offset_kuka.push_back(kuka_local_receive_time[kuka_index] - initial_local_receive_time_kuka); device_time_offset_kuka.push_back(kuka_deviceTime[kuka_index]-initial_device_time_kuka); device_time_offset_FT.push_back(FT_deviceTime[FT_index]-initial_device_time_FT); kuka_index++; itkuka_local_request_time++; - - - } else if( *itkuka_local_request_time > *itFT_local_request_time ) { - local_request_time.push_back( *itFT_local_request_time - initial_local_request_time ); + device_type.push_back(0); + + + } else if( *itkuka_local_request_time > *itFT_local_request_time) { + local_request_time_offset.push_back( *itFT_local_request_time - initial_local_request_time ); local_receive_time_offset_FT.push_back(FT_local_receive_time[FT_index] - initial_local_receive_time_FT); local_receive_time_offset_kuka.push_back(kuka_local_receive_time[kuka_index] - initial_local_receive_time_kuka); device_time_offset_kuka.push_back(kuka_deviceTime[kuka_index]-initial_device_time_kuka); device_time_offset_FT.push_back(FT_deviceTime[FT_index]-initial_device_time_FT); FT_index++; itFT_local_request_time++; + device_type.push_back(1); } else { - local_request_time.push_back( *itFT_local_request_time - initial_local_request_time ); + local_request_time_offset.push_back( *itFT_local_request_time - initial_local_request_time ); local_receive_time_offset_FT.push_back(FT_local_receive_time[FT_index] - initial_local_receive_time_FT); local_receive_time_offset_kuka.push_back(kuka_local_receive_time[kuka_index] - initial_local_receive_time_kuka); device_time_offset_kuka.push_back(kuka_deviceTime[kuka_index]-initial_device_time_kuka); @@ -111,43 +110,71 @@ void writetoCSVforFusionTrackKukaiiwa(std::string KUKAbinaryfilename, std::strin kuka_index++; itkuka_local_request_time++; itFT_local_request_time++; + device_type.push_back(2); } } - // copy rest of kuka_local_request_time array - while ( itkuka_local_request_time != itkuka_local_request_timeEnd ){ - FT_index = FT_state_size-1; - local_request_time.push_back( *itkuka_local_request_time - initial_local_request_time); - local_receive_time_offset_FT.push_back(FT_local_receive_time[FT_index] - initial_local_receive_time_FT); - local_receive_time_offset_kuka.push_back(kuka_local_receive_time[kuka_index] - initial_local_receive_time_kuka); - device_time_offset_kuka.push_back(kuka_deviceTime[kuka_index]-initial_device_time_kuka); - device_time_offset_FT.push_back(FT_deviceTime[FT_index]-initial_device_time_FT); - kuka_index++; - itkuka_local_request_time++; - } - - - // copy rest of FT_local_request_time array - while ( itFT_local_request_time != itFT_local_request_timeEnd ) { - kuka_index = kuka_state_size-1; - local_request_time.push_back( *itFT_local_request_time - initial_local_request_time ); - local_receive_time_offset_FT.push_back(FT_local_receive_time[FT_index] - initial_local_receive_time_FT); - local_receive_time_offset_kuka.push_back(kuka_local_receive_time[kuka_index] - initial_local_receive_time_kuka); - device_time_offset_kuka.push_back(kuka_deviceTime[kuka_index]-initial_device_time_kuka); - device_time_offset_FT.push_back(FT_deviceTime[FT_index]-initial_device_time_FT); - FT_index++; - itFT_local_request_time++; - } + + // // copy rest of kuka_local_request_time array + // while ( itkuka_local_request_time != itkuka_local_request_timeEnd ){ + // FT_index = FT_state_size-2; + // local_request_time_offset.push_back( *itkuka_local_request_time - initial_local_request_time); + // local_receive_time_offset_FT.push_back(FT_local_receive_time[FT_index] - initial_local_receive_time_FT); + // local_receive_time_offset_kuka.push_back(kuka_local_receive_time[kuka_index] - initial_local_receive_time_kuka); + // device_time_offset_kuka.push_back(kuka_deviceTime[kuka_index]-initial_device_time_kuka); + // device_time_offset_FT.push_back(FT_deviceTime[FT_index]-initial_device_time_FT); + // kuka_index++; + // itkuka_local_request_time++; + // device_type.push_back(0); + + // } + + + // // copy rest of FT_local_request_time array + // while ( itFT_local_request_time != itFT_local_request_timeEnd ) { + // kuka_index = kuka_state_size-2; + // local_request_time_offset.push_back( *itFT_local_request_time - initial_local_request_time ); + // local_receive_time_offset_FT.push_back(FT_local_receive_time[FT_index] - initial_local_receive_time_FT); + // local_receive_time_offset_kuka.push_back(kuka_local_receive_time[kuka_index] - initial_local_receive_time_kuka); + // device_time_offset_kuka.push_back(kuka_deviceTime[kuka_index]-initial_device_time_kuka); + // device_time_offset_FT.push_back(FT_deviceTime[FT_index]-initial_device_time_FT); + // FT_index++; + // itFT_local_request_time++; + // device_type.push_back(1); + // // std::cout<<"kuka_local_receive_time: " << kuka_index <<" "< Date: Mon, 5 Feb 2018 22:01:27 -0500 Subject: [PATCH 043/102] Clean some codes based on the review --- example/kukaiiwaExample.cpp | 231 -------------- include/grl/flatbuffer/flatbuffer.hpp | 20 +- .../readDatafromBinary.hpp} | 128 ++++---- include/grl/kuka/Kuka.hpp | 14 +- include/grl/kuka/KukaFRIClientDataDriver.hpp | 281 +++++++++--------- .../sensor/readFusionTrackFromFlatbuffer.hpp | 103 ------- test/CMakeLists.txt | 4 +- ...=> FlatbuffersExampleWithKukaIiwaTest.cpp} | 6 +- test/readFRIFBTest.cpp | 124 +++++--- 9 files changed, 293 insertions(+), 618 deletions(-) delete mode 100644 example/kukaiiwaExample.cpp rename include/grl/{kuka/readKukaiiwaFromFlatbuffer.hpp => flatbuffer/readDatafromBinary.hpp} (71%) delete mode 100644 include/grl/sensor/readFusionTrackFromFlatbuffer.hpp rename test/{kukaiiwaTest.cpp => FlatbuffersExampleWithKukaIiwaTest.cpp} (98%) diff --git a/example/kukaiiwaExample.cpp b/example/kukaiiwaExample.cpp deleted file mode 100644 index 291a7e5..0000000 --- a/example/kukaiiwaExample.cpp +++ /dev/null @@ -1,231 +0,0 @@ -// system includes -#include -#include -#include -#include - - - -#include "grl/kuka/KukaToFlatbuffer.hpp" -#include "grl/kuka/KukaJAVAdriver.hpp" -#include "grl/kuka/Kuka.hpp" -#include "grl/kuka/KukaDriver.hpp" - -#include "flatbuffers/util.h" - - -// #include "grl/flatbuffer/flatbuffer.hpp" -/// Make reference to void map_repeatedDouble() in pb_frimessages_callback.c and pb_frimessages_callbacks.h -pb_callback_t makeupJointValues(std::vector &jointvalue) { - /// Make reference to void map_repeatedDouble() in pb_frimessages_callback.c and pb_frimessages_callbacks.h - pb_callback_t *values = new pb_callback_t; - int numDOF = 7; - values->funcs.encode = &encode_repeatedDouble; - values->funcs.decode = &decode_repeatedDouble; - tRepeatedDoubleArguments *arg = new tRepeatedDoubleArguments; - // map the local container data to the message data fields - arg->max_size = numDOF; - arg->size = 0; - arg->value = (double*) malloc(numDOF * sizeof(double)); - - for(int i=0; ivalue[i] = jointvalue[i]; - } - values->arg = arg; - return *values; -} - -int main(int argc, char **argv) -{ - - bool debug = true; - if (debug) std::cout << "starting KukaTest" << std::endl; - std::shared_ptr fbbP; - fbbP = std::make_shared(); - double duration = boost::chrono::high_resolution_clock::now().time_since_epoch().count(); - grl::robot::arm::KukaDriver::Params params = std::make_tuple( - "Robotiiwa" , // RobotName, - "KUKA_LBR_IIWA_14_R820" , // RobotModel (options are KUKA_LBR_IIWA_14_R820, KUKA_LBR_IIWA_7_R800) - "0.0.0.0" , // LocalUDPAddress - "30010" , // LocalUDPPort - "172.31.1.147" , // RemoteUDPAddress - "192.170.10.100" , // LocalHostKukaKoniUDPAddress, - "30200" , // LocalHostKukaKoniUDPPort, - "192.170.10.2" , // RemoteHostKukaKoniUDPAddress, - "30200" , // RemoteHostKukaKoniUDPPort - "JAVA" , // KukaCommandMode (options are FRI, JAVA) - "FRI" // KukaMonitorMode (options are FRI, JAVA) - ); - std::string RobotName("Robotiiwa" ); - std::string destination("where this message is going (URI)"); - std::string source("where this message came from (URI)"); - std::string basename = RobotName; //std::get<0>(params); - bool setArmConfiguration_ = true; // set the arm config first time - bool max_control_force_stop_ = false; - double nullspace_stiffness_ = 2.0; - double nullspace_damping_ = 0.5; - - grl::flatbuffer::ArmState armControlMode_(grl::flatbuffer::ArmState::StartArm); - grl::flatbuffer::KUKAiiwaInterface commandInterface_ = grl::flatbuffer::KUKAiiwaInterface::SmartServo;// KUKAiiwaInterface::SmartServo; - grl::flatbuffer::KUKAiiwaInterface monitorInterface_ = grl::flatbuffer::KUKAiiwaInterface::FRI; - ::ControlMode controlMode = ::ControlMode::ControlMode_POSITION_CONTROLMODE; - ::ClientCommandMode clientCommandMode = ::ClientCommandMode::ClientCommandMode_POSITION; - ::OverlayType overlayType = ::OverlayType::OverlayType_NO_OVERLAY; - ::ConnectionInfo connectionInfo{::FRISessionState::FRISessionState_IDLE, - ::FRIConnectionQuality::FRIConnectionQuality_POOR, - true, 4, false, 0}; - grl::robot::arm::KukaState armState; - std::shared_ptr friData(std::make_shared(7)); - cartographer::common::Time startTime; - - std::vector joint_stiffness_ = {0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7}; - std::vector joint_damping_ = {1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0}; - std::vector joint_AccelerationRel_(7,0.5); - std::vector joint_VelocityRel_(7,1.0); - bool updateMinimumTrajectoryExecutionTime = false; - double minimumTrajectoryExecutionTime = 4; - - std::vector absoluteGoalPos(7,0.2); - - std::size_t goal_position_command_time_duration = 4; - std::shared_ptr> highLevelDriverClassP; - std::shared_ptr kukaDriverP; - - kukaDriverP=std::make_shared(params); - kukaDriverP->set(grl::flatbuffer::ArmState::MoveArmJointServo); - kukaDriverP->set(goal_position_command_time_duration,grl::time_duration_command_tag()); - std::cout << "KUKA COMMAND MODE: " << std::get(params) << "\n"; - int64_t sequenceNumber = 0; - auto controlState = grl::toFlatBuffer(*fbbP, basename, sequenceNumber++, duration, armState, armControlMode_); - - //Cartesian Impedance Values - grl::flatbuffer::Vector3d cart_stiffness_trans_ = grl::flatbuffer::Vector3d(500,500,500); - grl::flatbuffer::EulerRotation cart_stifness_rot_ = grl::flatbuffer::EulerRotation(200,200,200,grl::flatbuffer::EulerOrder::xyz); - grl::flatbuffer::Vector3d cart_damping_trans_ = grl::flatbuffer::Vector3d(0.3,0.3,0.3); - grl::flatbuffer::EulerRotation cart_damping_rot_ = grl::flatbuffer::EulerRotation(0.3,0.3,0.3,grl::flatbuffer::EulerOrder::xyz); - grl::flatbuffer::EulerPose cart_stiffness_ = grl::flatbuffer::EulerPose(cart_stiffness_trans_, cart_stifness_rot_); - grl::flatbuffer::EulerPose cart_damping_ = grl::flatbuffer::EulerPose(cart_damping_trans_, cart_damping_rot_); - - grl::flatbuffer::EulerPose cart_max_path_deviation_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(1000,1000,100), grl::flatbuffer::EulerRotation(5.,5.,5., grl::flatbuffer::EulerOrder::xyz)); - grl::flatbuffer::EulerPose cart_max_ctrl_vel_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(1000,1000,1000), grl::flatbuffer::EulerRotation(6.3,6.3,6.3, grl::flatbuffer::EulerOrder::xyz)); - grl::flatbuffer::EulerPose cart_max_ctrl_force_ = grl::flatbuffer::EulerPose(grl::flatbuffer::Vector3d(200,200,200), grl::flatbuffer::EulerRotation(200.,200.,200., grl::flatbuffer::EulerOrder::xyz)); - - - - auto setCartesianImpedance = grl::toFlatBuffer(*fbbP, cart_stiffness_, cart_damping_, - nullspace_stiffness_, nullspace_damping_, cart_max_path_deviation_, cart_max_ctrl_vel_, cart_max_ctrl_force_, max_control_force_stop_); - auto setJointImpedance = grl::toFlatBuffer(*fbbP, joint_stiffness_, joint_damping_); - auto setSmartServo = grl::toFlatBuffer(*fbbP, joint_AccelerationRel_, joint_VelocityRel_, updateMinimumTrajectoryExecutionTime, minimumTrajectoryExecutionTime); - auto FRIConfig = grl::toFlatBuffer(*fbbP, overlayType, connectionInfo, false, 3501, false, 3502); - std::vector> tools; - std::vector> processData; - for(int i=0; i<7; i++) { - std::string linkname = "Link" + std::to_string(i); - std::string parent = i==0?"Base":("Link" + std::to_string(i-1)); - grl::flatbuffer::Pose pose = grl::flatbuffer::Pose(grl::flatbuffer::Vector3d(i,i,i), grl::flatbuffer::Quaternion(i,i,i,i)); - grl::flatbuffer::Inertia inertia = grl::flatbuffer::Inertia(1, pose, 1, 2, 3, 4, 5, 6); - flatbuffers::Offset linkObject = grl::toFlatBuffer(*fbbP, linkname, parent, pose, inertia); - tools.push_back(linkObject); - processData.push_back( - grl::toFlatBuffer(*fbbP, - "dataType"+ std::to_string(i), - "defaultValue"+ std::to_string(i), - "displayName"+ std::to_string(i), - "id"+ std::to_string(i), - "min"+ std::to_string(i), - "max"+ std::to_string(i), - "unit"+ std::to_string(i), - "value"+ std::to_string(i))); - - } - std::string currentMotionCenter = "currentMotionCenter"; - auto kukaiiwaArmConfiguration = grl::toFlatBuffer( - *fbbP, - RobotName, - commandInterface_, - monitorInterface_, - clientCommandMode, - overlayType, - controlMode, - setCartesianImpedance, - setJointImpedance, - setSmartServo, - FRIConfig, - tools, - processData, - "currentMotionCenter", - true); - bool setArmControlState = true; // only actually change the arm state when this is true. - ::MessageHeader messageHeader{1,2,3}; - ::RobotInfo robotInfo{true, 7, true, ::SafetyState::SafetyState_NORMAL_OPERATION}; - ::JointValues strjointValues; - std::vector jointValues(7, 0.1); - - - - strjointValues.value = makeupJointValues(jointValues); - ::MessageMonitorData messageMonitorData{ - true, strjointValues, - true, strjointValues, - true, strjointValues, - true, strjointValues, - true, strjointValues, - true, ::TimeStamp{1,2} - }; - ::MessageIpoData ipoData{ - true, strjointValues, - true, clientCommandMode, - true, overlayType, - true, 4.0 - }; - ::Transformation transformation{ "Transformation", 12, {1,2,3,4,5,6,7,8,9,10,11,12}, true, ::TimeStamp{3,4}}; - /// Transformation requestedTransformations[5] = { transformation, transformation, transformation, transformation, transformation }; - ::MessageEndOf endOfMessageData{true, 32, true, ::Checksum{true, 32}}; - ::FRIMonitoringMessage friMonitoringMessage {messageHeader, - true, robotInfo, - true, messageMonitorData, - true, connectionInfo, - true, ipoData, - 5, - { transformation, transformation, transformation, transformation, transformation }, - true, endOfMessageData}; - grl::TimeEvent time_event_stamp; - auto friMessageLog = grl::toFlatBuffer( - *fbbP, - ::FRISessionState::FRISessionState_IDLE, - ::FRIConnectionQuality::FRIConnectionQuality_POOR, - controlMode, - friMonitoringMessage, - time_event_stamp); - auto kukaiiwaState = grl::toFlatBuffer( - *fbbP, - RobotName, - destination, - source, - duration, - true, controlState, - true, kukaiiwaArmConfiguration, - false, 0, - false, 0, - friMessageLog); - std::vector> kukaiiwaStateVec; - kukaiiwaStateVec.push_back(kukaiiwaState); - auto states = grl::toFlatBuffer(*fbbP, kukaiiwaStateVec); - grl::flatbuffer::FinishKUKAiiwaStatesBuffer(*fbbP, states); - flatbuffers::Verifier verifier(fbbP->GetBufferPointer(),fbbP->GetSize()); - std::cout<< "VerifyKUKAiiwaStatesBuffer: " << grl::flatbuffer::VerifyKUKAiiwaStatesBuffer(verifier) << std::endl; - - - std::string binary_file_path = "Kuka_test_binary.flik"; - std::string json_file_path = "kuka_test_text.json"; - - std::string fbs_filename("KUKAiiwa.fbs"); - grl::SaveFlatBufferFile( - fbbP->GetBufferPointer(), - fbbP->GetSize(), - binary_file_path, - fbs_filename, - json_file_path); - std::cout << "End of the program" << std::endl; - return success; -} // End of main function diff --git a/include/grl/flatbuffer/flatbuffer.hpp b/include/grl/flatbuffer/flatbuffer.hpp index 34b47a5..7d4eb9d 100644 --- a/include/grl/flatbuffer/flatbuffer.hpp +++ b/include/grl/flatbuffer/flatbuffer.hpp @@ -1,8 +1,8 @@ #ifndef GRL_FLATBUFFER_HPP #define GRL_FLATBUFFER_HPP -#include /* defines FILENAME_MAX */ #include +// source: http://www.codebind.com/cpp-tutorial/c-get-current-directory-linuxwindows/ // #define WINDOWS /* uncomment this line to use it for windows.*/ #ifdef WINDOWS #include @@ -30,7 +30,6 @@ namespace grl { bool LoadJSONFlatbuffer(flatbuffers::Parser& parser, std::string schemaPath, std::string jsonPath, std::string include_directory, bool binary = false) { // source: https://github.com/google/flatbuffers/blob/master/samples/sample_text.cpp - // load FlatBuffer schema (.fbs) and JSON from disk std::string schemafile; std::string jsonfile; @@ -68,7 +67,6 @@ namespace grl { std::vector includePaths = std::vector(), bool read_binary_schema=false) { - bool loadfbsfile_ok = true; std::string schemafile; if(includePaths.empty()) @@ -89,13 +87,13 @@ namespace grl { //std::cout << "a full path wasn't passed, so check all the include paths" << std::endl; for(auto includePath : includePaths) { - std::string fbs_trypath = flatbuffers::ConCatPathFileName(includePath, fbs_filename); - if(flatbuffers::FileExists(fbs_trypath.c_str())) - { - fbs_fullpath = fbs_trypath; - // we found it! no need to keep looping - break; - } + std::string fbs_trypath = flatbuffers::ConCatPathFileName(includePath, fbs_filename); + if(flatbuffers::FileExists(fbs_trypath.c_str())) + { + fbs_fullpath = fbs_trypath; + // we found it! no need to keep looping + break; + } } } @@ -111,7 +109,7 @@ namespace grl { // create a list of char* pointers so we can call Parse std::vector include_directories; for(int i = 0; i < includePaths.size(); i++){ - include_directories.push_back(includePaths[i].c_str()); + include_directories.push_back(includePaths[i].c_str()); } include_directories.push_back(nullptr); return parser.Parse(schemafile.c_str(), &include_directories[0]); diff --git a/include/grl/kuka/readKukaiiwaFromFlatbuffer.hpp b/include/grl/flatbuffer/readDatafromBinary.hpp similarity index 71% rename from include/grl/kuka/readKukaiiwaFromFlatbuffer.hpp rename to include/grl/flatbuffer/readDatafromBinary.hpp index 47edb9f..3a2dd70 100644 --- a/include/grl/kuka/readKukaiiwaFromFlatbuffer.hpp +++ b/include/grl/flatbuffer/readDatafromBinary.hpp @@ -1,5 +1,5 @@ -#ifndef GRL_READ_KUKAIIWA_FROM_FLATBUFFER -#define GRL_READ_KUKAIIWA_FROM_FLATBUFFER +#ifndef GRL_READ_DATA_FROM_BINARY +#define GRL_READ_DATA_FROM_BINARY #define FLATBUFFERS_DEBUG_VERIFICATION_FAILURE @@ -10,6 +10,8 @@ #include "grl/flatbuffer/LinkObject_generated.h" #include "grl/flatbuffer/Time_generated.h" #include "grl/flatbuffer/flatbuffer.hpp" +#include "grl/flatbuffer/FusionTrack_generated.h" +#include "grl/flatbuffer/LogKUKAiiwaFusionTrack_generated.h" #include #include // For printing and file access. @@ -23,26 +25,11 @@ #include #include namespace grl { - const grl::flatbuffer::KUKAiiwaStates* getKUKAiiwaStatesFromFlatbuffer(const std::string filename){ - - //std::cout << "CurrentWorkingDir: " << curWorkingDir << std::endl; - if(filename.empty()) return nullptr; - FILE* file = std::fopen(filename.c_str(), "rb"); - if(!file) { - std::perror("File opening failed"); - return nullptr; - } - std::fseek(file, 0L, SEEK_END); // seek to end - std::size_t length = std::ftell(file); - std::fseek(file, 0L, SEEK_SET); // seek to start - std::vector buffer(length); - std::fread(buffer.data(), sizeof(uint8_t), buffer.size(), file); - std::fclose(file); - return grl::flatbuffer::GetKUKAiiwaStates(&buffer[0]); - } - - std::vector getTimeStamp_Kuka(const fbs_tk::Root &kukaStatesP, std::string time_type){ + struct kuka_tag {}; + struct fusiontracker_tag {}; + enum TimeType { device_time = 0, local_request_time = 1, local_receive_time = 2 }; + std::vector getTimeStamp(const fbs_tk::Root &kukaStatesP, kuka_tag, TimeType time_type){ auto states = kukaStatesP->states(); std::vector timeStamp; std::size_t state_size = states->size(); @@ -50,18 +37,57 @@ namespace grl { auto KUKAiiwaState = states->Get(i); auto FRIMessage = KUKAiiwaState->FRIMessage(); auto timeEvent = FRIMessage->timeStamp(); - - if(time_type =="device_time"){ - timeStamp.push_back(timeEvent->device_time()); - } else if (time_type == "local_request_time") { - timeStamp.push_back(timeEvent->local_request_time()); - } else if (time_type == "local_receive_time"){ - timeStamp.push_back(timeEvent->local_receive_time()); + switch(time_type) { + case device_time:{ + timeStamp.push_back(timeEvent->device_time()); + break; + } + case local_request_time:{ + timeStamp.push_back(timeEvent->local_request_time()); + break; + } + case local_receive_time:{ + timeStamp.push_back(timeEvent->local_receive_time()); + break; + } + default:{ + std::perror("No time type!"); + } } } return timeStamp; } + std::vector getTimeStamp(const fbs_tk::Root &logKUKAiiwaFusionTrackP, fusiontracker_tag, TimeType time_type){ + + auto states = logKUKAiiwaFusionTrackP->states(); + std::vector timeStamp; + + std::size_t state_size = states->size(); + //Eigen::VectoXd time_stamp(state_size); + for(int i = 0; iGet(i); + auto timeEvent = kukaiiwaFusionTrackMessage->timeEvent(); + switch(time_type) { + case device_time:{ + timeStamp.push_back(timeEvent->device_time()); + break; + } + case local_request_time:{ + timeStamp.push_back(timeEvent->local_request_time()); + break; + } + case local_receive_time:{ + timeStamp.push_back(timeEvent->local_receive_time()); + break; + } + default:{ + std::perror("No time type!"); + } + } + } + return timeStamp; + } /// Return the joint_index th joint angle @@ -145,51 +171,5 @@ namespace grl { // } // fs.close(); // } - /* - * A class to create and write data in a csv file. - */ -class CSVWriter -{ - std::string fileName; - std::string delimeter; - int linesCount; - -public: - CSVWriter(std::string filename, std::string delm = ",") : - fileName(filename), delimeter(delm), linesCount(0) - {} - /* - * Member function to store a range as comma seperated value - */ - template - void addDatainRow(T first, T last); -}; - -/* - * This Function accepts a range and appends all the elements in the range - * to the last row, seperated by delimeter (Default is comma) - */ -template -void CSVWriter::addDatainRow(T first, T last) -{ - std::fstream file; - // Open the file in truncate mode if first line else in Append Mode - file.open(fileName, std::ios::out | (linesCount ? std::ios::app : std::ios::trunc)); - - // Iterate over the range and add each lement to file seperated by delimeter. - for (; first != last; ) - { - file << *first; - if (++first != last) - file << delimeter; - } - file << "\n"; - linesCount++; - - // Close the file - file.close(); -} - - } #endif \ No newline at end of file diff --git a/include/grl/kuka/Kuka.hpp b/include/grl/kuka/Kuka.hpp index 61e4f3c..0585df8 100644 --- a/include/grl/kuka/Kuka.hpp +++ b/include/grl/kuka/Kuka.hpp @@ -87,16 +87,6 @@ struct KukaState { flatbuffer::EOperationMode operationMode; // KUKA::FRI::EOperationMode flatbuffer::EDriveState driveState; // KUKA::FRI::EDriveState - // The point in time associated with the current measured - // state of the arm (position, torque, etc.). When commanding - // the arm use commanded_goal_timestamp. - /// TODO(Chunting) remove this and make all code that uses it instead set the time_event_stamp - /// most likely the device_time, but double check the correctness of that - - /// This parameter is replaced by grl::TimeEvent time_event_stamp, - /// then how to integrate the TimeStamp in FRIMessage.pb.h? - /// time_point_type timestamp; - ///////////////////////////////////////////////////////////////////////////////////////////// // members below here define the driver state and are not part of the FRI arm // message format @@ -125,6 +115,10 @@ struct KukaState { /// and the local computer time after receiving data /// This makes it possible to more accurately synchronize /// time between multiple hardware devices. + + // The point in time associated with the current measured + // state of the arm (position, torque, etc.). When commanding + // the arm use commanded_goal_timestamp. grl::TimeEvent time_event_stamp; void clear() { diff --git a/include/grl/kuka/KukaFRIClientDataDriver.hpp b/include/grl/kuka/KukaFRIClientDataDriver.hpp index 8ab60f0..739ab81 100644 --- a/include/grl/kuka/KukaFRIClientDataDriver.hpp +++ b/include/grl/kuka/KukaFRIClientDataDriver.hpp @@ -365,16 +365,13 @@ void update_state(boost::asio::ip::udp::socket &socket, // get a local clock timestamp, then the latest frame from the device, then another timestamp timeEvent.local_request_time = cartographer::common::UniversalTimeScaleClock::now(); - //auto start_time = std::chrono::high_resolution_clock::now(); receive_bytes_transferred = socket.receive_from( boost::asio::buffer(friData.receiveBuffer, KUKA::FRI::FRI_MONITOR_MSG_MAX_SIZE), sender_endpoint, message_flags, receive_ec); - //auto end_time = std::chrono::high_resolution_clock::now(); - // std::cout << "Time Delay in chrono: "<(end_time - start_time).count() << " microseconds" << std::endl; - timeEvent.local_receive_time = cartographer::common::UniversalTimeScaleClock::now(); - - // std::cout << "Time Delay in UniversalTimeScaleClock: "< 0); decode(friData, receive_bytes_transferred); friData.lastSendCounter++; @@ -607,157 +604,157 @@ class KukaFRIClientDataDriver } } - ~KukaFRIClientDataDriver() { destruct(); } + ~KukaFRIClientDataDriver() { destruct(); } - /// Is everything ok? - /// @return true if the kuka fri connection is actively running without any - /// issues - /// @todo consider expanding to support real error codes - bool is_active() { return !exceptionPtr && isConnectionEstablished_; } + /// Is everything ok? + /// @return true if the kuka fri connection is actively running without any + /// issues + /// @todo consider expanding to support real error codes + bool is_active() { return !exceptionPtr && isConnectionEstablished_; } private: /// Reads data off of the real kuka fri device in a separate thread /// @todo consider switching to single producer single consumer queue to avoid /// locking overhead, but keep latency in mind /// https://github.com/facebook/folly/blob/master/folly/docs/ProducerConsumerQueue.md -void update() { - try { - - LowLevelStepAlgorithmType step_alg; - /// nextState is the object currently being loaded with data off the network - /// the driver thread should access this exclusively in update() - LatestState nextState = make_valid_LatestState(); - LatestState latestCommandBackup = make_valid_LatestState(); - /// Where is the sender_endpoint initialized? It should be bonded with the sender address and port. - boost::asio::ip::udp::endpoint sender_endpoint; - boost::asio::ip::udp::socket socket(connect(params_, io_service_, sender_endpoint)); - KukaState kukastate; ///< @todo TODO(ahundt) remove this line when new - /// api works completely since old one is deprecated - - ///////////// - // run the primary update loop in a separate thread - while (!m_shouldStop) { - /// @todo maybe there is a more convienient way to set this that is - /// easier for users? perhaps initializeClientDataForiiwa()? - - // nextState and latestCommandBackup should never be null - BOOST_VERIFY(std::get(nextState)); - BOOST_VERIFY(std::get(latestCommandBackup)); - - // set the flag that must always be there - std::get(nextState)->expectedMonitorMsgID = KUKA::LBRState::LBRMONITORMESSAGEID; - - auto lowLevelAlgorithmParamP = std::get(nextState); - - // if there is a valid low level algorithm param command set the new goal - if(lowLevelAlgorithmParamP) step_alg.setGoal(*lowLevelAlgorithmParamP); - - // actually talk over the network to receive an update and send out a new command - grl::robot::arm::update_state( - socket, step_alg, - *std::get(nextState), - std::get(nextState), - std::get(nextState), - std::get(nextState), - std::get(nextState), - std::get(nextState)); - - /// @todo use atomics to eliminate the global mutex lock for this object - // lock the mutex to communicate with the user thread - // if it cannot lock, simply send the previous message - // again - // bool try_lock(): Attempt to obtain ownership for the current thread without blocking. - if (ptrMutex_.try_lock()) { - ////////////////////////////////////////////// - // right now this is the state of everything: - ////////////////////////////////////////////// - // - // Always Valid: - // - // nextState: valid, contains the latest update - // latestCommandBackup: should always be valid (though hasCommand - // might be false) - // - // Either Valid or Null: - // latestStateForUser_ : null if the user took data out, valid otherwise - // newCommandForDriver_: null if there is no new command data from the user, vaild otherwise - - // 1) set the outgoing latest state for the user to pick up - // latestStateForUser_ is finalized - std::swap(latestStateForUser_, nextState); - - // 2) get a new incoming command if available and set incoming command - // variable to null - if (std::get(newCommandForDriver_)) { - // 3) back up the new incoming command - // latestCommandBackup is finalized, newCommandForDriver_ needs to be cleared out - std::swap(latestCommandBackup, newCommandForDriver_); - - // nextState may be null - if (!std::get(nextState)) { - nextState = std::move(newCommandForDriver_); - } else if (!(spareStates_.size() == spareStates_.capacity())) { - spareStates_.emplace_back(std::move(newCommandForDriver_)); - } else { - std::get(newCommandForDriver_).reset(); - } - } + void update() { + try { - // finalized: latestStateForUser_, latestCommandBackup, - // newCommandForDriver_ is definitely null - // issues to be resolved: - // nextState: may be null right now, and it should be valid - // newCommandForDriver_: needs to be cleared with 100% certainty - BOOST_VERIFY(spareStates_.size() > 0); + LowLevelStepAlgorithmType step_alg; + /// nextState is the object currently being loaded with data off the network + /// the driver thread should access this exclusively in update() + LatestState nextState = make_valid_LatestState(); + LatestState latestCommandBackup = make_valid_LatestState(); + /// Where is the sender_endpoint initialized? It should be bonded with the sender address and port. + boost::asio::ip::udp::endpoint sender_endpoint; + boost::asio::ip::udp::socket socket(connect(params_, io_service_, sender_endpoint)); + KukaState kukastate; ///< @todo TODO(ahundt) remove this line when new + /// api works completely since old one is deprecated + + ///////////// + // run the primary update loop in a separate thread + while (!m_shouldStop) { + /// @todo maybe there is a more convienient way to set this that is + /// easier for users? perhaps initializeClientDataForiiwa()? + + // nextState and latestCommandBackup should never be null + BOOST_VERIFY(std::get(nextState)); + BOOST_VERIFY(std::get(latestCommandBackup)); + + // set the flag that must always be there + std::get(nextState)->expectedMonitorMsgID = KUKA::LBRState::LBRMONITORMESSAGEID; + + auto lowLevelAlgorithmParamP = std::get(nextState); + + // if there is a valid low level algorithm param command set the new goal + if(lowLevelAlgorithmParamP) step_alg.setGoal(*lowLevelAlgorithmParamP); + + // actually talk over the network to receive an update and send out a new command + grl::robot::arm::update_state( + socket, step_alg, + *std::get(nextState), + std::get(nextState), + std::get(nextState), + std::get(nextState), + std::get(nextState), + std::get(nextState)); + + /// @todo use atomics to eliminate the global mutex lock for this object + // lock the mutex to communicate with the user thread + // if it cannot lock, simply send the previous message + // again + // bool try_lock(): Attempt to obtain ownership for the current thread without blocking. + if (ptrMutex_.try_lock()) { + ////////////////////////////////////////////// + // right now this is the state of everything: + ////////////////////////////////////////////// + // + // Always Valid: + // + // nextState: valid, contains the latest update + // latestCommandBackup: should always be valid (though hasCommand + // might be false) + // + // Either Valid or Null: + // latestStateForUser_ : null if the user took data out, valid otherwise + // newCommandForDriver_: null if there is no new command data from the user, vaild otherwise + + // 1) set the outgoing latest state for the user to pick up + // latestStateForUser_ is finalized + std::swap(latestStateForUser_, nextState); + + // 2) get a new incoming command if available and set incoming command + // variable to null + if (std::get(newCommandForDriver_)) { + // 3) back up the new incoming command + // latestCommandBackup is finalized, newCommandForDriver_ needs to be cleared out + std::swap(latestCommandBackup, newCommandForDriver_); + + // nextState may be null + if (!std::get(nextState)) { + nextState = std::move(newCommandForDriver_); + } else if (!(spareStates_.size() == spareStates_.capacity())) { + spareStates_.emplace_back(std::move(newCommandForDriver_)); + } else { + std::get(newCommandForDriver_).reset(); + } + } + // finalized: latestStateForUser_, latestCommandBackup, + // newCommandForDriver_ is definitely null + // issues to be resolved: + // nextState: may be null right now, and it should be valid + // newCommandForDriver_: needs to be cleared with 100% certainty + BOOST_VERIFY(spareStates_.size() > 0); - if (!std::get(nextState) && - spareStates_.size()) { - // move the last element out and shorten the vector - nextState = std::move(*(spareStates_.end() - 1)); - spareStates_.pop_back(); - } - BOOST_VERIFY(std::get(nextState)); + if (!std::get(nextState) && + spareStates_.size()) { + // move the last element out and shorten the vector + nextState = std::move(*(spareStates_.end() - 1)); + spareStates_.pop_back(); + } - KUKA::FRI::ClientData &nextClientData = - *std::get(nextState); - KUKA::FRI::ClientData &latestClientData = - *std::get(latestStateForUser_); - - // copy essential data from latestStateForUser_ to nextState - nextClientData.lastState = latestClientData.lastState; - nextClientData.sequenceCounter = latestClientData.sequenceCounter; - nextClientData.lastSendCounter = latestClientData.lastSendCounter; - nextClientData.expectedMonitorMsgID = latestClientData.expectedMonitorMsgID; - - // copy command from latestCommandBackup to nextState aka - // nextClientData - KUKA::FRI::ClientData &latestCommandBackupClientData = - *std::get(latestCommandBackup); - set(nextClientData.commandMsg, - latestCommandBackupClientData.commandMsg); - - // if there are no error codes and we have received data, - // then we can consider the connection established! - /// @todo perhaps data should always send too? - if (!std::get(nextState) && - !std::get(nextState) && - std::get(nextState)) { - isConnectionEstablished_ = true; + BOOST_VERIFY(std::get(nextState)); + + KUKA::FRI::ClientData &nextClientData = + *std::get(nextState); + KUKA::FRI::ClientData &latestClientData = + *std::get(latestStateForUser_); + + // copy essential data from latestStateForUser_ to nextState + nextClientData.lastState = latestClientData.lastState; + nextClientData.sequenceCounter = latestClientData.sequenceCounter; + nextClientData.lastSendCounter = latestClientData.lastSendCounter; + nextClientData.expectedMonitorMsgID = latestClientData.expectedMonitorMsgID; + + // copy command from latestCommandBackup to nextState aka + // nextClientData + KUKA::FRI::ClientData &latestCommandBackupClientData = + *std::get(latestCommandBackup); + set(nextClientData.commandMsg, + latestCommandBackupClientData.commandMsg); + + // if there are no error codes and we have received data, + // then we can consider the connection established! + /// @todo perhaps data should always send too? + if (!std::get(nextState) && + !std::get(nextState) && + std::get(nextState)) { + isConnectionEstablished_ = true; + } + ptrMutex_.unlock(); } - ptrMutex_.unlock(); } + } catch (...) { + // transport the exception to the main thread in a safe manner + exceptionPtr = std::current_exception(); + m_shouldStop = true; + isConnectionEstablished_ = false; } - } catch (...) { - // transport the exception to the main thread in a safe manner - exceptionPtr = std::current_exception(); - m_shouldStop = true; - isConnectionEstablished_ = false; - } - isConnectionEstablished_ = false; - } + isConnectionEstablished_ = false; + } enum LatestStateIndex { latest_low_level_algorithm_params, @@ -783,7 +780,7 @@ void update() { static LatestState make_LatestState(std::shared_ptr lowLevelAlgorithmParams, std::shared_ptr &clientData) { - return std::make_tuple(lowLevelAlgorithmParams, + return std::make_tuple(lowLevelAlgorithmParams, clientData, boost::system::error_code(), std::size_t(), boost::system::error_code(), std::size_t(), diff --git a/include/grl/sensor/readFusionTrackFromFlatbuffer.hpp b/include/grl/sensor/readFusionTrackFromFlatbuffer.hpp deleted file mode 100644 index ec9eb29..0000000 --- a/include/grl/sensor/readFusionTrackFromFlatbuffer.hpp +++ /dev/null @@ -1,103 +0,0 @@ -#ifndef GRL_READ_FUSIONTRACK_FROM_FLATBUFFER -#define GRL_READ_FUSIONTRACK_FROM_FLATBUFFER - -#define FLATBUFFERS_DEBUG_VERIFICATION_FAILURE -#include "grl/flatbuffer/FusionTrack_generated.h" -#include "grl/flatbuffer/Time_generated.h" -#include "grl/flatbuffer/LogKUKAiiwaFusionTrack_generated.h" -#include -#include - -namespace grl { - - const grl::flatbuffer::LogKUKAiiwaFusionTrack* getLogKUKAiiwaFusionTrackFromFlatbuffer(const std::string filename){ - - //std::cout << "CurrentWorkingDir: " << curWorkingDir << std::endl; - if(filename.empty()) return nullptr; - FILE* file = std::fopen(filename.c_str(), "rb"); - if(!file) { - std::perror("File opening failed"); - return nullptr; - } - std::fseek(file, 0L, SEEK_END); // seek to end - std::size_t length = std::ftell(file); - std::fseek(file, 0L, SEEK_SET); // seek to start - std::vector buffer(length); - std::fread(buffer.data(), sizeof(uint8_t), buffer.size(), file); - std::fclose(file); - return grl::flatbuffer::GetLogKUKAiiwaFusionTrack(&buffer[0]); - } - - /// Return the joint_index th joint angle - // std::vector getGeoPositions(const grl::flatbuffer::LogKUKAiiwaFusionTrack* logKUKAiiwaFusionTrackP, int joint_index){ - // if(kukaStatesP == nullptr) - // return std::vector(); - // auto states = kukaStatesP->states(); - // std::vector jointPosition; - // std::size_t state_size = states->size(); - // for(int i = 1; iGet(i); - // auto FRIMessage = KUKAiiwaState->FRIMessage(); - // auto joints_Position = FRIMessage->measuredJointPosition(); // flatbuffers::Vector * - // jointPosition.push_back(joints_Position->Get(joint_index)); - // } - // return jointPosition; - // } - - std::vector getTimeStamp_FusionTrack(const fbs_tk::Root &logKUKAiiwaFusionTrackP, std::string time_type){ - - auto states = logKUKAiiwaFusionTrackP->states(); - std::vector timeStamp; - - std::size_t state_size = states->size(); - //Eigen::VectoXd time_stamp(state_size); - for(int i = 0; iGet(i); - auto timeEvent = kukaiiwaFusionTrackMessage->timeEvent(); - - if(time_type =="device_time"){ - timeStamp.push_back(timeEvent->device_time()); - //time_stamp<device_time(); - } else if (time_type == "local_request_time") { - timeStamp.push_back(timeEvent->local_request_time()); - } else if (time_type == "local_receive_time"){ - timeStamp.push_back(timeEvent->local_receive_time()); - } - } - return timeStamp; - } - - // void writetoCSVforFusionTrack(std::string binaryfilename, std::string csvfilename) { - // FILE* file = std::fopen(binaryfilename.c_str(), "rb"); - // std::fseek(file, 0L, SEEK_END); // seek to end - // std::size_t length = std::ftell(file); - // std::fseek(file, 0L, SEEK_SET); // seek to start - // std::vector buffer(length); - // std::fread(buffer.data(), sizeof(uint8_t), buffer.size(), file); - // std::fclose(file); - // auto logKUKAiiwaFusionTrack = grl::flatbuffer::GetLogKUKAiiwaFusionTrack(&buffer[0]); - // auto states = logKUKAiiwaFusionTrack->states(); - // std::size_t state_size = states->size(); - // std::cout<< "------FusionTrack State Size: "< deviceTime = grl:: getTimeStamp_FusionTrack(logKUKAiiwaFusionTrack, "device_time"); - // std::vector local_request_time = grl:: getTimeStamp_FusionTrack(logKUKAiiwaFusionTrack, "local_request_time"); - // std::vector local_receive_time = grl:: getTimeStamp_FusionTrack(logKUKAiiwaFusionTrack, "local_receive_time"); - - // // create an ofstream for the file output (see the link on streams for more info) - - // std::ofstream fs; - // // create a name for the file output - - // fs.open(csvfilename, std::ofstream::out | std::ofstream::app); - // // write the file headers - // fs << "local_request_time" << ",local_receive_time" << ",Time_Difference" << ",Device_Time" <set(grl::flatbuffer::ArmState::MoveArmJointServo); kukaDriverP->set(goal_position_command_time_duration,grl::time_duration_command_tag()); - std::cout << "KUKA COMMAND MODE: " << std::get(params) << "\n"; + // std::cout << "KUKA COMMAND MODE: " << std::get(params) << "\n"; } unsigned int num_missed = 0; @@ -383,7 +383,7 @@ BOOST_AUTO_TEST_CASE(runRepeatedly) // std::cout<< "single message data size (bytes): " << newData << " Buffer size: " << builder_size_bytes/MegaByte <<" MB" << std::endl; } - std::cout<< "kukaiiwaStateVec:" << kukaiiwaStateVec.size() << std::endl; + //std::cout<< "kukaiiwaStateVec:" << kukaiiwaStateVec.size() << std::endl; flatbuffers::Offset kukaStates = grl::toFlatBuffer(*fbbP, kukaiiwaStateVec); @@ -399,7 +399,7 @@ BOOST_AUTO_TEST_CASE(runRepeatedly) OK = OK && grl::flatbuffer::VerifyKUKAiiwaStatesBuffer(verifier); assert(OK && "VerifyKUKAiiwaStatesBuffer"); - std::cout << "Buffer size: " << bufsize << std::endl; + //std::cout << "Buffer size: " << bufsize << std::endl; std::string binary_file_path = "Kuka_test_binary.iiwa"; std::string json_file_path = "kuka_test_text.json"; diff --git a/test/readFRIFBTest.cpp b/test/readFRIFBTest.cpp index 7b7c97a..f0fe28e 100644 --- a/test/readFRIFBTest.cpp +++ b/test/readFRIFBTest.cpp @@ -2,8 +2,7 @@ // Library includes #define FLATBUFFERS_DEBUG_VERIFICATION_FAILURE #include "flatbuffers/util.h" -#include "grl/kuka/readKukaiiwaFromFlatbuffer.hpp" -#include "grl/sensor/readFusionTrackFromFlatbuffer.hpp" +#include "grl/flatbuffer/readDatafromBinary.hpp" #include #include "grl/flatbuffer/FusionTrack_generated.h" #include "grl/flatbuffer/Time_generated.h" @@ -17,34 +16,45 @@ #include #include -void writetoCSVforFusionTrackKukaiiwa(std::string KUKAbinaryfilename, std::string FusionTrackbinaryfilename, std::string csvfilename); +std::string foldname = "/home/chunting/src/V-REP_PRO_EDU_V3_4_0_Linux/"; +std::string kukaBinaryfile = foldname + "2018_02_02_18_21_43_Kukaiiwa.iiwa"; +std::string fusiontrackBinaryfile = foldname + "2018_02_02_18_21_58_FusionTrack.flik"; +std::string CSVfilename = foldname + current_date_and_time_string() + "_FTKUKA.csv"; +std::string FT_CSVfilename = foldname + current_date_and_time_string() + "_FT.csv"; +std::string KUKA_CSVfilename = foldname + current_date_and_time_string() + "_KUKA.csv"; + +void writetoCSV(std::string & csvfilename, std::vector device_time, std::vector local_request_time, std::vector local_receive_time); +void writetoCSVforFusionTrackKukaiiwa(); + int main(int argc, char* argv[]) { - std::string foldname = "/home/chunting/src/V-REP_PRO_EDU_V3_4_0_Linux/"; - std::string kukaBinaryfile = foldname + "2018_02_02_18_21_43_Kukaiiwa.iiwa"; - std::string fusiontrackBinaryfile = foldname + "2018_02_02_18_21_58_FusionTrack.flik"; - std::string CSVfilename = foldname + current_date_and_time_string() + "_FTKUKA.csv"; - writetoCSVforFusionTrackKukaiiwa(kukaBinaryfile, fusiontrackBinaryfile, CSVfilename); + + writetoCSVforFusionTrackKukaiiwa(); return 0; } -void writetoCSVforFusionTrackKukaiiwa(std::string KUKAbinaryfilename, std::string FusionTrackbinaryfilename, std::string csvfilename) { - fbs_tk::Root KUKAiiwaStatesRoot = fbs_tk::open_root(KUKAbinaryfilename); +void writetoCSVforFusionTrackKukaiiwa() { + fbs_tk::Root KUKAiiwaStatesRoot = + fbs_tk::open_root(kukaBinaryfile); auto KUKA_states = KUKAiiwaStatesRoot->states(); std::size_t kuka_state_size = KUKA_states->size(); std::cout<< "------Kuka_state_size: "<< kuka_state_size << std::endl; - std::vector kuka_deviceTime = grl:: getTimeStamp_Kuka(KUKAiiwaStatesRoot, "device_time"); - std::vector kuka_local_request_time = grl::getTimeStamp_Kuka(KUKAiiwaStatesRoot, "local_request_time"); - std::vector kuka_local_receive_time = grl::getTimeStamp_Kuka(KUKAiiwaStatesRoot, "local_receive_time"); + std::vector kuka_deviceTime = grl::getTimeStamp(KUKAiiwaStatesRoot, grl::kuka_tag(), grl::TimeType::device_time); + std::vector kuka_local_request_time = grl::getTimeStamp(KUKAiiwaStatesRoot, grl::kuka_tag(), grl::TimeType::local_request_time); + std::vector kuka_local_receive_time = grl::getTimeStamp(KUKAiiwaStatesRoot, grl::kuka_tag(), grl::TimeType::local_receive_time); + + writetoCSV(KUKA_CSVfilename, kuka_deviceTime, kuka_local_request_time, kuka_local_receive_time); + - fbs_tk::Root FusionTrackStatesRoot = fbs_tk::open_root(FusionTrackbinaryfilename); + fbs_tk::Root FusionTrackStatesRoot = fbs_tk::open_root(fusiontrackBinaryfile); auto FT_states = FusionTrackStatesRoot->states(); std::size_t FT_state_size = FT_states->size(); std::cout<< "------FusionTrack State Size: "<< FT_state_size << std::endl; - std::vector FT_deviceTime = grl:: getTimeStamp_FusionTrack(FusionTrackStatesRoot, "device_time"); - std::vector FT_local_request_time = grl::getTimeStamp_FusionTrack(FusionTrackStatesRoot, "local_request_time"); - std::vector FT_local_receive_time = grl::getTimeStamp_FusionTrack(FusionTrackStatesRoot, "local_receive_time"); + std::vector FT_deviceTime = grl:: getTimeStamp(FusionTrackStatesRoot, grl::fusiontracker_tag(), grl::TimeType::device_time); + std::vector FT_local_request_time = grl::getTimeStamp(FusionTrackStatesRoot, grl::fusiontracker_tag(), grl::TimeType::local_request_time); + std::vector FT_local_receive_time = grl::getTimeStamp(FusionTrackStatesRoot, grl::fusiontracker_tag(), grl::TimeType::local_receive_time); + writetoCSV( FT_CSVfilename, FT_deviceTime, FT_local_request_time, FT_local_receive_time); std::vector local_request_time_offset; std::vector local_receive_time_offset_FT; @@ -52,9 +62,11 @@ void writetoCSVforFusionTrackKukaiiwa(std::string KUKAbinaryfilename, std::strin std::vector device_time_offset_kuka; std::vector device_time_offset_FT; std::vector device_type; - int64_t initial_local_request_time = std::min(kuka_local_request_time[0], FT_local_request_time[0]); - int64_t initial_local_receive_time_kuka = kuka_local_receive_time[0]; - int64_t initial_local_receive_time_FT = FT_local_receive_time[0]; + + int64_t initial_local_time = std::min(FT_local_request_time[0], kuka_local_request_time[0]); + + + int64_t initial_device_time_kuka = kuka_deviceTime[0]; int64_t initial_device_time_FT = FT_deviceTime[0]; @@ -64,26 +76,21 @@ void writetoCSVforFusionTrackKukaiiwa(std::string KUKAbinaryfilename, std::strin auto itkuka_local_request_timeEnd = kuka_local_request_time.end(); auto itFT_local_request_timeEnd = FT_local_request_time.end(); - // local_request_time_offset.reserve(kuka_state_size+FT_state_size); - // local_receive_time_offset_FT.reserve(kuka_state_size+FT_state_size); - // local_receive_time_offset_kuka.reserve(kuka_state_size+FT_state_size); - // device_time_offset_kuka.reserve(kuka_state_size+FT_state_size); - // device_time_offset_FT.reserve(kuka_state_size+FT_state_size); - int kuka_index = 0; int FT_index = 0; bool flag = false; while(*itkuka_local_request_time > *itFT_local_request_time){ itFT_local_request_time++; + FT_index++; } while ( itkuka_local_request_time != itkuka_local_request_timeEnd && itFT_local_request_time != itFT_local_request_timeEnd ) { if ( *itkuka_local_request_time < *itFT_local_request_time ){ - local_request_time_offset.push_back( *itkuka_local_request_time - initial_local_request_time); - local_receive_time_offset_FT.push_back(FT_local_receive_time[FT_index] - initial_local_receive_time_FT); - local_receive_time_offset_kuka.push_back(kuka_local_receive_time[kuka_index] - initial_local_receive_time_kuka); + local_request_time_offset.push_back( *itkuka_local_request_time - initial_local_time); + local_receive_time_offset_FT.push_back(FT_local_receive_time[FT_index] - initial_local_time); + local_receive_time_offset_kuka.push_back(kuka_local_receive_time[kuka_index] - initial_local_time); device_time_offset_kuka.push_back(kuka_deviceTime[kuka_index]-initial_device_time_kuka); device_time_offset_FT.push_back(FT_deviceTime[FT_index]-initial_device_time_FT); kuka_index++; @@ -92,18 +99,18 @@ void writetoCSVforFusionTrackKukaiiwa(std::string KUKAbinaryfilename, std::strin } else if( *itkuka_local_request_time > *itFT_local_request_time) { - local_request_time_offset.push_back( *itFT_local_request_time - initial_local_request_time ); - local_receive_time_offset_FT.push_back(FT_local_receive_time[FT_index] - initial_local_receive_time_FT); - local_receive_time_offset_kuka.push_back(kuka_local_receive_time[kuka_index] - initial_local_receive_time_kuka); + local_request_time_offset.push_back( *itFT_local_request_time - initial_local_time ); + local_receive_time_offset_FT.push_back(FT_local_receive_time[FT_index] - initial_local_time); + local_receive_time_offset_kuka.push_back(kuka_local_receive_time[kuka_index] - initial_local_time); device_time_offset_kuka.push_back(kuka_deviceTime[kuka_index]-initial_device_time_kuka); device_time_offset_FT.push_back(FT_deviceTime[FT_index]-initial_device_time_FT); FT_index++; itFT_local_request_time++; device_type.push_back(1); } else { - local_request_time_offset.push_back( *itFT_local_request_time - initial_local_request_time ); - local_receive_time_offset_FT.push_back(FT_local_receive_time[FT_index] - initial_local_receive_time_FT); - local_receive_time_offset_kuka.push_back(kuka_local_receive_time[kuka_index] - initial_local_receive_time_kuka); + local_request_time_offset.push_back( *itFT_local_request_time - initial_local_time ); + local_receive_time_offset_FT.push_back(FT_local_receive_time[FT_index] - initial_local_time); + local_receive_time_offset_kuka.push_back(kuka_local_receive_time[kuka_index] - initial_local_time); device_time_offset_kuka.push_back(kuka_deviceTime[kuka_index]-initial_device_time_kuka); device_time_offset_FT.push_back(FT_deviceTime[FT_index]-initial_device_time_FT); FT_index++; @@ -118,9 +125,9 @@ void writetoCSVforFusionTrackKukaiiwa(std::string KUKAbinaryfilename, std::strin // // copy rest of kuka_local_request_time array // while ( itkuka_local_request_time != itkuka_local_request_timeEnd ){ // FT_index = FT_state_size-2; - // local_request_time_offset.push_back( *itkuka_local_request_time - initial_local_request_time); - // local_receive_time_offset_FT.push_back(FT_local_receive_time[FT_index] - initial_local_receive_time_FT); - // local_receive_time_offset_kuka.push_back(kuka_local_receive_time[kuka_index] - initial_local_receive_time_kuka); + // local_request_time_offset.push_back( *itkuka_local_request_time - initial_local_time); + // local_receive_time_offset_FT.push_back(FT_local_receive_time[FT_index] - initial_local_time); + // local_receive_time_offset_kuka.push_back(kuka_local_receive_time[kuka_index] - initial_local_time); // device_time_offset_kuka.push_back(kuka_deviceTime[kuka_index]-initial_device_time_kuka); // device_time_offset_FT.push_back(FT_deviceTime[FT_index]-initial_device_time_FT); // kuka_index++; @@ -133,9 +140,9 @@ void writetoCSVforFusionTrackKukaiiwa(std::string KUKAbinaryfilename, std::strin // // copy rest of FT_local_request_time array // while ( itFT_local_request_time != itFT_local_request_timeEnd ) { // kuka_index = kuka_state_size-2; - // local_request_time_offset.push_back( *itFT_local_request_time - initial_local_request_time ); - // local_receive_time_offset_FT.push_back(FT_local_receive_time[FT_index] - initial_local_receive_time_FT); - // local_receive_time_offset_kuka.push_back(kuka_local_receive_time[kuka_index] - initial_local_receive_time_kuka); + // local_request_time_offset.push_back( *itFT_local_request_time - initial_local_time ); + // local_receive_time_offset_FT.push_back(FT_local_receive_time[FT_index] - initial_local_time); + // local_receive_time_offset_kuka.push_back(kuka_local_receive_time[kuka_index] - initial_local_time); // device_time_offset_kuka.push_back(kuka_deviceTime[kuka_index]-initial_device_time_kuka); // device_time_offset_FT.push_back(FT_deviceTime[FT_index]-initial_device_time_FT); // FT_index++; @@ -144,11 +151,13 @@ void writetoCSVforFusionTrackKukaiiwa(std::string KUKAbinaryfilename, std::strin // // std::cout<<"kuka_local_receive_time: " << kuka_index <<" "< device_time, + std::vector local_request_time, + std::vector local_receive_time){ + + int64_t initial_local_time = local_request_time[0]; + int64_t initial_device_time = device_time[0]; + std::size_t size = device_time.size(); + + std::ofstream fs; + // create a name for the file output + fs.open( csvfilename, std::ofstream::out | std::ofstream::app); + // write the file headers + fs << "local_request_time_X," + << "local_receive_time_offset," + << "device_time_offset," + << "Y" << std::endl; + for(int i=0; i Date: Fri, 9 Feb 2018 11:02:51 -0500 Subject: [PATCH 044/102] Fix a bug about timeevent --- include/grl/flatbuffer/readDatafromBinary.hpp | 154 +++++----- test/readFRIFBTest.cpp | 286 +++++++++--------- 2 files changed, 226 insertions(+), 214 deletions(-) diff --git a/include/grl/flatbuffer/readDatafromBinary.hpp b/include/grl/flatbuffer/readDatafromBinary.hpp index 3a2dd70..1cc6596 100644 --- a/include/grl/flatbuffer/readDatafromBinary.hpp +++ b/include/grl/flatbuffer/readDatafromBinary.hpp @@ -13,6 +13,7 @@ #include "grl/flatbuffer/FusionTrack_generated.h" #include "grl/flatbuffer/LogKUKAiiwaFusionTrack_generated.h" #include +#include #include // For printing and file access. #include @@ -24,30 +25,32 @@ #include #include #include + namespace grl { + typedef Eigen::Matrix VectorXd; struct kuka_tag {}; struct fusiontracker_tag {}; enum TimeType { device_time = 0, local_request_time = 1, local_receive_time = 2 }; - std::vector getTimeStamp(const fbs_tk::Root &kukaStatesP, kuka_tag, TimeType time_type){ + grl::VectorXd getTimeStamp(const fbs_tk::Root &kukaStatesP, kuka_tag, TimeType time_type){ auto states = kukaStatesP->states(); - std::vector timeStamp; std::size_t state_size = states->size(); - for(int i = 1; iGet(i); auto FRIMessage = KUKAiiwaState->FRIMessage(); auto timeEvent = FRIMessage->timeStamp(); switch(time_type) { case device_time:{ - timeStamp.push_back(timeEvent->device_time()); + timeStamp(i) = timeEvent->device_time(); break; } case local_request_time:{ - timeStamp.push_back(timeEvent->local_request_time()); + timeStamp(i) = timeEvent->local_request_time(); break; } case local_receive_time:{ - timeStamp.push_back(timeEvent->local_receive_time()); + timeStamp(i) = timeEvent->local_receive_time(); break; } default:{ @@ -58,27 +61,26 @@ namespace grl { return timeStamp; } - std::vector getTimeStamp(const fbs_tk::Root &logKUKAiiwaFusionTrackP, fusiontracker_tag, TimeType time_type){ + grl::VectorXd getTimeStamp(const fbs_tk::Root &logKUKAiiwaFusionTrackP, fusiontracker_tag, TimeType time_type){ auto states = logKUKAiiwaFusionTrackP->states(); - std::vector timeStamp; - std::size_t state_size = states->size(); + grl::VectorXd timeStamp(state_size); //Eigen::VectoXd time_stamp(state_size); for(int i = 0; iGet(i); auto timeEvent = kukaiiwaFusionTrackMessage->timeEvent(); switch(time_type) { case device_time:{ - timeStamp.push_back(timeEvent->device_time()); + timeStamp(i) = timeEvent->device_time(); break; } case local_request_time:{ - timeStamp.push_back(timeEvent->local_request_time()); + timeStamp(i) = timeEvent->local_request_time(); break; } case local_receive_time:{ - timeStamp.push_back(timeEvent->local_receive_time()); + timeStamp(i) = timeEvent->local_receive_time(); break; } default:{ @@ -91,85 +93,93 @@ namespace grl { /// Return the joint_index th joint angle - std::vector getJointAngles(const grl::flatbuffer::KUKAiiwaStates* kukaStatesP, int joint_index){ - if(kukaStatesP == nullptr) - return std::vector(); + Eigen::VectorXf getJointAnglesFromKUKA(const fbs_tk::Root &kukaStatesP, int joint_index){ + // if(kukaStatesP == nullptr) + // return Eigen::VectorXf(); auto states = kukaStatesP->states(); - std::vector jointPosition; + std::size_t state_size = states->size(); - for(int i = 1; iGet(i); auto FRIMessage = KUKAiiwaState->FRIMessage(); auto joints_Position = FRIMessage->measuredJointPosition(); // flatbuffers::Vector * - jointPosition.push_back(joints_Position->Get(joint_index)); + jointPosition(i) = joints_Position->Get(joint_index); } return jointPosition; } /// Return the joint_index th joint angle - std::vector> getAllJointAngles(const grl::flatbuffer::KUKAiiwaStates* kukaStatesP){ - if(kukaStatesP == nullptr) - return std::vector>(); - auto states = kukaStatesP->states(); - std::vector> allJointPosition; + Eigen::MatrixXf getAllJointAngles(const fbs_tk::Root &kukaStatesP){ + // if(kukaStatesP == nullptr) + // return Eigen::MatrixXf(); + auto states = kukaStatesP->states(); std::size_t state_size = states->size(); - for(int i = 1; iGet(i); auto FRIMessage = KUKAiiwaState->FRIMessage(); auto joints_Position = FRIMessage->measuredJointPosition(); // flatbuffers::Vector * - std::vector oneJointPosition; + Eigen::VectorXf oneStateJointPosition; for(int joint_index=0; joint_index<7; joint_index++){ - oneJointPosition.push_back(joints_Position->Get(joint_index)); + oneStateJointPosition(i) = joints_Position->Get(joint_index); } - allJointPosition.push_back(oneJointPosition); + allJointPosition.row(i) = oneStateJointPosition.transpose(); } return allJointPosition; } - // void writetoCSVforKuka(std::string binaryfilename, std::string csvfilename) { - // FILE* file = std::fopen(binaryfilename.c_str(), "rb"); - // std::fseek(file, 0L, SEEK_END); // seek to end - // std::size_t length = std::ftell(file); - // std::fseek(file, 0L, SEEK_SET); // seek to start - // std::vector buffer(length); - // std::fread(buffer.data(), sizeof(uint8_t), buffer.size(), file); - // std::fclose(file); - // auto kukaiiwaStates = grl::flatbuffer::GetKUKAiiwaStates(&buffer[0]); - // auto states = kukaiiwaStates->states(); - // std::size_t state_size = states->size(); - // std::cout<< "------Kuka_state_size: "< deviceTime = grl:: getTimeStamp_Kuka(kukaiiwaStates, "device_time"); - // std::vector local_request_time = grl:: getTimeStamp_Kuka(kukaiiwaStates, "local_request_time"); - // std::vector local_receive_time = grl:: getTimeStamp_Kuka(kukaiiwaStates, "local_receive_time"); - - // std::vector jointAngles_0 = grl::getJointAngles(kukaiiwaStates, 0); - // std::vector jointAngles_1 = grl::getJointAngles(kukaiiwaStates, 1); - // std::vector jointAngles_2 = grl::getJointAngles(kukaiiwaStates, 2); - // std::vector jointAngles_3 = grl::getJointAngles(kukaiiwaStates, 3); - // std::vector jointAngles_4 = grl::getJointAngles(kukaiiwaStates, 4); - // std::vector jointAngles_5 = grl::getJointAngles(kukaiiwaStates, 5); - // std::vector jointAngles_6 = grl::getJointAngles(kukaiiwaStates, 6); - - // std::vector> allJointAngles = grl::getAllJointAngles(kukaiiwaStates); - // // if(allJointAngles.size() > 0){ - // // std::cout<< "------States size: "< KUKAiiwaStatesRoot = fbs_tk::open_root(kukaBinaryfile); + auto states = KUKAiiwaStatesRoot->states(); + std::size_t kuka_state_size = states->size(); + std::cout<< "------Kuka_state_size: "< device_time, std::vector local_request_time, std::vector local_receive_time); +void writetoCSV(std::string & csvfilename, grl::VectorXd device_time, grl::VectorXd local_request_time, grl::VectorXd local_receive_time); void writetoCSVforFusionTrackKukaiiwa(); int main(int argc, char* argv[]) { writetoCSVforFusionTrackKukaiiwa(); - + grl::writetoCSVforKuka(kukaBinaryfile, KUKA_CSVfilename_Joint); return 0; } @@ -40,161 +41,161 @@ void writetoCSVforFusionTrackKukaiiwa() { auto KUKA_states = KUKAiiwaStatesRoot->states(); std::size_t kuka_state_size = KUKA_states->size(); std::cout<< "------Kuka_state_size: "<< kuka_state_size << std::endl; - std::vector kuka_deviceTime = grl::getTimeStamp(KUKAiiwaStatesRoot, grl::kuka_tag(), grl::TimeType::device_time); - std::vector kuka_local_request_time = grl::getTimeStamp(KUKAiiwaStatesRoot, grl::kuka_tag(), grl::TimeType::local_request_time); - std::vector kuka_local_receive_time = grl::getTimeStamp(KUKAiiwaStatesRoot, grl::kuka_tag(), grl::TimeType::local_receive_time); - writetoCSV(KUKA_CSVfilename, kuka_deviceTime, kuka_local_request_time, kuka_local_receive_time); + grl::VectorXd kuka_device_time = grl::getTimeStamp(KUKAiiwaStatesRoot, grl::kuka_tag(), grl::TimeType::device_time); + grl::VectorXd kuka_local_request_time = grl::getTimeStamp(KUKAiiwaStatesRoot, grl::kuka_tag(), grl::TimeType::local_request_time); + grl::VectorXd kuka_local_receive_time = grl::getTimeStamp(KUKAiiwaStatesRoot, grl::kuka_tag(), grl::TimeType::local_receive_time); + writetoCSV(KUKA_CSVfilename, kuka_device_time, kuka_local_request_time, kuka_local_receive_time); + fbs_tk::Root FusionTrackStatesRoot = fbs_tk::open_root(fusiontrackBinaryfile); auto FT_states = FusionTrackStatesRoot->states(); std::size_t FT_state_size = FT_states->size(); std::cout<< "------FusionTrack State Size: "<< FT_state_size << std::endl; - std::vector FT_deviceTime = grl:: getTimeStamp(FusionTrackStatesRoot, grl::fusiontracker_tag(), grl::TimeType::device_time); - std::vector FT_local_request_time = grl::getTimeStamp(FusionTrackStatesRoot, grl::fusiontracker_tag(), grl::TimeType::local_request_time); - std::vector FT_local_receive_time = grl::getTimeStamp(FusionTrackStatesRoot, grl::fusiontracker_tag(), grl::TimeType::local_receive_time); - writetoCSV( FT_CSVfilename, FT_deviceTime, FT_local_request_time, FT_local_receive_time); - - std::vector local_request_time_offset; - std::vector local_receive_time_offset_FT; - std::vector local_receive_time_offset_kuka; - std::vector device_time_offset_kuka; - std::vector device_time_offset_FT; - std::vector device_type; - - int64_t initial_local_time = std::min(FT_local_request_time[0], kuka_local_request_time[0]); - - - - int64_t initial_device_time_kuka = kuka_deviceTime[0]; - int64_t initial_device_time_FT = FT_deviceTime[0]; - - auto itkuka_local_request_time = kuka_local_request_time.begin(); - auto itFT_local_request_time = FT_local_request_time.begin(); - - auto itkuka_local_request_timeEnd = kuka_local_request_time.end(); - auto itFT_local_request_timeEnd = FT_local_request_time.end(); + grl::VectorXd FT_deviceTime = grl:: getTimeStamp(FusionTrackStatesRoot, grl::fusiontracker_tag(), grl::TimeType::device_time); + grl::VectorXd FT_local_request_time = grl::getTimeStamp(FusionTrackStatesRoot, grl::fusiontracker_tag(), grl::TimeType::local_request_time); + grl::VectorXd FT_local_receive_time = grl::getTimeStamp(FusionTrackStatesRoot, grl::fusiontracker_tag(), grl::TimeType::local_receive_time); + writetoCSV(FT_CSVfilename, FT_deviceTime, FT_local_request_time, FT_local_receive_time); + + auto initial_local_time = std::min(FT_local_request_time(0), kuka_local_request_time(0)); + auto initial_device_time_kuka = kuka_device_time(0); + auto initial_device_time_FT = FT_deviceTime(0); + FT_local_request_time = FT_local_request_time - initial_local_time * grl::VectorXd::Ones(FT_state_size); + FT_local_receive_time = FT_local_receive_time - initial_local_time * grl::VectorXd::Ones(FT_state_size); + FT_deviceTime = FT_deviceTime - initial_device_time_FT * grl::VectorXd::Ones(FT_state_size); + + kuka_local_request_time = kuka_local_request_time - initial_local_time * grl::VectorXd::Ones(kuka_state_size); + kuka_local_receive_time = kuka_local_receive_time - initial_local_time * grl::VectorXd::Ones(kuka_state_size); + kuka_device_time = kuka_device_time - initial_device_time_kuka * grl::VectorXd::Ones(kuka_state_size); + + std::size_t states_size = kuka_state_size+FT_state_size; + grl::VectorXd local_request_time_offset(states_size); + grl::VectorXd local_receive_time_offset_FT(states_size); + grl::VectorXd local_receive_time_offset_kuka(states_size); + grl::VectorXd device_time_offset_kuka(states_size); + grl::VectorXd device_time_offset_FT(states_size); + grl::VectorXd device_type(states_size); + + // auto kuka_size = kuka_local_request_time.size(); int kuka_index = 0; int FT_index = 0; - bool flag = false; - while(*itkuka_local_request_time > *itFT_local_request_time){ - itFT_local_request_time++; - FT_index++; - } - - while ( itkuka_local_request_time != itkuka_local_request_timeEnd && - itFT_local_request_time != itFT_local_request_timeEnd ) - { - if ( *itkuka_local_request_time < *itFT_local_request_time ){ - local_request_time_offset.push_back( *itkuka_local_request_time - initial_local_time); - local_receive_time_offset_FT.push_back(FT_local_receive_time[FT_index] - initial_local_time); - local_receive_time_offset_kuka.push_back(kuka_local_receive_time[kuka_index] - initial_local_time); - device_time_offset_kuka.push_back(kuka_deviceTime[kuka_index]-initial_device_time_kuka); - device_time_offset_FT.push_back(FT_deviceTime[FT_index]-initial_device_time_FT); - kuka_index++; - itkuka_local_request_time++; - device_type.push_back(0); - - - } else if( *itkuka_local_request_time > *itFT_local_request_time) { - local_request_time_offset.push_back( *itFT_local_request_time - initial_local_time ); - local_receive_time_offset_FT.push_back(FT_local_receive_time[FT_index] - initial_local_time); - local_receive_time_offset_kuka.push_back(kuka_local_receive_time[kuka_index] - initial_local_time); - device_time_offset_kuka.push_back(kuka_deviceTime[kuka_index]-initial_device_time_kuka); - device_time_offset_FT.push_back(FT_deviceTime[FT_index]-initial_device_time_FT); - FT_index++; - itFT_local_request_time++; - device_type.push_back(1); - } else { - local_request_time_offset.push_back( *itFT_local_request_time - initial_local_time ); - local_receive_time_offset_FT.push_back(FT_local_receive_time[FT_index] - initial_local_time); - local_receive_time_offset_kuka.push_back(kuka_local_receive_time[kuka_index] - initial_local_time); - device_time_offset_kuka.push_back(kuka_deviceTime[kuka_index]-initial_device_time_kuka); - device_time_offset_FT.push_back(FT_deviceTime[FT_index]-initial_device_time_FT); - FT_index++; - kuka_index++; - itkuka_local_request_time++; - itFT_local_request_time++; - device_type.push_back(2); - } - } - - - // // copy rest of kuka_local_request_time array - // while ( itkuka_local_request_time != itkuka_local_request_timeEnd ){ - // FT_index = FT_state_size-2; - // local_request_time_offset.push_back( *itkuka_local_request_time - initial_local_time); - // local_receive_time_offset_FT.push_back(FT_local_receive_time[FT_index] - initial_local_time); - // local_receive_time_offset_kuka.push_back(kuka_local_receive_time[kuka_index] - initial_local_time); - // device_time_offset_kuka.push_back(kuka_deviceTime[kuka_index]-initial_device_time_kuka); - // device_time_offset_FT.push_back(FT_deviceTime[FT_index]-initial_device_time_FT); - // kuka_index++; - // itkuka_local_request_time++; - // device_type.push_back(0); - + int index = kuka_index + FT_index; + // // while(kuka_local_request_time(kuka_index) > FT_local_request_time(FT_index)){ + // // itFT_local_request_time++; + // // FT_index++; + // // } + + // while ( kuka_index < kuka_state_size && FT_index < FT_state_size ) + // { + // if ( kuka_local_request_time(kuka_index) < FT_local_request_time(FT_index) ){ + // local_request_time_offset(index) = kuka_local_request_time(kuka_index); + // local_receive_time_offset_FT(index) = FT_local_receive_time(FT_index); + // local_receive_time_offset_kuka(index) = kuka_local_receive_time(kuka_index); + // device_time_offset_kuka (index) = kuka_device_time(kuka_index); + // device_time_offset_FT(index) = FT_deviceTime(FT_index); + // device_type(index) = 0; + // kuka_index++; + + + // } else if( kuka_local_request_time(kuka_index) > FT_local_request_time(FT_index)) { + // local_request_time_offset(index) = FT_local_request_time(FT_index); + // local_receive_time_offset_FT(index) = FT_local_receive_time(FT_index); + // local_receive_time_offset_kuka(index) = kuka_local_receive_time(kuka_index); + // device_time_offset_kuka(index) = kuka_device_time(kuka_index); + // device_time_offset_FT(index) = FT_deviceTime(FT_index); + // device_type(index) = 1; + // FT_index++; + // } else { + // local_request_time_offset(index) = FT_local_request_time(FT_index); + // local_receive_time_offset_FT(index) = FT_local_receive_time(FT_index); + // local_receive_time_offset_kuka(index) = kuka_local_receive_time(kuka_index); + // device_time_offset_kuka(index)= kuka_device_time(kuka_index); + // device_time_offset_FT(index) = FT_deviceTime(FT_index); + // device_type(index) = 2; + // FT_index++; + // kuka_index++; + // } + // index++; // } - // // copy rest of FT_local_request_time array - // while ( itFT_local_request_time != itFT_local_request_timeEnd ) { - // kuka_index = kuka_state_size-2; - // local_request_time_offset.push_back( *itFT_local_request_time - initial_local_time ); - // local_receive_time_offset_FT.push_back(FT_local_receive_time[FT_index] - initial_local_time); - // local_receive_time_offset_kuka.push_back(kuka_local_receive_time[kuka_index] - initial_local_time); - // device_time_offset_kuka.push_back(kuka_deviceTime[kuka_index]-initial_device_time_kuka); - // device_time_offset_FT.push_back(FT_deviceTime[FT_index]-initial_device_time_FT); - // FT_index++; - // itFT_local_request_time++; - // device_type.push_back(1); - // // std::cout<<"kuka_local_receive_time: " << kuka_index <<" "< device_time, - std::vector local_request_time, - std::vector local_receive_time){ + grl::VectorXd device_time, + grl::VectorXd local_request_time, + grl::VectorXd local_receive_time){ - int64_t initial_local_time = local_request_time[0]; - int64_t initial_device_time = device_time[0]; std::size_t size = device_time.size(); + auto initial_local_time = local_request_time(0); + auto initial_device_time = device_time(0); + local_request_time = local_request_time - initial_local_time * grl::VectorXd::Ones(size); + local_receive_time = local_receive_time - initial_local_time * grl::VectorXd::Ones(size); + device_time = device_time - initial_device_time * grl::VectorXd::Ones(size); std::ofstream fs; // create a name for the file output @@ -206,10 +207,10 @@ void writetoCSV( << "Y" << std::endl; for(int i=0; i Date: Wed, 14 Feb 2018 00:31:00 -0500 Subject: [PATCH 045/102] Improvements for ploting data and comments for java --- include/grl/kuka/Kuka.hpp | 6 +- include/grl/kuka/KukaFRIdriver.hpp | 7 +- include/grl/sensor/FusionTrackLogAndTrack.hpp | 8 +- include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp | 3 +- src/java/grl/src/grl/FRIMode.java | 63 ++- src/java/grl/src/grl/UDPManager.java | 108 ++-- src/java/grl/src/grl/UpdateConfiguration.java | 17 +- src/java/grl/src/grl/driver/GRL_Driver.java | 52 +- .../grl/src/grl/driver/ZMQ_SmartServoFRI.java | 36 +- test/CMakeLists.txt | 7 +- test/kukaiiwaURDF.h | 482 ++++++++++++++++++ test/readFRIFBTest.cpp | 154 ++++-- 12 files changed, 760 insertions(+), 183 deletions(-) create mode 100644 test/kukaiiwaURDF.h diff --git a/include/grl/kuka/Kuka.hpp b/include/grl/kuka/Kuka.hpp index 0585df8..4d6bcf7 100644 --- a/include/grl/kuka/Kuka.hpp +++ b/include/grl/kuka/Kuka.hpp @@ -250,9 +250,9 @@ class KukaUDP { std::string remotehost(std::get(params)); std::string rp(std::get(params)); short remoteport = boost::lexical_cast(rp); - std::cout << "using: " - << " " << localhost << " " << localport << " " << remotehost - << " " << remoteport << "\n"; + // std::cout << "using: " + // << " " << localhost << " " << localport << " " << remotehost + // << " " << remoteport << "\n"; boost::asio::ip::udp::socket s( io_service_, diff --git a/include/grl/kuka/KukaFRIdriver.hpp b/include/grl/kuka/KukaFRIdriver.hpp index f83e626..a2d6cb5 100644 --- a/include/grl/kuka/KukaFRIdriver.hpp +++ b/include/grl/kuka/KukaFRIdriver.hpp @@ -751,8 +751,8 @@ void saveToDisk() { const std::size_t MegaByte = 1024*1024; // If we write too large a flatbuffer - const std::size_t single_buffer_limit_bytes = 1024*MegaByte; - const std::size_t single_buffer_limit_states = 1350; + const std::size_t single_buffer_limit_bytes = 24*MegaByte; + const std::size_t single_buffer_limit_states = 1350000000; // run the primary update loop in a separate thread bool saveFileNow = false; @@ -763,7 +763,8 @@ void saveToDisk() // There is a flatbuffers file size limit of 2GB, but we use a conservative 512MB int buffsize = m_logFileBufferBuilderP->GetSize(); int statessize = m_KUKAiiwaStateBufferP->size(); - if( buffsize > single_buffer_limit_bytes || statessize > single_buffer_limit_states) + // if( buffsize > single_buffer_limit_bytes || statessize > single_buffer_limit_states) + if( buffsize > single_buffer_limit_bytes) { // save the file if we are over the limit saveFileNow = true; diff --git a/include/grl/sensor/FusionTrackLogAndTrack.hpp b/include/grl/sensor/FusionTrackLogAndTrack.hpp index bfcc541..4e7f44c 100644 --- a/include/grl/sensor/FusionTrackLogAndTrack.hpp +++ b/include/grl/sensor/FusionTrackLogAndTrack.hpp @@ -439,8 +439,8 @@ class FusionTrackLogAndTrack : public std::enable_shared_from_this deviceSerialNumbers; try { @@ -498,8 +498,8 @@ class FusionTrackLogAndTrack : public std::enable_shared_from_thisGetSize() > single_buffer_limit_bytes || - m_KUKAiiwaFusionTrackMessageBufferP->size() > single_buffer_limit_messages) + if(m_logFileBufferBuilderP->GetSize() > single_buffer_limit_bytes ) + // || m_KUKAiiwaFusionTrackMessageBufferP->size() > single_buffer_limit_messages) { // save the file if we are over the limit saveFileNow = true; diff --git a/include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp b/include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp index 7b78c95..e5f6816 100644 --- a/include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp +++ b/include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp @@ -171,7 +171,7 @@ class KukaVrepPlugin : public std::enable_shared_from_this { kukaDriverP_->construct(); // Default to joint servo mode for commanding motion kukaDriverP_->set(flatbuffer::ArmState::MoveArmJointServo); - std::cout << "KUKA COMMAND MODE: " << std::get(params) << "\n"; + // std::cout << "KUKA COMMAND MODE: " << std::get(params) << "\n"; initHandles(); } @@ -328,7 +328,6 @@ class KukaVrepPlugin : public std::enable_shared_from_this { vrepMeasuredRobotArmDriverP_->setState(measuredArmState); } - } /// @todo if there aren't real limits set via the kuka model already then implement me diff --git a/src/java/grl/src/grl/FRIMode.java b/src/java/grl/src/grl/FRIMode.java index e772d2a..35d6118 100755 --- a/src/java/grl/src/grl/FRIMode.java +++ b/src/java/grl/src/grl/FRIMode.java @@ -22,15 +22,17 @@ import com.kuka.task.ITaskLogger; /** - * + * * Activate FRI mode in a separate thread * */ + // Provide a Runnable object. The Runnable interface defines a single method, run, meant to contain the code executed in the thread. + // The Runnable object is passed to the Thread constructor, public class FRIMode implements Runnable { - private LBR _lbr; + private LBR _lbr; + - private AbstractMotionControlMode _activeMotionControlMode; private FRISession _friSession = null; @@ -41,13 +43,13 @@ public class FRIMode implements Runnable { private volatile boolean isEnableEnded; private volatile boolean stop = false; private volatile boolean timedOut = false; - + IMotionContainer currentMotion = null; ITaskLogger _logger = null; int iter = 0; /** - * + * * @param flangeAttachment * @param maxAllowedJoints * @param minAllowedJoints @@ -61,6 +63,12 @@ public FRIMode(LBR lbr, String hostName, int sendPeriodMillisec) { _friConfiguration = FRIConfiguration.createRemoteConfiguration(_lbr, _hostName); _friConfiguration.setSendPeriodMilliSec(sendPeriodMillisec); + /** + * Using an instance of the class FRISession, the FRI connection between the + * robot controller and external system is opened and Monitor mode is automatically activated. + * If Monitor mode is activated, the connection quality is continuously determined + * and evaluated. + */ if(_friSession == null) _friSession = new FRISession(_friConfiguration); //_motionOverlay = new FRIJointOverlay(_friSession); } @@ -72,21 +80,28 @@ public void setLogger(ITaskLogger logger) { public boolean isCommandingWaitOrActive() { boolean ret; + /** + * The method getFRIChannelInformation() can be used to poll the following information + * and save it in a variable of type FRIChannelInformation. + * Type: FRIChannelInformation + * Variable in which the data for the FRI connection and FRI state are saved + */ + synchronized (this) { ret = _friSession.getFRIChannelInformation().getFRISessionState().compareTo(FRISessionState.COMMANDING_ACTIVE) != 0 && _friSession.getFRIChannelInformation().getFRISessionState().compareTo(FRISessionState.COMMANDING_WAIT) != 0; } - + return !ret; } - + public String getQualityString() { return _friSession.getFRIChannelInformation().getQuality().toString(); } - + /** - * + * * @param setControlMode */ public void setControlMode(AbstractMotionControlMode teachModeControlMode) { @@ -95,7 +110,7 @@ public void setControlMode(AbstractMotionControlMode teachModeControlMode) { _activeMotionControlMode = teachModeControlMode; } } - + public void setFRIConfiguration(FRIConfiguration friConfguration){ _friConfiguration = friConfguration; } @@ -106,10 +121,10 @@ public void enable() { useHandGuidingMotion = true; } } - + /** * Tell the hand guiding motion (teach mode) to stop - * and the thread exits. Make sure + * and the thread exits. Make sure * isEnableEnded returns true before calling this! * @return false on failure; true on successfully starting the shutdown process */ @@ -118,36 +133,36 @@ public synchronized boolean stop(){ stop = true; if(currentMotion !=null) currentMotion.cancel(); - + return true; } - + public synchronized boolean isTimedOut(){ return timedOut; } - + /** - * + * * Cancel the motion, thread stays in existence. */ public boolean cancel(){ //warn("Cancel received"); - + synchronized (this) { useHandGuidingMotion = false; - + if(currentMotion !=null) currentMotion.cancel(); } return true; //warn("Cancel complete"); } - + /** * @brief true there are no outstanding calls made to enable() and you can switch modes - * + * * @see enable() - * + * * The sunrise HandGuidingMotion mode has an API * quirk where you need to push the physical button * to end the hand guiding mode. This tells you if @@ -178,7 +193,7 @@ public void run() { useHandGuidingMotion = false; } _motionOverlay = null; - + while(!stop && !timedOut) { //warn("Starting new hand guiding motion"); @@ -201,7 +216,7 @@ public void run() { } continue; } - + warn("creating FRI Joint Overlay " + useHandGuidingMotion); isEnableEnded = false; _motionOverlay = new FRIJointOverlay(_friSession); @@ -227,7 +242,7 @@ public void run() { _motionOverlay = new FRIJointOverlay(_friSession); currentMotion = _lbr.move(positionHold(_activeMotionControlMode, -1, TimeUnit.SECONDS).addMotionOverlay(_motionOverlay)); _logger.info("FRI Joint Overlay ended..."); - + } catch (TimeoutException e) { //_logger.error("FRISession timed out, closing..."); //e.printStackTrace(); diff --git a/src/java/grl/src/grl/UDPManager.java b/src/java/grl/src/grl/UDPManager.java index 9816806..00ba910 100755 --- a/src/java/grl/src/grl/UDPManager.java +++ b/src/java/grl/src/grl/UDPManager.java @@ -7,15 +7,16 @@ import java.net.*; public class UDPManager { - + + // This class represents a socket for sending and receiving datagram packets. DatagramSocket socket = null; ITaskLogger logger; String _Remote_IP; int _Remote_Port; - + // This class represents an Internet Protocol (IP) address. InetAddress _address_send = null; - + int statesLength = 0; long message_counter = 0; long noMessageCounter = 0; @@ -33,59 +34,64 @@ public class UDPManager { long lastMessageStartTime; long lastMessageElapsedTime; long lastMessageTimeoutMilliseconds = 1000; - + int retriesAllowed = 3; int retriesAttempted = 0; - + public UDPManager(String laptopIP, String laptopPort, ITaskLogger errorlogger) { super(); this.logger = errorlogger; _Remote_IP = laptopIP; _Remote_Port = Integer.parseInt(laptopPort); - + try { _address_send = InetAddress.getByName(_Remote_IP); } catch (UnknownHostException e) { logger.error("Could not create InetAddress for sending"); } - + } /** * Blocks until a connection is established or stop() is called. - * + * * @return error code: false on success, otherwise failure (or told to stop) - * @throws UnknownHostException + * @throws UnknownHostException */ public boolean connect() { logger.info("Waiting for connection initialization..."); - + try { socket = new DatagramSocket(); } catch (SocketException e1) { logger.info("failed to create socket."); } - - + + try { + // Enable/disable SO_TIMEOUT with the specified timeout, in milliseconds. + // With this option set to a non-zero timeout, a read() call on the InputStream associated with this Socket will block for only this amount of time. If the timeout expires, a java.net.SocketTimeoutException is raised, socket.setSoTimeout(100); } catch (SocketException e1) { logger.error("UDPManager failed to set socket timeout"); } - + startTime = System.currentTimeMillis(); elapsedTime = 0L; grl.flatbuffer.KUKAiiwaStates newKUKAiiwaStates = null; - int newStatesLength = 0; - + int newStatesLength = 0; + boolean connectionEstablished = false; - + while(newStatesLength<1 && newKUKAiiwaStates == null){ - + // Datagram packets are used to implement a connectionless packet delivery service. + // Constructs a DatagramPacket for receiving packets of length length. + // recBuf - buffer for holding the incoming datagram. + // DatagramPacket packet = new DatagramPacket(recBuf, recBuf.length); - + // continues sending dummy messages until the server receives the address of this machine and sends a message back while (!connectionEstablished){ connectionEstablished = preConnect(); @@ -94,14 +100,14 @@ public boolean connect() { return true; // asked to exit } } - - if(packet.getLength() > 0){ + if(packet.getLength() > 0){ + // Wraps a byte array into a buffer. bb = ByteBuffer.wrap(recBuf); - + newKUKAiiwaStates = grl.flatbuffer.KUKAiiwaStates.getRootAsKUKAiiwaStates(bb); newStatesLength = newKUKAiiwaStates.statesLength(); - + } if (stop) { @@ -109,12 +115,12 @@ public boolean connect() { return true; // asked to exit } } - + _currentKUKAiiwaStates = newKUKAiiwaStates; statesLength = newStatesLength; logger.info("States initialized..."); - + startTime = System.currentTimeMillis(); lastMessageStartTime = startTime; lastMessageElapsedTime = System.currentTimeMillis() - lastMessageStartTime; @@ -122,14 +128,14 @@ public boolean connect() { return false; // no error } - + /// @return true successfully sent and received a message, false otherwise private boolean preConnect() { // Dummy message to send to Remote pc (server), in order for the server to know the address of the client (this machine) /// @todo Should probably just make this a real flatbuffers init message too String dummyMessage = "Hi"; - + DatagramPacket packetSend= new DatagramPacket(dummyMessage.getBytes(), dummyMessage.getBytes().length, _address_send, _Remote_Port); try { @@ -142,7 +148,7 @@ private boolean preConnect() socket.receive(packet); return true; } catch (SocketTimeoutException e) { - // TimeOut reached, continue sending until we receive something + // TimeOut reached, continue sending until we receive something return false; } catch (IOException e) { // Could not receive packet @@ -152,24 +158,24 @@ private boolean preConnect() /** * Blocks until a connection is re-established or stop() is called. - * + * * @return error code: false on success, otherwise failure (or told to stop) - * @throws IOException + * @throws IOException */ - + public boolean sendMessage(byte[] msg, int size) throws IOException { DatagramPacket packet= new DatagramPacket(msg, size, _address_send , _Remote_Port ); socket.send(packet); return true; } - + public grl.flatbuffer.KUKAiiwaState waitForNextMessage() { boolean haveNextMessage = false; while(!stop && !haveNextMessage) { - + DatagramPacket packet = new DatagramPacket(recBuf, recBuf.length); try { socket.receive(packet); @@ -178,9 +184,9 @@ public grl.flatbuffer.KUKAiiwaState waitForNextMessage() message_counter+=1; bb = ByteBuffer.wrap(recBuf); - + _currentKUKAiiwaStates = grl.flatbuffer.KUKAiiwaStates.getRootAsKUKAiiwaStates(bb, _currentKUKAiiwaStates); - + if(_currentKUKAiiwaStates.statesLength()>0) { // initialize the fist state grl.flatbuffer.KUKAiiwaState tmp = _currentKUKAiiwaStates.states(0); @@ -194,13 +200,13 @@ public grl.flatbuffer.KUKAiiwaState waitForNextMessage() _previousKUKAiiwaState = _currentKUKAiiwaState; _currentKUKAiiwaState = tmp; } - + if (_currentKUKAiiwaState == null) { noMessageCounter+=1; logger.error("Missing current state message!"); continue; } - + haveNextMessage=true; noMessageCounter = 0; lastMessageStartTime = System.currentTimeMillis(); @@ -222,22 +228,22 @@ public grl.flatbuffer.KUKAiiwaState waitForNextMessage() preConnect(); } } - + } - - return _currentKUKAiiwaState; + + return _currentKUKAiiwaState; } - + public grl.flatbuffer.KUKAiiwaState getCurrentMessage(){ return _currentKUKAiiwaState; } - + public grl.flatbuffer.KUKAiiwaState getPrevMessage(){ return _previousKUKAiiwaState; } - - + + public boolean isStop() { return stop; } @@ -249,14 +255,14 @@ public void setStop(boolean stop) { socket.close(); logger.error("socket closed"); this.stop = stop; - + } } - + public void stop(){ setStop(true); } - + protected void finalize() { try { logger.error("Trying to close socket"); @@ -269,9 +275,9 @@ protected void finalize() { } } - - - - - + + + + + } diff --git a/src/java/grl/src/grl/UpdateConfiguration.java b/src/java/grl/src/grl/UpdateConfiguration.java index 583254e..cac8213 100755 --- a/src/java/grl/src/grl/UpdateConfiguration.java +++ b/src/java/grl/src/grl/UpdateConfiguration.java @@ -51,12 +51,12 @@ public void set_FRISession(FRISession _FRISession) { } private FRISession _FRISession = null; - + public UpdateConfiguration(LBR lbr, Tool flangeAttachment){ _lbr = lbr; _flangeAttachment = flangeAttachment; } - + /** * Create an End effector tool * @param link the Flatbuffer representing the tool @@ -81,7 +81,7 @@ public PhysicalObject linkObjectToPhysicalObject(grl.flatbuffer.LinkObject link link.inertia().mass(), centerOfMassInMillimeter); } - + /** * Sets the FRI Configuration and FRI Session stored by the UpdateConfiguration object * @param friConfigBuf flatbuffer containing fri settings @@ -95,11 +95,18 @@ public boolean FRIflatbufferToConfigAndSession (grl.flatbuffer.FRI friConfigBuf, if(friConfigBuf !=null && friConfigBuf.sendPeriodMillisec() >0){ _FRIConfiguration.setSendPeriodMilliSec(friConfigBuf.sendPeriodMillisec()); + /** + * Defines the answer rate factor (type: int) + * The answer rate factor defines the rate at which the external + * system is to send data to the robot controller: + * Answer rate = answer rate factor*send rate + */ + _FRIConfiguration.setReceiveMultiplier(friConfigBuf.setReceiveMultiplier()); } - + _FRISession = new FRISession(_FRIConfiguration); - + return false; } diff --git a/src/java/grl/src/grl/driver/GRL_Driver.java b/src/java/grl/src/grl/driver/GRL_Driver.java index b23b3c6..104752c 100644 --- a/src/java/grl/src/grl/driver/GRL_Driver.java +++ b/src/java/grl/src/grl/driver/GRL_Driver.java @@ -166,7 +166,7 @@ public class GRL_Driver extends RoboticsAPIApplication // when we receive a message int message_counter = 0; int message_counter_since_last_mode_change = 0; - + private Lock configureSmartServoLock = new ReentrantLock(); // private FlexFellowIOGroup _flexFellowIOGroup; @@ -262,9 +262,9 @@ public void run() while (!stop && !_startStopUI.is_stopped() && _lbr.getSafetyState().getEmergencyStopInt()==EmergencyStop.INACTIVE) { message_counter+=1; _currentKUKAiiwaState = udpMan.waitForNextMessage(); - + //Some bug in this, just set _previousKUKAiiwaState = _currentKUKAiiwaState before looping/continue? - _previousKUKAiiwaState = udpMan.getPrevMessage(); + _previousKUKAiiwaState = udpMan.getPrevMessage(); ////////////////////////////////////////// @@ -419,13 +419,13 @@ else if (_currentKUKAiiwaState.armControlState().stateType() == grl.flatbuffer.A try{ if(_smartServoMotion == null){ // Initialize Smart servo the first time - getLogger().info("Initializing Smart Servo in " + getLogger().info("Initializing Smart Servo in " + grl.flatbuffer.EControlMode.name(_currentKUKAiiwaState.armConfiguration().controlMode())); switchSmartServoMotion(_currentKUKAiiwaState.armConfiguration().controlMode()); continue; }else if(_currentKUKAiiwaState.setArmConfiguration()){ //If change in mode requested - - //TODO: bug, _previousKUKAiiwaState & _currentKUKAiiwaState are same. + + //TODO: bug, _previousKUKAiiwaState & _currentKUKAiiwaState are same. /* getLogger().info("Change controlMode requested: " +grl.flatbuffer.EControlMode.name(_previousKUKAiiwaState.armConfiguration().controlMode()) +" -> "+grl.flatbuffer.EControlMode.name(_currentKUKAiiwaState.armConfiguration().controlMode()));*/ @@ -434,7 +434,7 @@ else if (_currentKUKAiiwaState.armControlState().stateType() == grl.flatbuffer.A } catch (Exception e) { - getLogger().error(e.getMessage()); + getLogger().error(e.getMessage()); getLogger().error("Exception in Starting/Switching SmartServo" ); continue; } @@ -501,7 +501,7 @@ else if (_currentKUKAiiwaState.armControlState().stateType() == grl.flatbuffer.A } else { getLogger().error("Couldn't issue motion command, smartServo motion was most likely reset. retrying..."); } - + } else if (_currentKUKAiiwaState.armControlState().stateType() == grl.flatbuffer.ArmState.StopArm) { _smartServoRuntime.stopMotion(); @@ -528,7 +528,7 @@ else if (_currentKUKAiiwaState.armControlState().stateType() == grl.flatbuffer.A if (currentMotion != null) currentMotion.cancel(); if(!cancelTeachMode()) continue; if(!cancelSmartServo()) continue; - + if(message_counter_since_last_mode_change % 500 == 0) getLogger().info("StartArm mode active, connection established!\nHolding Position while waiting for mode change...\n"); PositionControlMode controlMode = new PositionControlMode(); @@ -540,7 +540,7 @@ else if (_currentKUKAiiwaState.armControlState().stateType() == grl.flatbuffer.A System.out.println("Unsupported Mode! stopping"); stop = true; } - + /////////////////////////////////////////////////////////////////////////// /// Sending commands back to the C++ interface here @@ -590,7 +590,7 @@ else if (_currentKUKAiiwaState.armControlState().stateType() == grl.flatbuffer.A } } - + {_previousKUKAiiwaState = _currentKUKAiiwaState;} } // end primary while loop @@ -721,7 +721,7 @@ private boolean isSameControlMode(IMotionControlMode kukacm, byte controlMode) { return roscmname.equals(kukacmname); } - + /** * Initialize the appropriate control mode based on passed parameters * @@ -735,7 +735,7 @@ public AbstractMotionControlMode getMotionControlMode(byte controlMode, boolean if(controlMode == grl.flatbuffer.EControlMode.POSITION_CONTROL_MODE){ mcm = new PositionControlMode(); } else if(controlMode==grl.flatbuffer.EControlMode.CART_IMP_CONTROL_MODE){ - + if(defaultParams){ // TODO: Get missing default params from processData /// @note setMaxCartesianVelocity STOPS THE ROBOT ABOVE THAT VELOCITY RATHER THAN CAPPING THE VELOCITY @@ -745,14 +745,14 @@ public AbstractMotionControlMode getMotionControlMode(byte controlMode, boolean cicm.setNullSpaceDamping(0.5); cicm.setNullSpaceStiffness(2); cicm.setMaxControlForce(200, 200, 200, 200, 200, 200, true); - + cicm.parametrize(CartDOF.X).setStiffness(_processDataManager.get_CartesianImpedenceStiffnessX()); cicm.parametrize(CartDOF.Y).setStiffness(_processDataManager.get_CartesianImpedenceStiffnessY()); cicm.parametrize(CartDOF.Z).setStiffness(_processDataManager.get_CartesianImpedenceStiffnessZ()); cicm.parametrize(CartDOF.A).setStiffness(_processDataManager.get_CartesianImpedenceStiffnessA()); cicm.parametrize(CartDOF.B).setStiffness(_processDataManager.get_CartesianImpedenceStiffnessB()); cicm.parametrize(CartDOF.C).setStiffness(_processDataManager.get_CartesianImpedenceStiffnessC()); - + cicm.parametrize(CartDOF.X).setDamping(_processDataManager.get_CartesianImpedenceDampingX()); cicm.parametrize(CartDOF.Y).setDamping(_processDataManager.get_CartesianImpedenceDampingY()); cicm.parametrize(CartDOF.Z).setDamping(_processDataManager.get_CartesianImpedenceDampingZ()); @@ -762,9 +762,9 @@ public AbstractMotionControlMode getMotionControlMode(byte controlMode, boolean mcm = cicm; } else{ - + CartesianImpedanceControlMode cicm = new CartesianImpedanceControlMode(); - + cicm.setMaxCartesianVelocity(_currentKUKAiiwaState.armConfiguration().setCartImpedance().maxCartesianVelocity().position().x(), _currentKUKAiiwaState.armConfiguration().setCartImpedance().maxCartesianVelocity().position().y(), _currentKUKAiiwaState.armConfiguration().setCartImpedance().maxCartesianVelocity().position().z(), @@ -785,14 +785,14 @@ public AbstractMotionControlMode getMotionControlMode(byte controlMode, boolean _currentKUKAiiwaState.armConfiguration().setCartImpedance().maxControlForce().rotation().r3(), _currentKUKAiiwaState.armConfiguration().setCartImpedance().maxControlForce().rotation().r2(), _currentKUKAiiwaState.armConfiguration().setCartImpedance().maxControlForce().rotation().r1(), true); - + cicm.parametrize(CartDOF.X).setStiffness(_currentKUKAiiwaState.armConfiguration().setCartImpedance().stiffness().position().x()); cicm.parametrize(CartDOF.Y).setStiffness(_currentKUKAiiwaState.armConfiguration().setCartImpedance().stiffness().position().y()); cicm.parametrize(CartDOF.Z).setStiffness(_currentKUKAiiwaState.armConfiguration().setCartImpedance().stiffness().position().z()); cicm.parametrize(CartDOF.A).setStiffness(_currentKUKAiiwaState.armConfiguration().setCartImpedance().stiffness().rotation().r3()); cicm.parametrize(CartDOF.B).setStiffness(_currentKUKAiiwaState.armConfiguration().setCartImpedance().stiffness().rotation().r2()); cicm.parametrize(CartDOF.C).setStiffness(_currentKUKAiiwaState.armConfiguration().setCartImpedance().stiffness().rotation().r1()); - + cicm.parametrize(CartDOF.X).setDamping(_currentKUKAiiwaState.armConfiguration().setCartImpedance().damping().position().x()); cicm.parametrize(CartDOF.Y).setDamping(_currentKUKAiiwaState.armConfiguration().setCartImpedance().damping().position().y()); cicm.parametrize(CartDOF.Z).setDamping(_currentKUKAiiwaState.armConfiguration().setCartImpedance().damping().position().z()); @@ -811,12 +811,12 @@ public AbstractMotionControlMode getMotionControlMode(byte controlMode, boolean //cicm.parametrize(CartDOF.X).setStiffness(stiffnessX); //cicm.parametrize(CartDOF.Y).setStiffness(stiffnessY); //cicm.parametrize(CartDOF.Z).setStiffness(stiffnessZ); - + mcm = jicm; } else{ JointImpedanceControlMode jicm = new JointImpedanceControlMode(_lbr.getJointCount()); - + if(_lbr.getJointCount() != _currentKUKAiiwaState.armConfiguration().setJointImpedance().stiffnessLength()) { getLogger().info("stiffness/damping vector size is not correct. Set default values for now"); @@ -832,7 +832,7 @@ public AbstractMotionControlMode getMotionControlMode(byte controlMode, boolean damping[k] = _currentKUKAiiwaState.armConfiguration().setJointImpedance().damping(k); } jicm.setStiffness(stiffness); - jicm.setDamping(damping); + jicm.setDamping(damping); } mcm = jicm; } @@ -867,13 +867,15 @@ private void switchSmartServoMotion(byte controlMode) { if(_smartServoMotion!=null){ if(isSameControlMode(_smartServoMotion.getMode(), controlMode)) { // We can just change the parameters if the control strategy is the same. if (!(_smartServoMotion.getMode() instanceof PositionControlMode)) { // We are in PositioControlMode and the request was for the same mode, there are no parameters to change. - getLogger().info("Changing parameters of Smart Servo in " + getLogger().info("Changing parameters of Smart Servo in " + grl.flatbuffer.EControlMode.name(controlMode)); + // With the changeControlModeSettings(...) method, it is possible to change the controller parameters with which the + // servo motion was started. The control type itself cannot be changed during the servo motion. _smartServoMotion.getRuntime().changeControlModeSettings(getMotionControlMode(controlMode, defaultParams)); configureSmartServoLock.unlock(); return; } - } + } getLogger().info("switching controlMode requested: " + _smartServoMotion.getMode().getClass().getSimpleName() +" -> "+grl.flatbuffer.EControlMode.name(controlMode)); @@ -945,7 +947,7 @@ private void switchSmartServoMotion(byte controlMode) { configureSmartServoLock.unlock(); return; } - + configureSmartServoLock.unlock(); } diff --git a/src/java/grl/src/grl/driver/ZMQ_SmartServoFRI.java b/src/java/grl/src/grl/driver/ZMQ_SmartServoFRI.java index bf591f9..63a2c1e 100755 --- a/src/java/grl/src/grl/driver/ZMQ_SmartServoFRI.java +++ b/src/java/grl/src/grl/driver/ZMQ_SmartServoFRI.java @@ -25,6 +25,7 @@ public class ZMQ_SmartServoFRI extends RoboticsAPIApplication { private Controller _lbrController; + // Robot instance in the application private LBR _lbr; private String _hostName; private String _controllingLaptopIPAddress; @@ -44,7 +45,7 @@ public void initialize() // *** change next line to the KUKA address and Port Number *** // ********************************************************************** _controllingLaptopIPAddress = "tcp://172.31.1.100:30010"; - + // FIXME: Set proper Weights or use the plugin feature double[] translationOfTool = @@ -61,22 +62,27 @@ public void initialize() public void run() { // Configure and start FRI session + /** + * The FRI connection to an external system is configured via the static method + * createRemoteConfiguration(robot ,IP address). The method belongs to the class FRIConfiguration. + **/ FRIConfiguration friConfiguration = FRIConfiguration.createRemoteConfiguration(_lbr, _hostName); + // Define the send rate (type: int, unit ms) friConfiguration.setSendPeriodMilliSec(4); FRISession friSession = new FRISession(friConfiguration); - + // TODO: remove default start pose // move do default start pose _toolAttachedToLBR.move(ptp(Math.toRadians(10), Math.toRadians(10), Math.toRadians(10), Math.toRadians(-90), Math.toRadians(10), Math.toRadians(10),Math.toRadians(10))); - + // Prepare ZeroMQ context and dealer ZMQ.Context context = ZMQ.context(1); ZMQ.Socket subscriber = context.socket(ZMQ.DEALER); subscriber.connect(_controllingLaptopIPAddress); subscriber.setRcvHWM(1000000); - - + + JointPosition initialPosition = new JointPosition( _lbr.getCurrentJointPosition()); @@ -89,27 +95,27 @@ public void run() aSmartServoMotion.setMinimumTrajectoryExecutionTime(20e-3); _toolAttachedToLBR.getDefaultMotionFrame().moveAsync(aSmartServoMotion); - + // Fetch the Runtime of the Motion part theSmartServoRuntime = aSmartServoMotion.getRuntime(); - + // create an JointPosition Instance, to play with JointPosition destination = new JointPosition( _lbr.getJointCount()); - - + + byte [] data = subscriber.recv(); ByteBuffer bb = ByteBuffer.wrap(data); JointState jointState = JointState.getRootAsJointState(bb); - + // move to start pose //_lbr.move(ptp(jointState.position(0), jointState.position(1), jointState.position(2), jointState.position(3), jointState.position(4), jointState.position(5), jointState.position(6))); //.setJointAccelerationRel(acceleration)); - - + + // Receive Flat Buffer and Move to Position // TODO: make into while loop and add exception to exit loop // TODO: add a message that we send to the driver with data log strings @@ -120,7 +126,7 @@ public void run() bb = ByteBuffer.wrap(data); jointState = JointState.getRootAsJointState(bb); - + for (int k = 0; k < destination.getAxisCount(); ++k) { destination.set(k, jointState.position(k)); @@ -134,7 +140,7 @@ public void run() //_lbr.moveAsync(ptp(jointState.position(0), jointState.position(1), jointState.position(2), jointState.position(3), jointState.position(4), jointState.position(5), jointState.position(6))); //.setJointAccelerationRel(acceleration)); } - + // done subscriber.close(); context.term(); @@ -144,7 +150,7 @@ public void run() /** * main. - * + * * @param args * args */ diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 2773c39..f3788e1 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -95,9 +95,12 @@ if(TARGET KukaFRIClient OR FRI_Client_SDK_Cpp_FOUND) basis_add_executable(FlatbuffersExampleWithKukaIiwaTest.cpp)# ${GRL_FLATBUFFERS_OUTPUTS}) basis_target_link_libraries(FlatbuffersExampleWithKukaIiwaTest ${Boost_LIBRARIES} ${Boost_REGEX_LIBRARY} ${FRI_Client_SDK_Cpp_LIBRARIES} ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} KukaFRIClient ${FLATBUFFERS_STATIC_LIB}) - basis_add_executable(readFRIFBTest.cpp)# ${GRL_FLATBUFFERS_OUTPUTS}) + basis_add_executable(readFRIFBTest.cpp) basis_target_link_libraries(readFRIFBTest ${Boost_LIBRARIES} ${Boost_REGEX_LIBRARY} ${FRI_Client_SDK_Cpp_LIBRARIES} ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} KukaFRIClient ${FLATBUFFERS_STATIC_LIB} -lboost_iostreams -lboost_system -lboost_filesystem) -endif() + + # basis_add_executable(KukaPoseEstimationTest.cpp)# ${GRL_FLATBUFFERS_OUTPUTS}) + # basis_target_link_libraries(KukaPoseEstimationTest ${Boost_LIBRARIES} ${Boost_REGEX_LIBRARY} ${FRI_Client_SDK_Cpp_LIBRARIES} ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} KukaFRIClient ${FLATBUFFERS_STATIC_LIB} -lboost_iostreams -lboost_system -lboost_filesystem) + endif() if(CERES_FOUND OR USE_INTERNAL_CERES) diff --git a/test/kukaiiwaURDF.h b/test/kukaiiwaURDF.h new file mode 100644 index 0000000..652f0a4 --- /dev/null +++ b/test/kukaiiwaURDF.h @@ -0,0 +1,482 @@ +/* Copyright 2015-2017 CNRS-UM LIRMM, CNRS-AIST JRL + * + * This file is part of mc_rbdyn_urdf. + * + * mc_rbdyn_urdf is free software: you can redistribute it and/or modify + * it under the terms of the GNU Lesser General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * mc_rbdyn_urdf is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public License + * along with mc_rbdyn_urdf. If not, see . + */ +/** + * THIS IS A TEMPORAL FILE, AFTER FINISHING THE TEST, REMOVE IT. + * @CHUNTING + */ +#pragma once + +#include +#include + +#include "RBDyn/FK.h" + + +std::string XYZSarmUrdf( +R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + /iiwa + + + + + Gazebo/Grey + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Grey + 0.2 + 0.2 + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + +)"); + +namespace mc_rbdyn_urdf +{ +bool operator==( const Geometry::Mesh& m1, const Geometry::Mesh& m2) +{ + return m1.scale == m2.scale && m1.filename == m2.filename; +} + +bool operator==( const Geometry::Box& b1, const Geometry::Box& b2) +{ + return b1.size == b2.size; +} + +bool operator==( const Geometry::Sphere& b1, const Geometry::Sphere& b2) +{ + return b1.radius == b2.radius; +} + +bool operator==( const Geometry::Cylinder& b1, const Geometry::Cylinder& b2) +{ + return b1.radius == b2.radius && b1.length == b2.length; +} + +bool operator==(const Geometry& g1, const Geometry& g2) +{ + return g1.type == g2.type && g1.data == g2.data; +} +bool operator==(const Visual& v1, const Visual& v2) { + return v1.name == v2.name && v1.origin == v2.origin && v1.geometry == v2.geometry; +} +} /* mc_rbdyn_urdf */ + + +const double TOL = 1e-6; \ No newline at end of file diff --git a/test/readFRIFBTest.cpp b/test/readFRIFBTest.cpp index 1b8f2b1..0bbe02f 100644 --- a/test/readFRIFBTest.cpp +++ b/test/readFRIFBTest.cpp @@ -10,15 +10,14 @@ #include "grl/time.hpp" - #include #include #include #include std::string foldname = "/home/chunting/src/V-REP_PRO_EDU_V3_4_0_Linux/"; -std::string kukaBinaryfile = foldname + "2018_02_02_18_21_43_Kukaiiwa.iiwa"; -std::string fusiontrackBinaryfile = foldname + "2018_02_02_18_21_58_FusionTrack.flik"; +std::string kukaBinaryfile = foldname + "2018_02_13_19_06_04_Kukaiiwa.iiwa"; +std::string fusiontrackBinaryfile = foldname + "2018_02_13_19_05_49_FusionTrack.flik"; std::string CSVfilename = foldname + current_date_and_time_string() + "_FTKUKA.csv"; std::string FT_CSVfilename = foldname + current_date_and_time_string() + "_FT.csv"; std::string KUKA_CSVfilename = foldname + current_date_and_time_string() + "_KUKA.csv"; @@ -53,58 +52,110 @@ void writetoCSVforFusionTrackKukaiiwa() { auto FT_states = FusionTrackStatesRoot->states(); std::size_t FT_state_size = FT_states->size(); std::cout<< "------FusionTrack State Size: "<< FT_state_size << std::endl; - grl::VectorXd FT_deviceTime = grl:: getTimeStamp(FusionTrackStatesRoot, grl::fusiontracker_tag(), grl::TimeType::device_time); + grl::VectorXd FT_device_time = grl:: getTimeStamp(FusionTrackStatesRoot, grl::fusiontracker_tag(), grl::TimeType::device_time); grl::VectorXd FT_local_request_time = grl::getTimeStamp(FusionTrackStatesRoot, grl::fusiontracker_tag(), grl::TimeType::local_request_time); grl::VectorXd FT_local_receive_time = grl::getTimeStamp(FusionTrackStatesRoot, grl::fusiontracker_tag(), grl::TimeType::local_receive_time); - writetoCSV(FT_CSVfilename, FT_deviceTime, FT_local_request_time, FT_local_receive_time); + writetoCSV(FT_CSVfilename, FT_device_time, FT_local_request_time, FT_local_receive_time); - auto initial_local_time = std::min(FT_local_request_time(0), kuka_local_request_time(0)); + auto initial_local_time = std::min(FT_local_receive_time(0), kuka_local_receive_time(0)); auto initial_device_time_kuka = kuka_device_time(0); - auto initial_device_time_FT = FT_deviceTime(0); + auto initial_device_time_FT = FT_device_time(0); FT_local_request_time = FT_local_request_time - initial_local_time * grl::VectorXd::Ones(FT_state_size); FT_local_receive_time = FT_local_receive_time - initial_local_time * grl::VectorXd::Ones(FT_state_size); - FT_deviceTime = FT_deviceTime - initial_device_time_FT * grl::VectorXd::Ones(FT_state_size); + FT_device_time = FT_device_time - initial_device_time_FT * grl::VectorXd::Ones(FT_state_size); kuka_local_request_time = kuka_local_request_time - initial_local_time * grl::VectorXd::Ones(kuka_state_size); kuka_local_receive_time = kuka_local_receive_time - initial_local_time * grl::VectorXd::Ones(kuka_state_size); kuka_device_time = kuka_device_time - initial_device_time_kuka * grl::VectorXd::Ones(kuka_state_size); - std::size_t states_size = kuka_state_size+FT_state_size; - grl::VectorXd local_request_time_offset(states_size); - grl::VectorXd local_receive_time_offset_FT(states_size); - grl::VectorXd local_receive_time_offset_kuka(states_size); - grl::VectorXd device_time_offset_kuka(states_size); - grl::VectorXd device_time_offset_FT(states_size); - grl::VectorXd device_type(states_size); + // std::size_t states_size = kuka_state_size+FT_state_size; + // grl::VectorXd local_request_time_offset(states_size); + // grl::VectorXd local_receive_time_offset_FT(states_size); + // grl::VectorXd local_receive_time_offset_kuka(states_size); + // grl::VectorXd device_time_offset_kuka(states_size); + // grl::VectorXd FT_device_time_offset(states_size); + // grl::VectorXd device_type(states_size); // auto kuka_size = kuka_local_request_time.size(); int kuka_index = 0; int FT_index = 0; int index = kuka_index + FT_index; - // // while(kuka_local_request_time(kuka_index) > FT_local_request_time(FT_index)){ - // // itFT_local_request_time++; - // // FT_index++; - // // } + // while(kuka_local_request_time(kuka_index) > FT_local_request_time(FT_index)){ + // itFT_local_request_time++; + // FT_index++; + // } + std::ofstream fs; + // create a name for the file output + fs.open( CSVfilename, std::ofstream::out | std::ofstream::app); + // write the file headers + fs << "local_receive_time_offset_X," + << "FT_local_request_time," + << "KUKA_local_request_time," + // << "local_receive_time_offset_kuka," + << "FT_device_time_offset," + << "device_time_offset_kuka," + << "Y_FT," + << "Y_kuka" << std::endl; + + while ( kuka_index < kuka_state_size && FT_index < FT_state_size ) + { + if ( kuka_local_receive_time(kuka_index) < FT_local_receive_time(FT_index) ){ + // write the data to the output file + fs << kuka_local_receive_time(kuka_index) <<"," + <<"," + << kuka_local_request_time(kuka_index) << "," // A + // D + << "," + << kuka_device_time(kuka_index) <<"," // F + << "," + << kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index) << std::endl; // H + kuka_index++; + + + } else if( kuka_local_receive_time(kuka_index) > FT_local_receive_time(FT_index)) { + // write the data to the output file + fs << FT_local_receive_time(FT_index) <<"," + << FT_local_request_time(FT_index) << "," // A + <<"," // C + << FT_device_time(FT_index) << "," // D + << "," // E + << FT_device_time(FT_index) - FT_local_receive_time(FT_index) << "," + << "," + << std::endl; // H + FT_index++; + } else { + fs << FT_local_receive_time(FT_index) <<"," + << FT_local_request_time(FT_index) << "," + << kuka_local_request_time(kuka_index) << "," // A + << FT_device_time(FT_index) << "," // D + << kuka_device_time(kuka_index) <<"," + << FT_device_time(FT_index) - FT_local_receive_time(FT_index) << "," + << kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index) << std::endl; + FT_index++; + kuka_index++; + } + } + // while ( kuka_index < kuka_state_size && FT_index < FT_state_size ) // { - // if ( kuka_local_request_time(kuka_index) < FT_local_request_time(FT_index) ){ + // if ( kuka_local_receive_time(kuka_index) < FT_local_receive_time(FT_index) ){ // local_request_time_offset(index) = kuka_local_request_time(kuka_index); // local_receive_time_offset_FT(index) = FT_local_receive_time(FT_index); // local_receive_time_offset_kuka(index) = kuka_local_receive_time(kuka_index); // device_time_offset_kuka (index) = kuka_device_time(kuka_index); - // device_time_offset_FT(index) = FT_deviceTime(FT_index); + // FT_device_time_offset(index) = FT_device_time(FT_index); // device_type(index) = 0; // kuka_index++; - // } else if( kuka_local_request_time(kuka_index) > FT_local_request_time(FT_index)) { + // } else if( kuka_local_receive_time(kuka_index) > FT_local_receive_time(FT_index)) { // local_request_time_offset(index) = FT_local_request_time(FT_index); // local_receive_time_offset_FT(index) = FT_local_receive_time(FT_index); // local_receive_time_offset_kuka(index) = kuka_local_receive_time(kuka_index); // device_time_offset_kuka(index) = kuka_device_time(kuka_index); - // device_time_offset_FT(index) = FT_deviceTime(FT_index); + // FT_device_time_offset(index) = FT_device_time(FT_index); // device_type(index) = 1; // FT_index++; // } else { @@ -112,7 +163,7 @@ void writetoCSVforFusionTrackKukaiiwa() { // local_receive_time_offset_FT(index) = FT_local_receive_time(FT_index); // local_receive_time_offset_kuka(index) = kuka_local_receive_time(kuka_index); // device_time_offset_kuka(index)= kuka_device_time(kuka_index); - // device_time_offset_FT(index) = FT_deviceTime(FT_index); + // FT_device_time_offset(index) = FT_device_time(FT_index); // device_type(index) = 2; // FT_index++; // kuka_index++; @@ -128,7 +179,7 @@ void writetoCSVforFusionTrackKukaiiwa() { // // local_receive_time_offset_FT << (FT_local_receive_time(FT_index); // // local_receive_time_offset_kuka << (kuka_local_receive_time(kuka_index); // // device_time_offset_kuka << (kuka_device_time(kuka_index); - // // device_time_offset_FT << (FT_deviceTime(FT_index); + // // FT_device_time_offset << (FT_device_time(FT_index); // // kuka_index++; // // itkuka_local_request_time++; // // device_type << (0); @@ -142,7 +193,7 @@ void writetoCSVforFusionTrackKukaiiwa() { // // local_receive_time_offset_FT << (FT_local_receive_time(FT_index); // // local_receive_time_offset_kuka << (kuka_local_receive_time(kuka_index); // // device_time_offset_kuka << (kuka_device_time(kuka_index); - // // device_time_offset_FT << (FT_deviceTime(FT_index); + // // FT_device_time_offset << (FT_device_time(FT_index); // // FT_index++; // // itFT_local_request_time++; // // device_type << (1); @@ -153,36 +204,25 @@ void writetoCSVforFusionTrackKukaiiwa() { // auto real_size = local_request_time_offset.size(); // std::cout<< "------FT_KUKA: "<0) { + device_time_step = device_time(i) - device_time(i-1); + receive_time_step = local_receive_time(i) - local_receive_time(i-1); + } // write the data to the output file fs << local_request_time(i)<< "," // A << local_receive_time(i) <<"," // B << device_time(i) <<"," // C - << device_time(i) - local_request_time(i) << std::endl; //D + << device_time(i) - local_receive_time(i) << "," + << receive_request(i) << "," + << device_time_step << "," + << receive_time_step << "," + << std::endl; //D } fs.close(); } From 536e178121ac2ca18c24d67d627f591d4b7de58a Mon Sep 17 00:00:00 2001 From: Chunting Date: Thu, 15 Feb 2018 16:20:25 -0500 Subject: [PATCH 046/102] Record data properly --- test/readFRIFBTest.cpp | 156 ++++++++--------------------------------- 1 file changed, 31 insertions(+), 125 deletions(-) diff --git a/test/readFRIFBTest.cpp b/test/readFRIFBTest.cpp index 0bbe02f..0e73c62 100644 --- a/test/readFRIFBTest.cpp +++ b/test/readFRIFBTest.cpp @@ -56,10 +56,19 @@ void writetoCSVforFusionTrackKukaiiwa() { grl::VectorXd FT_local_request_time = grl::getTimeStamp(FusionTrackStatesRoot, grl::fusiontracker_tag(), grl::TimeType::local_request_time); grl::VectorXd FT_local_receive_time = grl::getTimeStamp(FusionTrackStatesRoot, grl::fusiontracker_tag(), grl::TimeType::local_receive_time); writetoCSV(FT_CSVfilename, FT_device_time, FT_local_request_time, FT_local_receive_time); + int kuka_index = 0; + int FT_index = 0; + // filter out the very beginning data, which can gurantee to record the data when the two devices work simultaniously. + while(kuka_local_receive_time(kuka_index) < FT_local_receive_time(FT_index)){ + kuka_index++; + } + while(kuka_local_receive_time(kuka_index) > FT_local_receive_time(FT_index) && kuka_index == 0){ + FT_index++; + } - auto initial_local_time = std::min(FT_local_receive_time(0), kuka_local_receive_time(0)); - auto initial_device_time_kuka = kuka_device_time(0); - auto initial_device_time_FT = FT_device_time(0); + auto initial_local_time = std::min(FT_local_receive_time(FT_index), kuka_local_receive_time(kuka_index)); + auto initial_device_time_kuka = kuka_device_time(kuka_index); + auto initial_device_time_FT = FT_device_time(FT_index); FT_local_request_time = FT_local_request_time - initial_local_time * grl::VectorXd::Ones(FT_state_size); FT_local_receive_time = FT_local_receive_time - initial_local_time * grl::VectorXd::Ones(FT_state_size); FT_device_time = FT_device_time - initial_device_time_FT * grl::VectorXd::Ones(FT_state_size); @@ -68,160 +77,57 @@ void writetoCSVforFusionTrackKukaiiwa() { kuka_local_receive_time = kuka_local_receive_time - initial_local_time * grl::VectorXd::Ones(kuka_state_size); kuka_device_time = kuka_device_time - initial_device_time_kuka * grl::VectorXd::Ones(kuka_state_size); - // std::size_t states_size = kuka_state_size+FT_state_size; - // grl::VectorXd local_request_time_offset(states_size); - // grl::VectorXd local_receive_time_offset_FT(states_size); - // grl::VectorXd local_receive_time_offset_kuka(states_size); - // grl::VectorXd device_time_offset_kuka(states_size); - // grl::VectorXd FT_device_time_offset(states_size); - // grl::VectorXd device_type(states_size); - - // auto kuka_size = kuka_local_request_time.size(); - - int kuka_index = 0; - int FT_index = 0; - int index = kuka_index + FT_index; - // while(kuka_local_request_time(kuka_index) > FT_local_request_time(FT_index)){ - // itFT_local_request_time++; - // FT_index++; - // } std::ofstream fs; // create a name for the file output fs.open( CSVfilename, std::ofstream::out | std::ofstream::app); - // write the file headers + // write the file headers fs << "local_receive_time_offset_X," << "FT_local_request_time," << "KUKA_local_request_time," - // << "local_receive_time_offset_kuka," << "FT_device_time_offset," << "device_time_offset_kuka," << "Y_FT," - << "Y_kuka" << std::endl; + << "Y_kuka" + << std::endl; + int64_t kuka_diff = kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index); + int64_t FT_diff = FT_device_time(FT_index) - FT_local_receive_time(FT_index); while ( kuka_index < kuka_state_size && FT_index < FT_state_size ) { if ( kuka_local_receive_time(kuka_index) < FT_local_receive_time(FT_index) ){ // write the data to the output file fs << kuka_local_receive_time(kuka_index) <<"," <<"," - << kuka_local_request_time(kuka_index) << "," // A - // D + << kuka_local_request_time(kuka_index) << "," << "," - << kuka_device_time(kuka_index) <<"," // F + << kuka_device_time(kuka_index) <<"," << "," - << kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index) << std::endl; // H + << kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index) - kuka_diff + << std::endl; kuka_index++; - - } else if( kuka_local_receive_time(kuka_index) > FT_local_receive_time(FT_index)) { // write the data to the output file fs << FT_local_receive_time(FT_index) <<"," - << FT_local_request_time(FT_index) << "," // A - <<"," // C - << FT_device_time(FT_index) << "," // D - << "," // E - << FT_device_time(FT_index) - FT_local_receive_time(FT_index) << "," + << FT_local_request_time(FT_index) << "," + <<"," + << FT_device_time(FT_index) << "," << "," - << std::endl; // H + << FT_device_time(FT_index) - FT_local_receive_time(FT_index) - FT_diff << "," + << std::endl; FT_index++; } else { fs << FT_local_receive_time(FT_index) <<"," << FT_local_request_time(FT_index) << "," - << kuka_local_request_time(kuka_index) << "," // A - << FT_device_time(FT_index) << "," // D + << kuka_local_request_time(kuka_index) << "," + << FT_device_time(FT_index) << "," << kuka_device_time(kuka_index) <<"," - << FT_device_time(FT_index) - FT_local_receive_time(FT_index) << "," - << kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index) << std::endl; + << FT_device_time(FT_index) - FT_local_receive_time(FT_index) - FT_diff << "," + << kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index) - kuka_diff + << std::endl; FT_index++; kuka_index++; } } - - - // while ( kuka_index < kuka_state_size && FT_index < FT_state_size ) - // { - // if ( kuka_local_receive_time(kuka_index) < FT_local_receive_time(FT_index) ){ - // local_request_time_offset(index) = kuka_local_request_time(kuka_index); - // local_receive_time_offset_FT(index) = FT_local_receive_time(FT_index); - // local_receive_time_offset_kuka(index) = kuka_local_receive_time(kuka_index); - // device_time_offset_kuka (index) = kuka_device_time(kuka_index); - // FT_device_time_offset(index) = FT_device_time(FT_index); - // device_type(index) = 0; - // kuka_index++; - - - // } else if( kuka_local_receive_time(kuka_index) > FT_local_receive_time(FT_index)) { - // local_request_time_offset(index) = FT_local_request_time(FT_index); - // local_receive_time_offset_FT(index) = FT_local_receive_time(FT_index); - // local_receive_time_offset_kuka(index) = kuka_local_receive_time(kuka_index); - // device_time_offset_kuka(index) = kuka_device_time(kuka_index); - // FT_device_time_offset(index) = FT_device_time(FT_index); - // device_type(index) = 1; - // FT_index++; - // } else { - // local_request_time_offset(index) = FT_local_request_time(FT_index); - // local_receive_time_offset_FT(index) = FT_local_receive_time(FT_index); - // local_receive_time_offset_kuka(index) = kuka_local_receive_time(kuka_index); - // device_time_offset_kuka(index)= kuka_device_time(kuka_index); - // FT_device_time_offset(index) = FT_device_time(FT_index); - // device_type(index) = 2; - // FT_index++; - // kuka_index++; - // } - // index++; - // } - - - // // // copy rest of kuka_local_request_time array - // // while ( itkuka_local_request_time != itkuka_local_request_timeEnd ){ - // // FT_index = FT_state_size-2; - // // local_request_time_offset << ( kuka_local_request_time(kuka_index); - // // local_receive_time_offset_FT << (FT_local_receive_time(FT_index); - // // local_receive_time_offset_kuka << (kuka_local_receive_time(kuka_index); - // // device_time_offset_kuka << (kuka_device_time(kuka_index); - // // FT_device_time_offset << (FT_device_time(FT_index); - // // kuka_index++; - // // itkuka_local_request_time++; - // // device_type << (0); - // // } - - - // // // copy rest of FT_local_request_time array - // // while ( itFT_local_request_time != itFT_local_request_timeEnd ) { - // // kuka_index = kuka_state_size-2; - // // local_request_time_offset << ( FT_local_request_time(FT_index); - // // local_receive_time_offset_FT << (FT_local_receive_time(FT_index); - // // local_receive_time_offset_kuka << (kuka_local_receive_time(kuka_index); - // // device_time_offset_kuka << (kuka_device_time(kuka_index); - // // FT_device_time_offset << (FT_device_time(FT_index); - // // FT_index++; - // // itFT_local_request_time++; - // // device_type << (1); - // // // std::cout<<"kuka_local_receive_time: " << kuka_index <<" "< Date: Mon, 19 Feb 2018 18:24:35 -0500 Subject: [PATCH 047/102] Save the pose of the EE to csv --- BasisProject.cmake | 1 + include/grl/flatbuffer/readDatafromBinary.hpp | 219 +++++++++++++++++- .../grl/vrep/InverseKinematicsVrepPlugin.hpp | 2 +- test/CMakeLists.txt | 9 +- test/KukaPoseEstimationTest.cpp | 169 ++++++++++++++ test/readFRIFBTest.cpp | 152 +----------- 6 files changed, 397 insertions(+), 155 deletions(-) create mode 100644 test/KukaPoseEstimationTest.cpp diff --git a/BasisProject.cmake b/BasisProject.cmake index dc9e90a..5abac38 100644 --- a/BasisProject.cmake +++ b/BasisProject.cmake @@ -119,6 +119,7 @@ basis_project ( sch-core # Algorithms for convex hulls https://github.com/jrl-umi3218/sch-core used for inverse kinematics RBDyn # Models the dynamics of rigid body system https://github.com/jrl-umi3218/RBDyn used for inverse kinematics Tasks # Real time control of Kinematic Trees https://github.com/jrl-umi3218/Tasks used for inverse kinematics + mc_rbdyn_urdf # This library allows to parse an URDF file and create RBDyn structure from it. https://github.com/jrl-umi3218/mc_rbdyn_urdf spdlog # fast logging library https://github.com/gabime/spdlog LibDL # Linux Dynamic Loader library, linux only https://refspecs.linuxfoundation.org/LSB_2.0.1/LSB-Core/LSB-Core/libdl.html TEST_DEPENDS diff --git a/include/grl/flatbuffer/readDatafromBinary.hpp b/include/grl/flatbuffer/readDatafromBinary.hpp index 1cc6596..fe2f262 100644 --- a/include/grl/flatbuffer/readDatafromBinary.hpp +++ b/include/grl/flatbuffer/readDatafromBinary.hpp @@ -120,16 +120,16 @@ namespace grl { auto KUKAiiwaState = states->Get(i); auto FRIMessage = KUKAiiwaState->FRIMessage(); auto joints_Position = FRIMessage->measuredJointPosition(); // flatbuffers::Vector * - Eigen::VectorXf oneStateJointPosition; + Eigen::VectorXf oneStateJointPosition(7); for(int joint_index=0; joint_index<7; joint_index++){ - oneStateJointPosition(i) = joints_Position->Get(joint_index); + oneStateJointPosition(joint_index) = joints_Position->Get(joint_index); } allJointPosition.row(i) = oneStateJointPosition.transpose(); } return allJointPosition; } - void writetoCSVforKuka(std::string kukaBinaryfile, std::string csvfilename) { + void writetoJointAngToCSV(std::string kukaBinaryfile, std::string csvfilename) { fbs_tk::Root KUKAiiwaStatesRoot = fbs_tk::open_root(kukaBinaryfile); auto states = KUKAiiwaStatesRoot->states(); std::size_t kuka_state_size = states->size(); @@ -181,5 +181,218 @@ namespace grl { } fs.close(); } + void writeTimeEventToCSV( + std::string & csvfilename, + grl::VectorXd device_time, + grl::VectorXd local_request_time, + grl::VectorXd local_receive_time){ + + std::size_t size = device_time.size(); + // auto initial_local_time = local_request_time(0); + auto initial_local_time = local_receive_time(0); + auto initial_device_time = device_time(0); + local_request_time = local_request_time - initial_local_time * grl::VectorXd::Ones(size); + local_receive_time = local_receive_time - initial_local_time * grl::VectorXd::Ones(size); + device_time = device_time - initial_device_time * grl::VectorXd::Ones(size); + grl::VectorXd receive_request = local_receive_time - local_request_time; + // grl::VectorXd device_time_offset = local_receive_time - local_request_time; + std::ofstream fs; + // create a name for the file output + fs.open( csvfilename, std::ofstream::out | std::ofstream::app); + // write the file headers + fs << "local_request_time_offset," + << "local_receive_time_X," + << "device_time_offset," + << "Y," + << "Receive-Request," + << "device_time_step," + << "receive_time_step," + << std::endl; + int64_t device_time_step = 0; + int64_t receive_time_step = 0; + for(int i=0; i0) { + device_time_step = device_time(i) - device_time(i-1); + receive_time_step = local_receive_time(i) - local_receive_time(i-1); + } + // write the data to the output file + fs << local_request_time(i)<< "," // A + << local_receive_time(i) <<"," // B + << device_time(i) <<"," // C + << device_time(i) - local_receive_time(i) << "," + << receive_request(i) << "," + << device_time_step << "," + << receive_time_step << "," + << std::endl; //D + } + fs.close(); +} + + void writePoseToCSV( + std::string & csvfilename, + grl::VectorXd device_time, + grl::VectorXd local_request_time, + grl::VectorXd local_receive_time, + Eigen::MatrixXd &poseEE){ + + std::size_t time_size = device_time.size(); + std::size_t row_size = poseEE.rows(); + assert(time_size == row_size); + + // auto initial_local_time = local_request_time(0); + auto initial_local_time = local_receive_time(0); + auto initial_device_time = device_time(0); + local_request_time = local_request_time - initial_local_time * grl::VectorXd::Ones(time_size); + local_receive_time = local_receive_time - initial_local_time * grl::VectorXd::Ones(time_size); + device_time = device_time - initial_device_time * grl::VectorXd::Ones(time_size); + grl::VectorXd receive_request = local_receive_time - local_request_time; + // grl::VectorXd device_time_offset = local_receive_time - local_request_time; + std::ofstream fs; + // create a name for the file output + fs.open( csvfilename, std::ofstream::out | std::ofstream::app); + // write the file headers + fs << "local_receive_time_X," + << "local_request_time_offset," + << "device_time_offset," + << "Y," + << "Receive-Request," + << "device_time_step," + << "receive_time_step," + << "X," + << "Y," + << "Z," + << "R," + << "P," + << "Y," + << std::endl; + int64_t device_time_step = 0; + int64_t receive_time_step = 0; + for(int i=0; i0) { + device_time_step = device_time(i) - device_time(i-1); + receive_time_step = local_receive_time(i) - local_receive_time(i-1); + } + Eigen::RowVectorXd pose = poseEE.row(i); + // write the data to the output file + fs << local_receive_time(i) <<"," // B + << local_request_time(i)<< "," // A + << device_time(i) <<"," // C + << device_time(i) - local_receive_time(i) << "," + << receive_request(i) << "," + << device_time_step << "," + << receive_time_step << "," + << pose(0) << "," + << pose(1) << "," + << pose(2) << "," + << pose(3) << "," + << pose(4) << "," + << pose(5) << "," + << std::endl; //D + } + fs.close(); +} + + void writetoCSVforFusionTrackKukaiiwa( + std::string &fusiontrackBinaryfile, + std::string &kukaBinaryfile, + std::string& FTKUKA_CSVfilename, + std::string& FT_CSVfilename, + std::string& KUKA_CSVfilename) { + fbs_tk::Root KUKAiiwaStatesRoot = + fbs_tk::open_root(kukaBinaryfile); + auto KUKA_states = KUKAiiwaStatesRoot->states(); + std::size_t kuka_state_size = KUKA_states->size(); + std::cout<< "------Kuka_state_size: "<< kuka_state_size << std::endl; + + grl::VectorXd kuka_device_time = grl::getTimeStamp(KUKAiiwaStatesRoot, grl::kuka_tag(), grl::TimeType::device_time); + grl::VectorXd kuka_local_request_time = grl::getTimeStamp(KUKAiiwaStatesRoot, grl::kuka_tag(), grl::TimeType::local_request_time); + grl::VectorXd kuka_local_receive_time = grl::getTimeStamp(KUKAiiwaStatesRoot, grl::kuka_tag(), grl::TimeType::local_receive_time); + writeTimeEventToCSV(KUKA_CSVfilename, kuka_device_time, kuka_local_request_time, kuka_local_receive_time); + + + + fbs_tk::Root FusionTrackStatesRoot = fbs_tk::open_root(fusiontrackBinaryfile); + auto FT_states = FusionTrackStatesRoot->states(); + std::size_t FT_state_size = FT_states->size(); + std::cout<< "------FusionTrack State Size: "<< FT_state_size << std::endl; + grl::VectorXd FT_device_time = grl:: getTimeStamp(FusionTrackStatesRoot, grl::fusiontracker_tag(), grl::TimeType::device_time); + grl::VectorXd FT_local_request_time = grl::getTimeStamp(FusionTrackStatesRoot, grl::fusiontracker_tag(), grl::TimeType::local_request_time); + grl::VectorXd FT_local_receive_time = grl::getTimeStamp(FusionTrackStatesRoot, grl::fusiontracker_tag(), grl::TimeType::local_receive_time); + writeTimeEventToCSV(FT_CSVfilename, FT_device_time, FT_local_request_time, FT_local_receive_time); + int kuka_index = 0; + int FT_index = 0; + // filter out the very beginning data, which can gurantee to record the data when the two devices work simultaniously. + while(kuka_local_receive_time(kuka_index) < FT_local_receive_time(FT_index)){ + kuka_index++; + } + while(kuka_local_receive_time(kuka_index) > FT_local_receive_time(FT_index) && kuka_index == 0){ + FT_index++; + } + + auto initial_local_time = std::min(FT_local_receive_time(FT_index), kuka_local_receive_time(kuka_index)); + auto initial_device_time_kuka = kuka_device_time(kuka_index); + auto initial_device_time_FT = FT_device_time(FT_index); + FT_local_request_time = FT_local_request_time - initial_local_time * grl::VectorXd::Ones(FT_state_size); + FT_local_receive_time = FT_local_receive_time - initial_local_time * grl::VectorXd::Ones(FT_state_size); + FT_device_time = FT_device_time - initial_device_time_FT * grl::VectorXd::Ones(FT_state_size); + + kuka_local_request_time = kuka_local_request_time - initial_local_time * grl::VectorXd::Ones(kuka_state_size); + kuka_local_receive_time = kuka_local_receive_time - initial_local_time * grl::VectorXd::Ones(kuka_state_size); + kuka_device_time = kuka_device_time - initial_device_time_kuka * grl::VectorXd::Ones(kuka_state_size); + + std::ofstream fs; + // create a name for the file output + fs.open( FTKUKA_CSVfilename, std::ofstream::out | std::ofstream::app); + // write the file headers + fs << "local_receive_time_offset_X," + << "FT_local_request_time," + << "KUKA_local_request_time," + << "FT_device_time_offset," + << "device_time_offset_kuka," + << "Y_FT," + << "Y_kuka" + << std::endl; + + int64_t kuka_diff = kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index); + int64_t FT_diff = FT_device_time(FT_index) - FT_local_receive_time(FT_index); + while ( kuka_index < kuka_state_size && FT_index < FT_state_size ) + { + if ( kuka_local_receive_time(kuka_index) < FT_local_receive_time(FT_index) ){ + // write the data to the output file + fs << kuka_local_receive_time(kuka_index) <<"," + <<"," + << kuka_local_request_time(kuka_index) << "," + << "," + << kuka_device_time(kuka_index) <<"," + << "," + << kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index) - kuka_diff + << std::endl; + kuka_index++; + } else if( kuka_local_receive_time(kuka_index) > FT_local_receive_time(FT_index)) { + // write the data to the output file + fs << FT_local_receive_time(FT_index) <<"," + << FT_local_request_time(FT_index) << "," + <<"," + << FT_device_time(FT_index) << "," + << "," + << FT_device_time(FT_index) - FT_local_receive_time(FT_index) - FT_diff << "," + << std::endl; + FT_index++; + } else { + fs << FT_local_receive_time(FT_index) <<"," + << FT_local_request_time(FT_index) << "," + << kuka_local_request_time(kuka_index) << "," + << FT_device_time(FT_index) << "," + << kuka_device_time(kuka_index) <<"," + << FT_device_time(FT_index) - FT_local_receive_time(FT_index) - FT_diff << "," + << kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index) - kuka_diff + << std::endl; + FT_index++; + kuka_index++; + } + } + fs.close(); +} + } #endif \ No newline at end of file diff --git a/include/grl/vrep/InverseKinematicsVrepPlugin.hpp b/include/grl/vrep/InverseKinematicsVrepPlugin.hpp index 383462d..5e89735 100644 --- a/include/grl/vrep/InverseKinematicsVrepPlugin.hpp +++ b/include/grl/vrep/InverseKinematicsVrepPlugin.hpp @@ -159,7 +159,6 @@ class InverseKinematicsVrepPlugin public: //typedef grl::InverseKinematicsController parent_type; - //using parent_type::currentKinematicsStateP_; //using parent_type::parent_type::InitializeKinematicsState; @@ -296,6 +295,7 @@ class InverseKinematicsVrepPlugin auto I = Eigen::Matrix3d::Identity(); auto h = Eigen::Vector3d::Zero(); /// @todo TODO(ahundt) consider the origin of the inertia! https://github.com/jrl-umi3218/Tasks/issues/10#issuecomment-257198604 + /// RBInertia is the Spatial Rigid Body Inertia implementation sva::RBInertiad rbi_i(mass, h, I); /// @todo TODO(ahundt) add real support for links, particularly the respondable aspects, see LBR_iiwa_14_R820_joint1_resp in RoboneSimulation.ttt // @todo TODO(ahundt) REMEMBER: The first joint is NOT respondable! diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index f3788e1..374153f 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -98,8 +98,13 @@ if(TARGET KukaFRIClient OR FRI_Client_SDK_Cpp_FOUND) basis_add_executable(readFRIFBTest.cpp) basis_target_link_libraries(readFRIFBTest ${Boost_LIBRARIES} ${Boost_REGEX_LIBRARY} ${FRI_Client_SDK_Cpp_LIBRARIES} ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} KukaFRIClient ${FLATBUFFERS_STATIC_LIB} -lboost_iostreams -lboost_system -lboost_filesystem) - # basis_add_executable(KukaPoseEstimationTest.cpp)# ${GRL_FLATBUFFERS_OUTPUTS}) - # basis_target_link_libraries(KukaPoseEstimationTest ${Boost_LIBRARIES} ${Boost_REGEX_LIBRARY} ${FRI_Client_SDK_Cpp_LIBRARIES} ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} KukaFRIClient ${FLATBUFFERS_STATIC_LIB} -lboost_iostreams -lboost_system -lboost_filesystem) + basis_add_executable(KukaPoseEstimationTest.cpp) + basis_target_link_libraries(KukaPoseEstimationTest ${Boost_LIBRARIES} ${Boost_REGEX_LIBRARY} ${FRI_Client_SDK_Cpp_LIBRARIES} ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} KukaFRIClient ${FLATBUFFERS_STATIC_LIB} -lboost_iostreams -lboost_system -lboost_filesystem + mc_rbdyn_urdf + SpaceVecAlg #TODO: re-enable when exported targets are correct again + sch-core + RBDyn + Tasks) endif() diff --git a/test/KukaPoseEstimationTest.cpp b/test/KukaPoseEstimationTest.cpp new file mode 100644 index 0000000..44bec3b --- /dev/null +++ b/test/KukaPoseEstimationTest.cpp @@ -0,0 +1,169 @@ +// Copyright 2012-2017 CNRS-UM LIRMM, CNRS-AIST JRL +// +// This file is part of RBDyn. +// +// RBDyn is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// RBDyn is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with RBDyn. If not, see . + +// includes +// std +#include + +// boost +#define BOOST_TEST_MODULE KukaPoseEstimationTest // master test suite name +#include +#include + + +#include +#include +#include +#include +#include +#include "camodocal/EigenUtils.h" + +#include "grl/flatbuffer/readDatafromBinary.hpp" +#include "grl/time.hpp" + +// SpaceVecAlg +// https://github.com/jrl-umi3218/SpaceVecAlg +#include + +// RBDyn +// https://github.com/jrl-umi3218/RBDyn +#include +#include +#include +#include +#include +#include + +// mc_rbdyn_urdf +// https://github.com/jrl-umi3218/mc_rbdyn_urdf +#include +#include "kukaiiwaURDF.h" + +std::string foldname = "/home/chunting/src/V-REP_PRO_EDU_V3_4_0_Linux/"; +std::string kukaBinaryfile = foldname + "2018_02_13_19_06_04_Kukaiiwa.iiwa"; +std::string fusiontrackBinaryfile = foldname + "2018_02_13_19_05_49_FusionTrack.flik"; +std::string FTKUKA_CSVfilename = foldname + current_date_and_time_string() + "_FTKUKA.csv"; +std::string FT_CSVfilename = foldname + current_date_and_time_string() + "_FT.csv"; +std::string KUKA_CSVfilename = foldname + current_date_and_time_string() + "_KUKA.csv"; +std::string KUKA_CSVfilename_Joint = foldname + current_date_and_time_string() + "_KUKA_Joint.csv"; + +int main(int argc, char* argv[]) +{ + using namespace Eigen; + using namespace sva; + using namespace rbd; + namespace cst = boost::math::constants; + auto strRobot = mc_rbdyn_urdf::rbdyn_from_urdf(XYZSarmUrdf); + MultiBody mb = strRobot.mb; + MultiBodyConfig mbc(mb); + MultiBodyGraph mbg(strRobot.mbg); + std::size_t nrJoints = mbg.nrJoints(); + std::vector jointNames; + std::cout<<"Joint Size: "<< nrJoints << std::endl; + fbs_tk::Root KUKAiiwaStatesRoot = fbs_tk::open_root(kukaBinaryfile); + grl::VectorXd kuka_device_time = grl::getTimeStamp(KUKAiiwaStatesRoot, grl::kuka_tag(), grl::TimeType::device_time); + grl::VectorXd kuka_local_request_time = grl::getTimeStamp(KUKAiiwaStatesRoot, grl::kuka_tag(), grl::TimeType::local_request_time); + grl::VectorXd kuka_local_receive_time = grl::getTimeStamp(KUKAiiwaStatesRoot, grl::kuka_tag(), grl::TimeType::local_receive_time); + + Eigen::MatrixXf jointAngles = grl::getAllJointAngles(KUKAiiwaStatesRoot); + std::size_t row_size = jointAngles.rows(); + std::size_t body_size = mbc.bodyPosW.size(); + /// The translation and Euler angles in world coordinate. + Eigen::MatrixXd poseEE(row_size,6); + + for(int rowIdx=0; rowIdx 0) { + mbc.q[jointIndex][0] = oneStateJointPosition[jointIdx]; + jointIdx++; + } + } + forwardKinematics(mb, mbc); + sva::PTransformd pos = mbc.bodyPosW[body_size-1]; + Eigen::Matrix3d E ; // rotation + + Eigen::Vector3d r ; // translation + E = pos.rotation(); + r = pos.translation(); + Eigen::Quaterniond q(E); + Eigen::RowVectorXd pose(6); + pose << r.transpose(), r.transpose(); + poseEE.row(rowIdx) = pose; + // std::cout << "-----------------------------------" << std::endl; + // std::cout << "Rotation:\n " << E << std::endl + // << "Translation:\n" << r < bodyPosW; + // std::size_t nrBodies = mb.nrBodies(); + // std::cout<<"Body Size: "<< nrBodies << std::endl; + // for(int bodyIndex=0; bodyIndex()/2.}}; +// /// @todo TODO(ahundt) warn/error when size!=0 or explicitly handle those cases +// // if(mbc.q[jointIdx].size()>0) simArmConfig.q[jointIdx][0] = futureAngle; +// //auto possize = mbc.q[1].size(); +// std::cout<<"Joint angle: "<< mbc.q[1][0] << std::endl; +// forwardKinematics(mb, mbc); + +// ee = mbc.bodyPosW.size(); +// std::cout<<"size: " << ee << std::endl; +// for(int i=0; i<10; ++i) { +// pos = mbc.bodyPosW[i]; +// std::cout<<"Joint Position: "<< mb.jointPosInParam(i)<< std::endl; +// std::cout << pos << std::endl <<"---------------"< KUKAiiwaStatesRoot = - fbs_tk::open_root(kukaBinaryfile); - auto KUKA_states = KUKAiiwaStatesRoot->states(); - std::size_t kuka_state_size = KUKA_states->size(); - std::cout<< "------Kuka_state_size: "<< kuka_state_size << std::endl; - - grl::VectorXd kuka_device_time = grl::getTimeStamp(KUKAiiwaStatesRoot, grl::kuka_tag(), grl::TimeType::device_time); - grl::VectorXd kuka_local_request_time = grl::getTimeStamp(KUKAiiwaStatesRoot, grl::kuka_tag(), grl::TimeType::local_request_time); - grl::VectorXd kuka_local_receive_time = grl::getTimeStamp(KUKAiiwaStatesRoot, grl::kuka_tag(), grl::TimeType::local_receive_time); - writetoCSV(KUKA_CSVfilename, kuka_device_time, kuka_local_request_time, kuka_local_receive_time); - - - - fbs_tk::Root FusionTrackStatesRoot = fbs_tk::open_root(fusiontrackBinaryfile); - auto FT_states = FusionTrackStatesRoot->states(); - std::size_t FT_state_size = FT_states->size(); - std::cout<< "------FusionTrack State Size: "<< FT_state_size << std::endl; - grl::VectorXd FT_device_time = grl:: getTimeStamp(FusionTrackStatesRoot, grl::fusiontracker_tag(), grl::TimeType::device_time); - grl::VectorXd FT_local_request_time = grl::getTimeStamp(FusionTrackStatesRoot, grl::fusiontracker_tag(), grl::TimeType::local_request_time); - grl::VectorXd FT_local_receive_time = grl::getTimeStamp(FusionTrackStatesRoot, grl::fusiontracker_tag(), grl::TimeType::local_receive_time); - writetoCSV(FT_CSVfilename, FT_device_time, FT_local_request_time, FT_local_receive_time); - int kuka_index = 0; - int FT_index = 0; - // filter out the very beginning data, which can gurantee to record the data when the two devices work simultaniously. - while(kuka_local_receive_time(kuka_index) < FT_local_receive_time(FT_index)){ - kuka_index++; - } - while(kuka_local_receive_time(kuka_index) > FT_local_receive_time(FT_index) && kuka_index == 0){ - FT_index++; - } - - auto initial_local_time = std::min(FT_local_receive_time(FT_index), kuka_local_receive_time(kuka_index)); - auto initial_device_time_kuka = kuka_device_time(kuka_index); - auto initial_device_time_FT = FT_device_time(FT_index); - FT_local_request_time = FT_local_request_time - initial_local_time * grl::VectorXd::Ones(FT_state_size); - FT_local_receive_time = FT_local_receive_time - initial_local_time * grl::VectorXd::Ones(FT_state_size); - FT_device_time = FT_device_time - initial_device_time_FT * grl::VectorXd::Ones(FT_state_size); - - kuka_local_request_time = kuka_local_request_time - initial_local_time * grl::VectorXd::Ones(kuka_state_size); - kuka_local_receive_time = kuka_local_receive_time - initial_local_time * grl::VectorXd::Ones(kuka_state_size); - kuka_device_time = kuka_device_time - initial_device_time_kuka * grl::VectorXd::Ones(kuka_state_size); - - std::ofstream fs; - // create a name for the file output - fs.open( CSVfilename, std::ofstream::out | std::ofstream::app); - // write the file headers - fs << "local_receive_time_offset_X," - << "FT_local_request_time," - << "KUKA_local_request_time," - << "FT_device_time_offset," - << "device_time_offset_kuka," - << "Y_FT," - << "Y_kuka" - << std::endl; - - int64_t kuka_diff = kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index); - int64_t FT_diff = FT_device_time(FT_index) - FT_local_receive_time(FT_index); - while ( kuka_index < kuka_state_size && FT_index < FT_state_size ) - { - if ( kuka_local_receive_time(kuka_index) < FT_local_receive_time(FT_index) ){ - // write the data to the output file - fs << kuka_local_receive_time(kuka_index) <<"," - <<"," - << kuka_local_request_time(kuka_index) << "," - << "," - << kuka_device_time(kuka_index) <<"," - << "," - << kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index) - kuka_diff - << std::endl; - kuka_index++; - } else if( kuka_local_receive_time(kuka_index) > FT_local_receive_time(FT_index)) { - // write the data to the output file - fs << FT_local_receive_time(FT_index) <<"," - << FT_local_request_time(FT_index) << "," - <<"," - << FT_device_time(FT_index) << "," - << "," - << FT_device_time(FT_index) - FT_local_receive_time(FT_index) - FT_diff << "," - << std::endl; - FT_index++; - } else { - fs << FT_local_receive_time(FT_index) <<"," - << FT_local_request_time(FT_index) << "," - << kuka_local_request_time(kuka_index) << "," - << FT_device_time(FT_index) << "," - << kuka_device_time(kuka_index) <<"," - << FT_device_time(FT_index) - FT_local_receive_time(FT_index) - FT_diff << "," - << kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index) - kuka_diff - << std::endl; - FT_index++; - kuka_index++; - } - } - fs.close(); -} -void writetoCSV( - std::string & csvfilename, - grl::VectorXd device_time, - grl::VectorXd local_request_time, - grl::VectorXd local_receive_time){ - - std::size_t size = device_time.size(); - // auto initial_local_time = local_request_time(0); - auto initial_local_time = local_receive_time(0); - auto initial_device_time = device_time(0); - local_request_time = local_request_time - initial_local_time * grl::VectorXd::Ones(size); - local_receive_time = local_receive_time - initial_local_time * grl::VectorXd::Ones(size); - device_time = device_time - initial_device_time * grl::VectorXd::Ones(size); - grl::VectorXd receive_request = local_receive_time - local_request_time; - // grl::VectorXd device_time_offset = local_receive_time - local_request_time; - std::ofstream fs; - // create a name for the file output - fs.open( csvfilename, std::ofstream::out | std::ofstream::app); - // write the file headers - fs << "local_request_time_offset," - << "local_receive_time_X," - << "device_time_offset," - << "Y," - << "Receive-Request," - << "device_time_step," - << "receive_time_step," - << std::endl; - int64_t device_time_step = 0; - int64_t receive_time_step = 0; - for(int i=0; i0) { - device_time_step = device_time(i) - device_time(i-1); - receive_time_step = local_receive_time(i) - local_receive_time(i-1); - } - // write the data to the output file - fs << local_request_time(i)<< "," // A - << local_receive_time(i) <<"," // B - << device_time(i) <<"," // C - << device_time(i) - local_receive_time(i) << "," - << receive_request(i) << "," - << device_time_step << "," - << receive_time_step << "," - << std::endl; //D - } - fs.close(); -} - From 1dfaee4068e7e19691182a775e0ec14e71c3494b Mon Sep 17 00:00:00 2001 From: Chunting Date: Tue, 20 Feb 2018 21:38:22 -0500 Subject: [PATCH 048/102] Draw straight line --- include/grl/flatbuffer/readDatafromBinary.hpp | 9 +++++---- test/KukaPoseEstimationTest.cpp | 19 ++++++++++++------- 2 files changed, 17 insertions(+), 11 deletions(-) diff --git a/include/grl/flatbuffer/readDatafromBinary.hpp b/include/grl/flatbuffer/readDatafromBinary.hpp index fe2f262..6379b9a 100644 --- a/include/grl/flatbuffer/readDatafromBinary.hpp +++ b/include/grl/flatbuffer/readDatafromBinary.hpp @@ -26,6 +26,7 @@ #include #include +const double PI = 3.14159265359; namespace grl { typedef Eigen::Matrix VectorXd; struct kuka_tag {}; @@ -254,16 +255,16 @@ namespace grl { fs << "local_receive_time_X," << "local_request_time_offset," << "device_time_offset," - << "Y," + << "time_Y," << "Receive-Request," << "device_time_step," << "receive_time_step," << "X," << "Y," << "Z," - << "R," - << "P," - << "Y," + << "A," + << "B," + << "C," << std::endl; int64_t device_time_step = 0; int64_t receive_time_step = 0; diff --git a/test/KukaPoseEstimationTest.cpp b/test/KukaPoseEstimationTest.cpp index 44bec3b..1ae809e 100644 --- a/test/KukaPoseEstimationTest.cpp +++ b/test/KukaPoseEstimationTest.cpp @@ -53,11 +53,15 @@ #include #include "kukaiiwaURDF.h" +// const double PI = 3.14159265359; +const double RadtoDegree = 180/3.14159265359; +const double MeterToMM = 1000; + std::string foldname = "/home/chunting/src/V-REP_PRO_EDU_V3_4_0_Linux/"; -std::string kukaBinaryfile = foldname + "2018_02_13_19_06_04_Kukaiiwa.iiwa"; -std::string fusiontrackBinaryfile = foldname + "2018_02_13_19_05_49_FusionTrack.flik"; -std::string FTKUKA_CSVfilename = foldname + current_date_and_time_string() + "_FTKUKA.csv"; -std::string FT_CSVfilename = foldname + current_date_and_time_string() + "_FT.csv"; +std::string kukaBinaryfile = foldname + "2018_02_20_21_21_08_Kukaiiwa.iiwa"; +// std::string fusiontrackBinaryfile = foldname + "2018_02_20_20_27_23_FusionTrack.flik"; +// std::string FTKUKA_CSVfilename = foldname + current_date_and_time_string() + "_FTKUKA.csv"; +// std::string FT_CSVfilename = foldname + current_date_and_time_string() + "_FT.csv"; std::string KUKA_CSVfilename = foldname + current_date_and_time_string() + "_KUKA.csv"; std::string KUKA_CSVfilename_Joint = foldname + current_date_and_time_string() + "_KUKA_Joint.csv"; @@ -103,10 +107,11 @@ int main(int argc, char* argv[]) Eigen::Vector3d r ; // translation E = pos.rotation(); - r = pos.translation(); + r = MeterToMM*pos.translation(); + Eigen::Vector3d eulerAngleEigen = RadtoDegree*E.eulerAngles(0,1,2); Eigen::Quaterniond q(E); Eigen::RowVectorXd pose(6); - pose << r.transpose(), r.transpose(); + pose << r.transpose(), eulerAngleEigen.transpose(); poseEE.row(rowIdx) = pose; // std::cout << "-----------------------------------" << std::endl; // std::cout << "Rotation:\n " << E << std::endl @@ -116,7 +121,7 @@ int main(int argc, char* argv[]) } // std::cout << poseEE << std::endl; grl::writePoseToCSV(KUKA_CSVfilename, kuka_device_time, kuka_local_request_time, kuka_local_receive_time, poseEE); - + grl::writetoJointAngToCSV(kukaBinaryfile, KUKA_CSVfilename_Joint); /// Bodies transformation in world coordinate. /// std::vector bodyPosW; // std::size_t nrBodies = mb.nrBodies(); From 60b6ce859e99c91bd55378851ed55eaa79405bbf Mon Sep 17 00:00:00 2001 From: Chunting Date: Fri, 23 Feb 2018 21:24:51 -0500 Subject: [PATCH 049/102] Plot FT Pose --- include/grl/flatbuffer/readDatafromBinary.hpp | 329 ++++++++++++++++-- test/KukaPoseEstimationTest.cpp | 60 +--- test/readFRIFBTest.cpp | 28 +- 3 files changed, 331 insertions(+), 86 deletions(-) diff --git a/include/grl/flatbuffer/readDatafromBinary.hpp b/include/grl/flatbuffer/readDatafromBinary.hpp index 6379b9a..33b51ed 100644 --- a/include/grl/flatbuffer/readDatafromBinary.hpp +++ b/include/grl/flatbuffer/readDatafromBinary.hpp @@ -26,13 +26,45 @@ #include #include -const double PI = 3.14159265359; + namespace grl { typedef Eigen::Matrix VectorXd; + typedef Eigen::Matrix MatrixXd; struct kuka_tag {}; struct fusiontracker_tag {}; enum TimeType { device_time = 0, local_request_time = 1, local_receive_time = 2 }; + /// Help initialize a vector array of strings. + /// See https://stackoverflow.com/questions/4268886/initialize-a-vector-array-of-strings + template + T * end(T (&ra)[N]) { + return ra + N; + } + /// Remove a certain row or colum for the given matrix in Eigen + /// See https://stackoverflow.com/questions/13290395/how-to-remove-a-certain-row-or-column-while-using-eigen-library-c + template + void removeRow(T& matrix, unsigned int rowToRemove) + { + unsigned int numRows = matrix.rows()-1; + unsigned int numCols = matrix.cols(); + + if( rowToRemove < numRows ) + matrix.block(rowToRemove,0,numRows-rowToRemove,numCols) = matrix.bottomRows(numRows-rowToRemove); + + matrix.conservativeResize(numRows,numCols); + } + template + void removeColumn(T& matrix, unsigned int colToRemove) + { + unsigned int numRows = matrix.rows(); + unsigned int numCols = matrix.cols()-1; + + if( colToRemove < numCols ) + matrix.block(0,colToRemove,numRows,numCols-colToRemove) = matrix.rightCols(numCols-colToRemove); + matrix.conservativeResize(numRows,numCols); + } + + /// Get the original time from KUKAiiwaStates grl::VectorXd getTimeStamp(const fbs_tk::Root &kukaStatesP, kuka_tag, TimeType time_type){ auto states = kukaStatesP->states(); std::size_t state_size = states->size(); @@ -61,7 +93,7 @@ namespace grl { } return timeStamp; } - + /// Get the original time from LogKUKAiiwaFusionTrack grl::VectorXd getTimeStamp(const fbs_tk::Root &logKUKAiiwaFusionTrackP, fusiontracker_tag, TimeType time_type){ auto states = logKUKAiiwaFusionTrackP->states(); @@ -91,12 +123,109 @@ namespace grl { } return timeStamp; } + void regularizeTimeEvent(grl::VectorXd &local_request_time, grl::VectorXd &local_receive_time, grl::VectorXd &device_time){ + std::size_t device_time_size = device_time.size(); + std::size_t local_request_time_size = local_request_time.size(); + std::size_t local_receive_time_size = local_receive_time.size(); + assert( local_receive_time_size >0 && device_time_size==local_request_time_size && local_request_time_size== local_receive_time_size); + + grl::MatrixXd timeEventM(local_receive_time_size,3); + auto initial_local_time = local_receive_time(0); + auto initial_device_time = device_time(0); + local_request_time = local_request_time - initial_local_time * grl::VectorXd::Ones(local_receive_time_size); + local_receive_time = local_receive_time - initial_local_time * grl::VectorXd::Ones(local_receive_time_size); + device_time = device_time - initial_device_time * grl::VectorXd::Ones(local_receive_time_size); + } + /// Use the first timestamp of local_receive_time and device_time as the base to get the time offset + grl::MatrixXd getRegularizedTimeStamp(const fbs_tk::Root &logKUKAiiwaFusionTrackP, fusiontracker_tag){ + auto states = logKUKAiiwaFusionTrackP->states(); + std::size_t state_size = states->size(); + std::cout<< "------FusionTrack State Size: "<< state_size << std::endl; + grl::VectorXd device_time = grl::getTimeStamp(logKUKAiiwaFusionTrackP, grl::fusiontracker_tag(), grl::TimeType::device_time); + grl::VectorXd local_request_time = grl::getTimeStamp(logKUKAiiwaFusionTrackP, grl::fusiontracker_tag(), grl::TimeType::local_request_time); + grl::VectorXd local_receive_time = grl::getTimeStamp(logKUKAiiwaFusionTrackP, grl::fusiontracker_tag(), grl::TimeType::local_receive_time); + + regularizeTimeEvent(local_request_time, local_receive_time, device_time); + grl::MatrixXd timeEventM(state_size,3); + timeEventM.col(0) = local_receive_time; + timeEventM.col(1) = local_request_time; + timeEventM.col(2) = device_time; + return timeEventM; + } + template + bool checkmonotonic( T &time){ + for(int i=1; i &kukaStatesP, kuka_tag){ + auto states = kukaStatesP->states(); + std::size_t state_size = states->size(); + std::cout<< "------Kuka State Size: "<< state_size << std::endl; + grl::VectorXd device_time = grl::getTimeStamp(kukaStatesP, grl::kuka_tag(), grl::TimeType::device_time); + grl::VectorXd local_request_time = grl::getTimeStamp(kukaStatesP, grl::kuka_tag(), grl::TimeType::local_request_time); + grl::VectorXd local_receive_time = grl::getTimeStamp(kukaStatesP, grl::kuka_tag(), grl::TimeType::local_receive_time); + regularizeTimeEvent(local_request_time, local_receive_time, device_time); + assert(checkmonotonic(local_receive_time)); + grl::MatrixXd timeEventM(state_size,3); + timeEventM.col(0) = local_receive_time; + timeEventM.col(1) = local_request_time; + timeEventM.col(2) = device_time; + return timeEventM; + } + + + Eigen::MatrixXd getMarkerPose(const fbs_tk::Root &logKUKAiiwaFusionTrackP, uint32_t &makerID, grl::MatrixXd& timeEvent){ + auto states = logKUKAiiwaFusionTrackP->states(); + std::size_t state_size = states->size(); + assert(state_size>0); + // The first columne is counter + std::size_t cols = 11; + int row = 0; + Eigen::MatrixXd markerPose(state_size, cols); + int BadCount = 0; + for(int i = 0; iGet(i); + auto FT_Message = kukaiiwaFusionTrackMessage->deviceState_as_FusionTrackMessage(); + auto FT_Frame = FT_Message->frame(); + auto counter = FT_Frame->counter(); + auto Makers = FT_Frame->markers(); + /// In some frames, there are no markers, where we skip that line. Also we need to remove the corresponding line in TimeEvent. + if(Makers!=nullptr) { + auto makerSize = Makers->size(); + for(int markerIndex=0; markerIndexGet(markerIndex); + auto marker_ID = marker->geometryID(); + if(marker_ID == makerID){ + // markerPose.conservativeResize(row + 1, cols); + auto Pose = marker->transform(); + auto position = Pose->position(); + auto orientationQtn = Pose->orientation(); + Eigen::VectorXd onePose(cols); + grl::VectorXd oneTime = timeEvent.row(i); + onePose << oneTime.cast(), counter,1000* position.x(), 1000*position.y(), 1000*position.z(), orientationQtn.x(), orientationQtn.y(), orientationQtn.z(), orientationQtn.w(); + markerPose.row(row++) = onePose.transpose(); + continue; + } + } + } + } + if(row < state_size) { + markerPose.conservativeResize(row, Eigen::NoChange_t{}); + } + std::cout <<"State size: " << state_size <<" markerPose size: " << markerPose.rows() << " timeEvent: " << timeEvent.rows() << " BadCount: " << BadCount < &kukaStatesP, int joint_index){ - // if(kukaStatesP == nullptr) - // return Eigen::VectorXf(); + auto states = kukaStatesP->states(); std::size_t state_size = states->size(); @@ -109,19 +238,17 @@ namespace grl { } return jointPosition; } - /// Return the joint_index th joint angle + /// Reture all joint angles + Eigen::MatrixXd getAllJointAngles(const fbs_tk::Root &kukaStatesP){ - Eigen::MatrixXf getAllJointAngles(const fbs_tk::Root &kukaStatesP){ - // if(kukaStatesP == nullptr) - // return Eigen::MatrixXf(); auto states = kukaStatesP->states(); std::size_t state_size = states->size(); - Eigen::MatrixXf allJointPosition(state_size, 7); + Eigen::MatrixXd allJointPosition(state_size, 7); for(int i = 0; iGet(i); auto FRIMessage = KUKAiiwaState->FRIMessage(); auto joints_Position = FRIMessage->measuredJointPosition(); // flatbuffers::Vector * - Eigen::VectorXf oneStateJointPosition(7); + Eigen::VectorXd oneStateJointPosition(7); for(int joint_index=0; joint_index<7; joint_index++){ oneStateJointPosition(joint_index) = joints_Position->Get(joint_index); } @@ -129,8 +256,36 @@ namespace grl { } return allJointPosition; } + template + void writeMatrixToCSV(const std::string& CSV_FileName, std::vector &labels, T& t){ + std::size_t labels_size = labels.size(); + std::size_t cols_size = t.cols(); + std::size_t rows_size = t.rows(); + assert(labels_size == cols_size && rows_size>0 && cols_size>0); + std::cout<< t.col(0) < KUKAiiwaStatesRoot = fbs_tk::open_root(kukaBinaryfile); auto states = KUKAiiwaStatesRoot->states(); std::size_t kuka_state_size = states->size(); @@ -145,6 +300,8 @@ namespace grl { kuka_local_receive_time = kuka_local_receive_time - initial_local_time * grl::VectorXd::Ones(kuka_state_size); kuka_deviceTime = kuka_deviceTime - initial_device_time_kuka * grl::VectorXd::Ones(kuka_state_size); + //Eigen::MatrixXd getAllJointAngles(const fbs_tk::Root &kukaStatesP); + Eigen::VectorXf jointAngles_0 = grl::getJointAnglesFromKUKA(KUKAiiwaStatesRoot, 0); Eigen::VectorXf jointAngles_1 = grl::getJointAnglesFromKUKA(KUKAiiwaStatesRoot, 1); Eigen::VectorXf jointAngles_2 = grl::getJointAnglesFromKUKA(KUKAiiwaStatesRoot, 2); @@ -158,14 +315,14 @@ namespace grl { std::ofstream fs; // create a name for the file output - fs.open(csvfilename, std::ofstream::out | std::ofstream::app); + fs.open(CSV_FileName, std::ofstream::out | std::ofstream::app); // write the file headers fs << "local_request_time" << ",local_receive_time" << ",Time_Difference" << ",Device_Time" - << ",JointPosition_0" << ",JointPosition_1" << ",JointPosition_2" - << ",JointPosition_3" << ",JointPosition_4" << ",JointPosition_5" << ",JointPosition_6" << std::endl; + << ",Joint_0" << ",Joint_1" << ",Joint_2" + << ",Joint_3" << ",Joint_4" << ",Joint_5" << ",Joint_6" << std::endl; for(int i=0; i0) { + device_time_step = device_time(i) - device_time(i-1); + receive_time_step = local_receive_time(i) - local_receive_time(i-1); + } + Eigen::RowVectorXd jointAngle = jointAngles.row(i); + // write the data to the output file + fs << local_receive_time(i) <<"," // B + << local_request_time(i)<< "," // A + << device_time(i) <<"," // C + << device_time(i) - local_receive_time(i) << "," + << receive_request(i) << "," + << device_time_step << "," + << receive_time_step << "," + << jointAngle[0] << "," + << jointAngle[1] << "," + << jointAngle[2] << "," + << jointAngle[3] << "," + << jointAngle[4] << "," + << jointAngle[5] << "," + << jointAngle[6] << std::endl; + } + fs.close(); + } + + + void writeTimeEventToCSV( - std::string & csvfilename, + std::string & CSV_FileName, grl::VectorXd device_time, grl::VectorXd local_request_time, grl::VectorXd local_receive_time){ @@ -199,7 +418,7 @@ namespace grl { // grl::VectorXd device_time_offset = local_receive_time - local_request_time; std::ofstream fs; // create a name for the file output - fs.open( csvfilename, std::ofstream::out | std::ofstream::app); + fs.open( CSV_FileName, std::ofstream::out | std::ofstream::app); // write the file headers fs << "local_request_time_offset," << "local_receive_time_X," @@ -227,10 +446,12 @@ namespace grl { << std::endl; //D } fs.close(); -} + } - void writePoseToCSV( - std::string & csvfilename, + + + void writeEEPoseToCSV( + std::string & CSV_FileName, grl::VectorXd device_time, grl::VectorXd local_request_time, grl::VectorXd local_receive_time, @@ -250,7 +471,7 @@ namespace grl { // grl::VectorXd device_time_offset = local_receive_time - local_request_time; std::ofstream fs; // create a name for the file output - fs.open( csvfilename, std::ofstream::out | std::ofstream::app); + fs.open( CSV_FileName, std::ofstream::out | std::ofstream::app); // write the file headers fs << "local_receive_time_X," << "local_request_time_offset," @@ -287,17 +508,57 @@ namespace grl { << pose(2) << "," << pose(3) << "," << pose(4) << "," + << pose(5) << std::endl; //D + } + fs.close(); + } + void writeMarkerPoseToCSV( + std::string & CSV_FileName, + grl::MatrixXd &timeEventM, + Eigen::MatrixXd &markerPose){ + std::size_t time_size = timeEventM.rows(); + std::size_t marker_size = markerPose.rows(); + assert(time_size == marker_size); + + std::ofstream fs; + // create a name for the file output + fs.open( CSV_FileName, std::ofstream::out | std::ofstream::app); + // write the file headers + fs << "local_receive_time_X," + << "local_request_time_offset," + << "device_time_offset," + << "P_X," + << "P_Y," + << "P_Z," + << "Q_X," + << "Q_Y," + << "Q_Z," + << "Q_W" + << std::endl; + for(int i=0; i KUKAiiwaStatesRoot = fbs_tk::open_root(kukaBinaryfile); @@ -312,14 +573,20 @@ namespace grl { - fbs_tk::Root FusionTrackStatesRoot = fbs_tk::open_root(fusiontrackBinaryfile); - auto FT_states = FusionTrackStatesRoot->states(); + fbs_tk::Root logKUKAiiwaFusionTrackP = fbs_tk::open_root(fusiontrackBinaryfile); + auto FT_states = logKUKAiiwaFusionTrackP->states(); std::size_t FT_state_size = FT_states->size(); - std::cout<< "------FusionTrack State Size: "<< FT_state_size << std::endl; - grl::VectorXd FT_device_time = grl:: getTimeStamp(FusionTrackStatesRoot, grl::fusiontracker_tag(), grl::TimeType::device_time); - grl::VectorXd FT_local_request_time = grl::getTimeStamp(FusionTrackStatesRoot, grl::fusiontracker_tag(), grl::TimeType::local_request_time); - grl::VectorXd FT_local_receive_time = grl::getTimeStamp(FusionTrackStatesRoot, grl::fusiontracker_tag(), grl::TimeType::local_receive_time); + //std::cout<< "------FusionTrack State Size: "<< FT_state_size << std::endl; + grl::VectorXd FT_device_time = grl:: getTimeStamp(logKUKAiiwaFusionTrackP, grl::fusiontracker_tag(), grl::TimeType::device_time); + grl::VectorXd FT_local_request_time = grl::getTimeStamp(logKUKAiiwaFusionTrackP, grl::fusiontracker_tag(), grl::TimeType::local_request_time); + grl::VectorXd FT_local_receive_time = grl::getTimeStamp(logKUKAiiwaFusionTrackP, grl::fusiontracker_tag(), grl::TimeType::local_receive_time); writeTimeEventToCSV(FT_CSVfilename, FT_device_time, FT_local_request_time, FT_local_receive_time); + uint32_t makerID = 22; + // Eigen::MatrixXd markerPose = grl::getMarkerPose(logKUKAiiwaFusionTrackP, makerID); + // std::cout <<"Rows: " << markerPose.rows() << std::endl; + //writeMarkerPoseToCSV(FT_PoseCSVfilename, FT_device_time, FT_local_request_time, FT_local_receive_time, markerPose); + + int kuka_index = 0; int FT_index = 0; // filter out the very beginning data, which can gurantee to record the data when the two devices work simultaniously. @@ -341,6 +608,8 @@ namespace grl { kuka_local_receive_time = kuka_local_receive_time - initial_local_time * grl::VectorXd::Ones(kuka_state_size); kuka_device_time = kuka_device_time - initial_device_time_kuka * grl::VectorXd::Ones(kuka_state_size); + + std::ofstream fs; // create a name for the file output fs.open( FTKUKA_CSVfilename, std::ofstream::out | std::ofstream::app); diff --git a/test/KukaPoseEstimationTest.cpp b/test/KukaPoseEstimationTest.cpp index 1ae809e..cbfc270 100644 --- a/test/KukaPoseEstimationTest.cpp +++ b/test/KukaPoseEstimationTest.cpp @@ -83,14 +83,14 @@ int main(int argc, char* argv[]) grl::VectorXd kuka_local_request_time = grl::getTimeStamp(KUKAiiwaStatesRoot, grl::kuka_tag(), grl::TimeType::local_request_time); grl::VectorXd kuka_local_receive_time = grl::getTimeStamp(KUKAiiwaStatesRoot, grl::kuka_tag(), grl::TimeType::local_receive_time); - Eigen::MatrixXf jointAngles = grl::getAllJointAngles(KUKAiiwaStatesRoot); + Eigen::MatrixXd jointAngles = grl::getAllJointAngles(KUKAiiwaStatesRoot); std::size_t row_size = jointAngles.rows(); std::size_t body_size = mbc.bodyPosW.size(); /// The translation and Euler angles in world coordinate. Eigen::MatrixXd poseEE(row_size,6); for(int rowIdx=0; rowIdx bodyPosW; - // std::size_t nrBodies = mb.nrBodies(); - // std::cout<<"Body Size: "<< nrBodies << std::endl; - // for(int bodyIndex=0; bodyIndex()/2.}}; -// /// @todo TODO(ahundt) warn/error when size!=0 or explicitly handle those cases -// // if(mbc.q[jointIdx].size()>0) simArmConfig.q[jointIdx][0] = futureAngle; -// //auto possize = mbc.q[1].size(); -// std::cout<<"Joint angle: "<< mbc.q[1][0] << std::endl; -// forwardKinematics(mb, mbc); - -// ee = mbc.bodyPosW.size(); -// std::cout<<"size: " << ee << std::endl; -// for(int i=0; i<10; ++i) { -// pos = mbc.bodyPosW[i]; -// std::cout<<"Joint Position: "<< mb.jointPosInParam(i)<< std::endl; -// std::cout << pos << std::endl <<"---------------"< std::string foldname = "/home/chunting/src/V-REP_PRO_EDU_V3_4_0_Linux/"; -std::string kukaBinaryfile = foldname + "2018_02_13_19_06_04_Kukaiiwa.iiwa"; -std::string fusiontrackBinaryfile = foldname + "2018_02_13_19_05_49_FusionTrack.flik"; +std::string kukaBinaryfile = foldname + "2018_02_20_20_27_40_Kukaiiwa.iiwa"; +std::string fusiontrackBinaryfile = foldname + "2018_02_20_21_18_39_FusionTrack.flik"; std::string FTKUKA_CSVfilename = foldname + current_date_and_time_string() + "_FTKUKA.csv"; std::string FT_CSVfilename = foldname + current_date_and_time_string() + "_FT.csv"; +std::string FT_PoseCSVfilename = foldname + current_date_and_time_string() + "_FT_Pose.csv"; std::string KUKA_CSVfilename = foldname + current_date_and_time_string() + "_KUKA.csv"; std::string KUKA_CSVfilename_Joint = foldname + current_date_and_time_string() + "_KUKA_Joint.csv"; +const char *Time_Labels[] = {"local_receive_time_X", "local_request_time_offset", "device_time_offset"}; +const char *FT_Pose_Labels[] = {"Counter", "P_X", "P_Y", "P_Z", "Q_X", "Q_Y", "Q_Z", "Q_W"}; +const char *FT_Labels[] = {Time_Labels[0], Time_Labels[1], Time_Labels[2], FT_Pose_Labels[0], FT_Pose_Labels[1], + FT_Pose_Labels[2], FT_Pose_Labels[3], FT_Pose_Labels[4], FT_Pose_Labels[5], FT_Pose_Labels[6], FT_Pose_Labels[7]}; + int main(int argc, char* argv[]) { - grl::writetoCSVforFusionTrackKukaiiwa(fusiontrackBinaryfile, kukaBinaryfile, FTKUKA_CSVfilename, FT_CSVfilename, KUKA_CSVfilename); - grl::writetoJointAngToCSV(kukaBinaryfile, KUKA_CSVfilename_Joint); + fbs_tk::Root FusionTrackStatesRoot = fbs_tk::open_root(fusiontrackBinaryfile); + grl::MatrixXd timeEventM_FT = grl::getRegularizedTimeStamp(FusionTrackStatesRoot,grl::fusiontracker_tag()); + uint32_t makerID = 22; + Eigen::MatrixXd markerPose = grl::getMarkerPose(FusionTrackStatesRoot, makerID, timeEventM_FT); + /// Combine two matrix into a new one. + // assert(markerPose.rows() == timeEventM_FT.rows()); + // Eigen::MatrixXd FT_Matrix(markerPose.rows(), timeEventM_FT.cols()+markerPose.cols()); + // /// Cast grl::MatrixXd to Eigen::MatrixXd + // FT_Matrix << timeEventM_FT.cast(), markerPose; + std::vector FT_Labels_Pose(FT_Labels, grl::end(FT_Labels)); + grl::writeMatrixToCSV(FT_PoseCSVfilename, FT_Labels_Pose, markerPose); + + // fbs_tk::Root KUKAiiwaStatesRoot = fbs_tk::open_root(kukaBinaryfile); + + // grl::writeFT_KUKATimeEventToCSV(fusiontrackBinaryfile, kukaBinaryfile, FTKUKA_CSVfilename, FT_CSVfilename, FT_PoseCSVfilename, KUKA_CSVfilename); + // grl::writeJointAngToCSV(kukaBinaryfile, KUKA_CSVfilename_Joint); return 0; } From f508a02b0301a3a1db0520bdc9c3516ac06f66ba Mon Sep 17 00:00:00 2001 From: Chunting Date: Sun, 25 Feb 2018 23:34:35 -0500 Subject: [PATCH 050/102] Rewrite the CSV writer --- include/grl/flatbuffer/readDatafromBinary.hpp | 266 +++++++++++------- test/readFRIFBTest.cpp | 58 ++-- 2 files changed, 200 insertions(+), 124 deletions(-) diff --git a/include/grl/flatbuffer/readDatafromBinary.hpp b/include/grl/flatbuffer/readDatafromBinary.hpp index 33b51ed..3261eee 100644 --- a/include/grl/flatbuffer/readDatafromBinary.hpp +++ b/include/grl/flatbuffer/readDatafromBinary.hpp @@ -30,6 +30,10 @@ namespace grl { typedef Eigen::Matrix VectorXd; typedef Eigen::Matrix MatrixXd; + + const char *Time_Labels[] = {"local_receive_time_X", "local_request_time_offset", "device_time_offset", "time_Y"}; + const char *FT_Pose_Labels[] = {"Counter", "P_X", "P_Y", "P_Z", "Q_X", "Q_Y", "Q_Z", "Q_W"}; + const char *Joint_Labels[] = {"Joint_1", "Joint_2", "Joint_3", "Joint_4", "Joint_5", "Joint_6", "Joint_7"}; struct kuka_tag {}; struct fusiontracker_tag {}; enum TimeType { device_time = 0, local_request_time = 1, local_receive_time = 2 }; @@ -39,6 +43,28 @@ namespace grl { T * end(T (&ra)[N]) { return ra + N; } + + + std::vector getLabels(fusiontracker_tag){ + std::vector Time_Labels_Vec(Time_Labels, grl::end(Time_Labels)); + std::vector FT_Pose_Labels_Vec(FT_Pose_Labels, grl::end(FT_Pose_Labels)); + std::vector FT_Labels_Pose; + FT_Labels_Pose.reserve(Time_Labels_Vec.size()+FT_Pose_Labels_Vec.size()); + FT_Labels_Pose.insert(FT_Labels_Pose.end(), Time_Labels_Vec.begin(), Time_Labels_Vec.end()); + FT_Labels_Pose.insert(FT_Labels_Pose.end(), FT_Pose_Labels_Vec.begin(), FT_Pose_Labels_Vec.end()); + return FT_Labels_Pose; + } + + std::vector getLabels(kuka_tag){ + std::vector Time_Labels_Vec(Time_Labels, grl::end(Time_Labels)); + std::vector Joint_Labels_Vec(Joint_Labels, grl::end(Joint_Labels)); + std::vector Kuka_Joint_Labels; + Kuka_Joint_Labels.reserve(Time_Labels_Vec.size()+Joint_Labels_Vec.size()); + Kuka_Joint_Labels.insert(Kuka_Joint_Labels.end(), Time_Labels_Vec.begin(), Time_Labels_Vec.end()); + Kuka_Joint_Labels.insert(Kuka_Joint_Labels.end(), Joint_Labels_Vec.begin(), Joint_Labels_Vec.end()); + return Kuka_Joint_Labels; + } + /// Remove a certain row or colum for the given matrix in Eigen /// See https://stackoverflow.com/questions/13290395/how-to-remove-a-certain-row-or-column-while-using-eigen-library-c template @@ -123,35 +149,52 @@ namespace grl { } return timeStamp; } - void regularizeTimeEvent(grl::VectorXd &local_request_time, grl::VectorXd &local_receive_time, grl::VectorXd &device_time){ - std::size_t device_time_size = device_time.size(); - std::size_t local_request_time_size = local_request_time.size(); - std::size_t local_receive_time_size = local_receive_time.size(); - assert( local_receive_time_size >0 && device_time_size==local_request_time_size && local_request_time_size== local_receive_time_size); + /// Return the original timeEvent + grl::MatrixXd getTimeStamp(const fbs_tk::Root &logKUKAiiwaFusionTrackP, fusiontracker_tag){ - grl::MatrixXd timeEventM(local_receive_time_size,3); - auto initial_local_time = local_receive_time(0); - auto initial_device_time = device_time(0); - local_request_time = local_request_time - initial_local_time * grl::VectorXd::Ones(local_receive_time_size); - local_receive_time = local_receive_time - initial_local_time * grl::VectorXd::Ones(local_receive_time_size); - device_time = device_time - initial_device_time * grl::VectorXd::Ones(local_receive_time_size); - } - /// Use the first timestamp of local_receive_time and device_time as the base to get the time offset - grl::MatrixXd getRegularizedTimeStamp(const fbs_tk::Root &logKUKAiiwaFusionTrackP, fusiontracker_tag){ auto states = logKUKAiiwaFusionTrackP->states(); std::size_t state_size = states->size(); - std::cout<< "------FusionTrack State Size: "<< state_size << std::endl; - grl::VectorXd device_time = grl::getTimeStamp(logKUKAiiwaFusionTrackP, grl::fusiontracker_tag(), grl::TimeType::device_time); - grl::VectorXd local_request_time = grl::getTimeStamp(logKUKAiiwaFusionTrackP, grl::fusiontracker_tag(), grl::TimeType::local_request_time); - grl::VectorXd local_receive_time = grl::getTimeStamp(logKUKAiiwaFusionTrackP, grl::fusiontracker_tag(), grl::TimeType::local_receive_time); - - regularizeTimeEvent(local_request_time, local_receive_time, device_time); - grl::MatrixXd timeEventM(state_size,3); - timeEventM.col(0) = local_receive_time; - timeEventM.col(1) = local_request_time; - timeEventM.col(2) = device_time; + assert(state_size); + grl::MatrixXd timeEventM(state_size,4); + for(int i = 0; iGet(i); + auto timeEvent = kukaiiwaFusionTrackMessage->timeEvent(); + timeEventM(i,0) = timeEvent->local_receive_time(); + timeEventM(i,1) = timeEvent->local_request_time(); + timeEventM(i,2) = timeEvent->device_time(); + timeEventM(i,3) = timeEventM(i,2) - timeEventM(i,0); + } return timeEventM; } + /// Return the original timeEvent + grl::MatrixXd getTimeStamp(const fbs_tk::Root &kukaStatesP, kuka_tag){ + auto states = kukaStatesP->states(); + std::size_t state_size = states->size(); + assert(state_size); + grl::MatrixXd timeEventM(state_size,4); + for(int i = 0; iGet(i); + auto FRIMessage = KUKAiiwaState->FRIMessage(); + auto timeEvent = FRIMessage->timeStamp(); + timeEventM(i,0) = timeEvent->local_receive_time(); + timeEventM(i,1) = timeEvent->local_request_time(); + timeEventM(i,2) = timeEvent->device_time(); + timeEventM(i,3) = timeEventM(i,2) - timeEventM(i,0); + } + return timeEventM; + } + + void regularizeTimeEvent(grl::MatrixXd& timeEventM){ + std::size_t time_size = timeEventM.rows(); + assert(time_size>0); + auto initial_local_time = timeEventM(0,0); + auto initial_device_time = timeEventM(0,3); + timeEventM.col(1) = timeEventM.col(1) - initial_local_time * grl::VectorXd::Ones(time_size); + timeEventM.col(0) = timeEventM.col(0) - initial_local_time * grl::VectorXd::Ones(time_size); + timeEventM.col(2) = timeEventM.col(2) - initial_device_time * grl::VectorXd::Ones(time_size); + timeEventM.col(3) = timeEventM.col(2) - timeEventM.col(0); + } + template bool checkmonotonic( T &time){ for(int i=1; i &kukaStatesP, kuka_tag){ - auto states = kukaStatesP->states(); - std::size_t state_size = states->size(); - std::cout<< "------Kuka State Size: "<< state_size << std::endl; - grl::VectorXd device_time = grl::getTimeStamp(kukaStatesP, grl::kuka_tag(), grl::TimeType::device_time); - grl::VectorXd local_request_time = grl::getTimeStamp(kukaStatesP, grl::kuka_tag(), grl::TimeType::local_request_time); - grl::VectorXd local_receive_time = grl::getTimeStamp(kukaStatesP, grl::kuka_tag(), grl::TimeType::local_receive_time); - regularizeTimeEvent(local_request_time, local_receive_time, device_time); - assert(checkmonotonic(local_receive_time)); - grl::MatrixXd timeEventM(state_size,3); - timeEventM.col(0) = local_receive_time; - timeEventM.col(1) = local_request_time; - timeEventM.col(2) = device_time; - return timeEventM; - } - - Eigen::MatrixXd getMarkerPose(const fbs_tk::Root &logKUKAiiwaFusionTrackP, uint32_t &makerID, grl::MatrixXd& timeEvent){ auto states = logKUKAiiwaFusionTrackP->states(); std::size_t state_size = states->size(); assert(state_size>0); // The first columne is counter - std::size_t cols = 11; + std::size_t cols = 12; int row = 0; Eigen::MatrixXd markerPose(state_size, cols); int BadCount = 0; @@ -217,7 +242,7 @@ namespace grl { if(row < state_size) { markerPose.conservativeResize(row, Eigen::NoChange_t{}); } - std::cout <<"State size: " << state_size <<" markerPose size: " << markerPose.rows() << " timeEvent: " << timeEvent.rows() << " BadCount: " << BadCount <0 && cols_size>0); - std::cout<< t.col(0) < KUKAiiwaStatesRoot = fbs_tk::open_root(kukaBinaryfile); - auto states = KUKAiiwaStatesRoot->states(); - std::size_t kuka_state_size = states->size(); - std::cout<< "------Kuka_state_size: "< &kukaStatesP); - - Eigen::VectorXf jointAngles_0 = grl::getJointAnglesFromKUKA(KUKAiiwaStatesRoot, 0); - Eigen::VectorXf jointAngles_1 = grl::getJointAnglesFromKUKA(KUKAiiwaStatesRoot, 1); - Eigen::VectorXf jointAngles_2 = grl::getJointAnglesFromKUKA(KUKAiiwaStatesRoot, 2); - Eigen::VectorXf jointAngles_3 = grl::getJointAnglesFromKUKA(KUKAiiwaStatesRoot, 3); - Eigen::VectorXf jointAngles_4 = grl::getJointAnglesFromKUKA(KUKAiiwaStatesRoot, 4); - Eigen::VectorXf jointAngles_5 = grl::getJointAnglesFromKUKA(KUKAiiwaStatesRoot, 5); - Eigen::VectorXf jointAngles_6 = grl::getJointAnglesFromKUKA(KUKAiiwaStatesRoot, 6); - - // create an ofstream for the file output (see the link on streams for more info) - - std::ofstream fs; - // create a name for the file output - - fs.open(CSV_FileName, std::ofstream::out | std::ofstream::app); - // write the file headers - fs << "local_request_time" - << ",local_receive_time" - << ",Time_Difference" - << ",Device_Time" - << ",Joint_0" << ",Joint_1" << ",Joint_2" - << ",Joint_3" << ",Joint_4" << ",Joint_5" << ",Joint_6" << std::endl; - for(int i=0; i timeEventM_FT(FT_index,0) && kuka_index == 0){ + FT_index++; + } + + auto initial_local_time = std::min(timeEventM_FT(FT_index,0), timeEventM_Kuka(kuka_index,0)); + auto initial_device_time_kuka = timeEventM_Kuka(kuka_index,2); + auto initial_device_time_FT = timeEventM_FT(FT_index,2); + grl::VectorXd FT_local_request_time = timeEventM_FT.col(1) - initial_local_time * grl::VectorXd::Ones(FT_state_size); + grl::VectorXd FT_local_receive_time = timeEventM_FT.col(0) - initial_local_time * grl::VectorXd::Ones(FT_state_size); + grl::VectorXd FT_device_time = timeEventM_FT.col(2) - initial_device_time_FT * grl::VectorXd::Ones(FT_state_size); + + grl::VectorXd kuka_local_request_time = timeEventM_Kuka.col(1) - initial_local_time * grl::VectorXd::Ones(kuka_state_size); + grl::VectorXd kuka_local_receive_time = timeEventM_Kuka.col(0) - initial_local_time * grl::VectorXd::Ones(kuka_state_size); + grl::VectorXd kuka_device_time = timeEventM_Kuka.col(2) - initial_device_time_kuka * grl::VectorXd::Ones(kuka_state_size); + + + // std::ofstream fs; + // // create a name for the file output + // fs.open( FTKUKA_CSVfilename, std::ofstream::out | std::ofstream::app); + // // write the file headers + // fs << "local_receive_time_offset_X," + // << "FT_local_request_time," + // << "KUKA_local_request_time," + // << "FT_device_time_offset," + // << "device_time_offset_kuka," + // << "Y_FT," + // << "Y_kuka" + // << std::endl; + + // int64_t kuka_diff = kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index); + // int64_t FT_diff = FT_device_time(FT_index) - FT_local_receive_time(FT_index); + // while ( kuka_index < kuka_state_size && FT_index < FT_state_size ) + // { + // if ( kuka_local_receive_time(kuka_index) < FT_local_receive_time(FT_index) ){ + // // write the data to the output file + // fs << kuka_local_receive_time(kuka_index) <<"," + // <<"," + // << kuka_local_request_time(kuka_index) << "," + // << "," + // << kuka_device_time(kuka_index) <<"," + // << "," + // << kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index) - kuka_diff + // << std::endl; + // kuka_index++; + // } else if( kuka_local_receive_time(kuka_index) > FT_local_receive_time(FT_index)) { + // // write the data to the output file + // fs << FT_local_receive_time(FT_index) <<"," + // << FT_local_request_time(FT_index) << "," + // <<"," + // << FT_device_time(FT_index) << "," + // << "," + // << FT_device_time(FT_index) - FT_local_receive_time(FT_index) - FT_diff << "," + // << std::endl; + // FT_index++; + // } else { + // fs << FT_local_receive_time(FT_index) <<"," + // << FT_local_request_time(FT_index) << "," + // << kuka_local_request_time(kuka_index) << "," + // << FT_device_time(FT_index) << "," + // << kuka_device_time(kuka_index) <<"," + // << FT_device_time(FT_index) - FT_local_receive_time(FT_index) - FT_diff << "," + // << kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index) - kuka_diff + // << std::endl; + // FT_index++; + // kuka_index++; + // } + // } + // fs.close(); +} + } #endif \ No newline at end of file diff --git a/test/readFRIFBTest.cpp b/test/readFRIFBTest.cpp index 6f29443..4915162 100644 --- a/test/readFRIFBTest.cpp +++ b/test/readFRIFBTest.cpp @@ -15,36 +15,58 @@ #include #include +#include +#include + std::string foldname = "/home/chunting/src/V-REP_PRO_EDU_V3_4_0_Linux/"; std::string kukaBinaryfile = foldname + "2018_02_20_20_27_40_Kukaiiwa.iiwa"; -std::string fusiontrackBinaryfile = foldname + "2018_02_20_21_18_39_FusionTrack.flik"; +std::string fusiontrackBinaryfile = foldname + "2018_02_23_16_55_17_FusionTrack.flik"; std::string FTKUKA_CSVfilename = foldname + current_date_and_time_string() + "_FTKUKA.csv"; std::string FT_CSVfilename = foldname + current_date_and_time_string() + "_FT.csv"; -std::string FT_PoseCSVfilename = foldname + current_date_and_time_string() + "_FT_Pose.csv"; +std::string FT_Marker22_CSVfilename = foldname + current_date_and_time_string() + "_FT_Marker22.csv"; +std::string FT_Marker55_CSVfilename = foldname + current_date_and_time_string() + "_FT_Marker55.csv"; std::string KUKA_CSVfilename = foldname + current_date_and_time_string() + "_KUKA.csv"; std::string KUKA_CSVfilename_Joint = foldname + current_date_and_time_string() + "_KUKA_Joint.csv"; -const char *Time_Labels[] = {"local_receive_time_X", "local_request_time_offset", "device_time_offset"}; -const char *FT_Pose_Labels[] = {"Counter", "P_X", "P_Y", "P_Z", "Q_X", "Q_Y", "Q_Z", "Q_W"}; -const char *FT_Labels[] = {Time_Labels[0], Time_Labels[1], Time_Labels[2], FT_Pose_Labels[0], FT_Pose_Labels[1], - FT_Pose_Labels[2], FT_Pose_Labels[3], FT_Pose_Labels[4], FT_Pose_Labels[5], FT_Pose_Labels[6], FT_Pose_Labels[7]}; int main(int argc, char* argv[]) { + + /// Write FT data to CSV fbs_tk::Root FusionTrackStatesRoot = fbs_tk::open_root(fusiontrackBinaryfile); - grl::MatrixXd timeEventM_FT = grl::getRegularizedTimeStamp(FusionTrackStatesRoot,grl::fusiontracker_tag()); - uint32_t makerID = 22; - Eigen::MatrixXd markerPose = grl::getMarkerPose(FusionTrackStatesRoot, makerID, timeEventM_FT); - /// Combine two matrix into a new one. - // assert(markerPose.rows() == timeEventM_FT.rows()); - // Eigen::MatrixXd FT_Matrix(markerPose.rows(), timeEventM_FT.cols()+markerPose.cols()); - // /// Cast grl::MatrixXd to Eigen::MatrixXd - // FT_Matrix << timeEventM_FT.cast(), markerPose; - std::vector FT_Labels_Pose(FT_Labels, grl::end(FT_Labels)); - grl::writeMatrixToCSV(FT_PoseCSVfilename, FT_Labels_Pose, markerPose); - - // fbs_tk::Root KUKAiiwaStatesRoot = fbs_tk::open_root(kukaBinaryfile); + grl::MatrixXd timeEventM_FT = grl::getTimeStamp(FusionTrackStatesRoot, grl::fusiontracker_tag()); + grl::regularizeTimeEvent(timeEventM_FT); + + uint32_t makerID_22 = 22; + uint32_t makerID_55 = 55; + Eigen::MatrixXd markerPose_22 = grl::getMarkerPose(FusionTrackStatesRoot, makerID_22, timeEventM_FT); + Eigen::MatrixXd markerPose_55 = grl::getMarkerPose(FusionTrackStatesRoot, makerID_55, timeEventM_FT); + std::vector FT_Labels_Pose = getLabels(grl::fusiontracker_tag()); + if(markerPose_22.rows()>2){ + grl::writeMatrixToCSV(FT_Marker22_CSVfilename, FT_Labels_Pose, markerPose_22); + } + if(markerPose_55.rows()>2) { + grl::writeMatrixToCSV(FT_Marker55_CSVfilename, FT_Labels_Pose, markerPose_55); + } + + /// Write KUKA data to CSV + fbs_tk::Root KUKAiiwaStatesRoot = fbs_tk::open_root(kukaBinaryfile); + grl::MatrixXd timeEventM_Kuka = grl::getTimeStamp(KUKAiiwaStatesRoot,grl::kuka_tag()); + grl::regularizeTimeEvent(timeEventM_Kuka); + + Eigen::MatrixXd jointAngles = grl::getAllJointAngles(KUKAiiwaStatesRoot); + std::vector Kuka_Joint_Labels = getLabels(grl::kuka_tag()); + + if(jointAngles.rows() == timeEventM_Kuka.rows()){ + Eigen::MatrixXd JointWithTime(jointAngles.rows(), jointAngles.cols()+timeEventM_Kuka.cols()); + JointWithTime << timeEventM_Kuka.cast(), jointAngles; + grl::writeMatrixToCSV(KUKA_CSVfilename_Joint, Kuka_Joint_Labels, JointWithTime); + } + + /// + + // grl::writeFT_KUKATimeEventToCSV(fusiontrackBinaryfile, kukaBinaryfile, FTKUKA_CSVfilename, FT_CSVfilename, FT_PoseCSVfilename, KUKA_CSVfilename); // grl::writeJointAngToCSV(kukaBinaryfile, KUKA_CSVfilename_Joint); From 4b2b60747064a80c30d78170e138fd60a806e61b Mon Sep 17 00:00:00 2001 From: Chunting Date: Mon, 26 Feb 2018 20:53:11 -0500 Subject: [PATCH 051/102] Plot the data of both markers --- include/grl/flatbuffer/readDatafromBinary.hpp | 119 +++++++++++------- include/grl/sensor/FusionTrackLogAndTrack.hpp | 17 ++- test/readFRIFBTest.cpp | 31 +++-- 3 files changed, 106 insertions(+), 61 deletions(-) diff --git a/include/grl/flatbuffer/readDatafromBinary.hpp b/include/grl/flatbuffer/readDatafromBinary.hpp index 3261eee..fb11e73 100644 --- a/include/grl/flatbuffer/readDatafromBinary.hpp +++ b/include/grl/flatbuffer/readDatafromBinary.hpp @@ -36,7 +36,10 @@ namespace grl { const char *Joint_Labels[] = {"Joint_1", "Joint_2", "Joint_3", "Joint_4", "Joint_5", "Joint_6", "Joint_7"}; struct kuka_tag {}; struct fusiontracker_tag {}; - enum TimeType { device_time = 0, local_request_time = 1, local_receive_time = 2 }; + int col_timeEvent=4; + int col_Kuka_Joint = 7; + int col_FT_Pose = 8; + enum TimeType { local_receive_time = 0, local_request_time = 1, device_time = 2, time_Y = 3}; /// Help initialize a vector array of strings. /// See https://stackoverflow.com/questions/4268886/initialize-a-vector-array-of-strings template @@ -75,7 +78,6 @@ namespace grl { if( rowToRemove < numRows ) matrix.block(rowToRemove,0,numRows-rowToRemove,numCols) = matrix.bottomRows(numRows-rowToRemove); - matrix.conservativeResize(numRows,numCols); } template @@ -90,6 +92,18 @@ namespace grl { matrix.conservativeResize(numRows,numCols); } + template + bool checkmonotonic( T &time){ + for(int i=1; i &kukaStatesP, kuka_tag, TimeType time_type){ auto states = kukaStatesP->states(); @@ -159,11 +173,13 @@ namespace grl { for(int i = 0; iGet(i); auto timeEvent = kukaiiwaFusionTrackMessage->timeEvent(); - timeEventM(i,0) = timeEvent->local_receive_time(); - timeEventM(i,1) = timeEvent->local_request_time(); - timeEventM(i,2) = timeEvent->device_time(); - timeEventM(i,3) = timeEventM(i,2) - timeEventM(i,0); + timeEventM(i,TimeType::local_receive_time) = timeEvent->local_receive_time(); + timeEventM(i,TimeType::local_request_time) = timeEvent->local_request_time(); + timeEventM(i,TimeType::device_time) = timeEvent->device_time(); + timeEventM(i,TimeType::time_Y) = timeEventM(i,TimeType::device_time) - timeEventM(i,TimeType::local_receive_time); } + grl::VectorXd timeCol = timeEventM.col(TimeType::local_receive_time); + assert(checkmonotonic(timeCol)); return timeEventM; } /// Return the original timeEvent @@ -176,42 +192,42 @@ namespace grl { auto KUKAiiwaState = states->Get(i); auto FRIMessage = KUKAiiwaState->FRIMessage(); auto timeEvent = FRIMessage->timeStamp(); - timeEventM(i,0) = timeEvent->local_receive_time(); - timeEventM(i,1) = timeEvent->local_request_time(); - timeEventM(i,2) = timeEvent->device_time(); - timeEventM(i,3) = timeEventM(i,2) - timeEventM(i,0); + timeEventM(i,TimeType::local_receive_time) = timeEvent->local_receive_time(); + timeEventM(i,TimeType::local_request_time) = timeEvent->local_request_time(); + timeEventM(i,TimeType::device_time) = timeEvent->device_time(); + timeEventM(i,TimeType::time_Y) = timeEventM(i,TimeType::device_time) - timeEventM(i,TimeType::local_receive_time); } + grl::VectorXd timeCol = timeEventM.col(TimeType::local_receive_time); + assert(checkmonotonic(timeCol)); return timeEventM; } void regularizeTimeEvent(grl::MatrixXd& timeEventM){ std::size_t time_size = timeEventM.rows(); assert(time_size>0); - auto initial_local_time = timeEventM(0,0); - auto initial_device_time = timeEventM(0,3); - timeEventM.col(1) = timeEventM.col(1) - initial_local_time * grl::VectorXd::Ones(time_size); - timeEventM.col(0) = timeEventM.col(0) - initial_local_time * grl::VectorXd::Ones(time_size); - timeEventM.col(2) = timeEventM.col(2) - initial_device_time * grl::VectorXd::Ones(time_size); - timeEventM.col(3) = timeEventM.col(2) - timeEventM.col(0); + auto initial_local_time = timeEventM(0,TimeType::local_receive_time); + auto initial_device_time = timeEventM(0,TimeType::device_time); + timeEventM.col(TimeType::local_receive_time) = timeEventM.col(TimeType::local_receive_time) - initial_local_time * grl::VectorXd::Ones(time_size); + timeEventM.col(TimeType::local_request_time) = timeEventM.col(TimeType::local_request_time) - initial_local_time * grl::VectorXd::Ones(time_size); + + timeEventM.col(TimeType::device_time) = timeEventM.col(TimeType::device_time) - initial_device_time * grl::VectorXd::Ones(time_size); + timeEventM.col(TimeType::time_Y) = timeEventM.col(TimeType::device_time) - timeEventM.col(TimeType::local_receive_time); + grl::VectorXd timeCol = timeEventM.col(TimeType::local_receive_time); + assert(checkmonotonic(timeCol)); } - template - bool checkmonotonic( T &time){ - for(int i=1; i &logKUKAiiwaFusionTrackP, uint32_t &makerID, grl::MatrixXd& timeEvent){ + + int getMarkerPose(const fbs_tk::Root &logKUKAiiwaFusionTrackP, + uint32_t &makerID, + grl::MatrixXd& timeEventM, + Eigen::MatrixXd& markerPose){ auto states = logKUKAiiwaFusionTrackP->states(); std::size_t state_size = states->size(); assert(state_size>0); // The first columne is counter - std::size_t cols = 12; int row = 0; - Eigen::MatrixXd markerPose(state_size, cols); + // Eigen::MatrixXd markerPose(state_size, grl::col_FT_Pose); int BadCount = 0; for(int i = 0; iGet(i); @@ -226,24 +242,30 @@ namespace grl { auto marker = Makers->Get(markerIndex); auto marker_ID = marker->geometryID(); if(marker_ID == makerID){ - // markerPose.conservativeResize(row + 1, cols); + + + auto timeEvent = kukaiiwaFusionTrackMessage->timeEvent(); + timeEventM(row,TimeType::local_receive_time) = timeEvent->local_receive_time(); + timeEventM(row,TimeType::local_request_time) = timeEvent->local_request_time(); + timeEventM(row,TimeType::device_time) = timeEvent->device_time(); + timeEventM(row,TimeType::time_Y) = timeEventM(row,TimeType::device_time) - timeEventM(row,TimeType::local_receive_time); + auto Pose = marker->transform(); auto position = Pose->position(); auto orientationQtn = Pose->orientation(); - Eigen::VectorXd onePose(cols); - grl::VectorXd oneTime = timeEvent.row(i); - onePose << oneTime.cast(), counter,1000* position.x(), 1000*position.y(), 1000*position.z(), orientationQtn.x(), orientationQtn.y(), orientationQtn.z(), orientationQtn.w(); + Eigen::VectorXd onePose(grl::col_FT_Pose); + onePose << counter,1000* position.x(), 1000*position.y(), 1000*position.z(), orientationQtn.x(), orientationQtn.y(), orientationQtn.z(), orientationQtn.w(); markerPose.row(row++) = onePose.transpose(); - continue; } } } } if(row < state_size) { markerPose.conservativeResize(row, Eigen::NoChange_t{}); + timeEventM.conservativeResize(row, Eigen::NoChange_t{}); } - std::cout <<"State size: " << state_size <<" markerPose size: " << markerPose.rows() << " timeEvent: " << timeEvent.rows() << " makerID: " << makerID < - void writeMatrixToCSV(const std::string& CSV_FileName, std::vector &labels, T& t){ + template + void writeMatrixToCSV(const std::string& CSV_FileName, std::vector &labels, T1& timeM, T2& t2){ std::size_t labels_size = labels.size(); - std::size_t cols_size = t.cols(); - std::size_t rows_size = t.rows(); - assert(labels_size == cols_size && rows_size>0 && cols_size>0); + std::size_t cols_size = timeM.cols() + t2.cols(); + std::size_t time_rows_size = timeM.rows(); + std::size_t t2_rows_size = t2.rows(); + assert(labels_size == cols_size && time_rows_size>0 && cols_size>0 && time_rows_size==t2_rows_size); + auto time = timeM.col(TimeType::local_receive_time); + assert(checkmonotonic(time)); // create an ofstream for the file output (see the link on streams for more info) std::ofstream fs; // create a name for the file output @@ -296,13 +321,17 @@ namespace grl { fs << labels[col] << ","; } fs << labels[cols_size-1] << std::endl; - for(int row_index=0; row_indexMarkers.size() << std::endl; for(const auto &marker : m_receivedFrame->Markers) { - // std::cout << "marker geometryid: " << marker.geometryId << std::endl; + // std::cout << "marker geometryid: " << marker.geometryId << std::endl; cameraToMarkerTransform = sensor::ftkMarkerToAffine3f(marker); auto configIterator = m_geometryIDToMotionConfigParams.find(marker.geometryId); - if (configIterator == m_geometryIDToMotionConfigParams.end()) continue; // no configuration for this item + if (configIterator == m_geometryIDToMotionConfigParams.end()) { + // std::cout << "marker geometryid: " << marker.geometryId << " NOT found config!" << std::endl; + continue; // no configuration for this item + } auto config = configIterator->second; - // std::cout << "<<<<<<<<<<<<<<<< found config!" << std::endl; + // invert the transform from the tracker to the object if needed if (m_opticalTrackerBase == std::get(config) && @@ -329,6 +335,7 @@ class FusionTrackLogAndTrack : public std::enable_shared_from_this(config), std::get(config), cameraToMarkerTransform)); } + // std::cout << "Pose size: " << poses.size() << std::endl; return poses; } @@ -439,8 +446,8 @@ class FusionTrackLogAndTrack : public std::enable_shared_from_this deviceSerialNumbers; try { diff --git a/test/readFRIFBTest.cpp b/test/readFRIFBTest.cpp index 4915162..ad941af 100644 --- a/test/readFRIFBTest.cpp +++ b/test/readFRIFBTest.cpp @@ -19,8 +19,8 @@ #include std::string foldname = "/home/chunting/src/V-REP_PRO_EDU_V3_4_0_Linux/"; -std::string kukaBinaryfile = foldname + "2018_02_20_20_27_40_Kukaiiwa.iiwa"; -std::string fusiontrackBinaryfile = foldname + "2018_02_23_16_55_17_FusionTrack.flik"; +std::string kukaBinaryfile = foldname + "2018_02_26_14_23_14_Kukaiiwa.iiwa"; +std::string fusiontrackBinaryfile = foldname + "2018_02_26_14_30_17_FusionTrack.flik"; std::string FTKUKA_CSVfilename = foldname + current_date_and_time_string() + "_FTKUKA.csv"; std::string FT_CSVfilename = foldname + current_date_and_time_string() + "_FT.csv"; std::string FT_Marker22_CSVfilename = foldname + current_date_and_time_string() + "_FT_Marker22.csv"; @@ -37,17 +37,26 @@ int main(int argc, char* argv[]) fbs_tk::Root FusionTrackStatesRoot = fbs_tk::open_root(fusiontrackBinaryfile); grl::MatrixXd timeEventM_FT = grl::getTimeStamp(FusionTrackStatesRoot, grl::fusiontracker_tag()); grl::regularizeTimeEvent(timeEventM_FT); + std::size_t FT_size = timeEventM_FT.rows(); uint32_t makerID_22 = 22; uint32_t makerID_55 = 55; - Eigen::MatrixXd markerPose_22 = grl::getMarkerPose(FusionTrackStatesRoot, makerID_22, timeEventM_FT); - Eigen::MatrixXd markerPose_55 = grl::getMarkerPose(FusionTrackStatesRoot, makerID_55, timeEventM_FT); + grl::MatrixXd timeEventM_FT_22(FT_size, grl::col_timeEvent); + + Eigen::MatrixXd markerPose_22(FT_size, grl::col_FT_Pose); + int validsize_22 = grl::getMarkerPose(FusionTrackStatesRoot, makerID_22, timeEventM_FT_22, markerPose_22); + grl::regularizeTimeEvent(timeEventM_FT_22); + Eigen::MatrixXd markerPose_55(FT_size, grl::col_FT_Pose); + grl::MatrixXd timeEventM_FT_55(FT_size, grl::col_timeEvent); + int validsize_55 = grl::getMarkerPose(FusionTrackStatesRoot, makerID_55, timeEventM_FT_55, markerPose_55); + grl::regularizeTimeEvent(timeEventM_FT_55); + std::cout<<"validsize_22: " << validsize_22 << " validsize_55: " << validsize_55 << std::endl; std::vector FT_Labels_Pose = getLabels(grl::fusiontracker_tag()); - if(markerPose_22.rows()>2){ - grl::writeMatrixToCSV(FT_Marker22_CSVfilename, FT_Labels_Pose, markerPose_22); + if(validsize_22>2){ + grl::writeMatrixToCSV(FT_Marker22_CSVfilename, FT_Labels_Pose, timeEventM_FT_22, markerPose_22); } - if(markerPose_55.rows()>2) { - grl::writeMatrixToCSV(FT_Marker55_CSVfilename, FT_Labels_Pose, markerPose_55); + if(validsize_55>2) { + grl::writeMatrixToCSV(FT_Marker55_CSVfilename, FT_Labels_Pose, timeEventM_FT_55, markerPose_55); } /// Write KUKA data to CSV @@ -59,9 +68,9 @@ int main(int argc, char* argv[]) std::vector Kuka_Joint_Labels = getLabels(grl::kuka_tag()); if(jointAngles.rows() == timeEventM_Kuka.rows()){ - Eigen::MatrixXd JointWithTime(jointAngles.rows(), jointAngles.cols()+timeEventM_Kuka.cols()); - JointWithTime << timeEventM_Kuka.cast(), jointAngles; - grl::writeMatrixToCSV(KUKA_CSVfilename_Joint, Kuka_Joint_Labels, JointWithTime); + // Eigen::MatrixXd JointWithTime(jointAngles.rows(), jointAngles.cols()+timeEventM_Kuka.cols()); + // JointWithTime << timeEventM_Kuka.cast(), jointAngles; + grl::writeMatrixToCSV(KUKA_CSVfilename_Joint, Kuka_Joint_Labels, timeEventM_Kuka, jointAngles); } /// From 4a922e27524f4a45836c0ed5802252d899cae4e1 Mon Sep 17 00:00:00 2001 From: Chunting Date: Tue, 27 Feb 2018 17:49:27 -0500 Subject: [PATCH 052/102] Improve some codes and add new features --- include/grl/flatbuffer/readDatafromBinary.hpp | 304 +++--------------- test/KukaPoseEstimationTest.cpp | 28 +- test/readFRIFBTest.cpp | 88 ++--- 3 files changed, 114 insertions(+), 306 deletions(-) diff --git a/include/grl/flatbuffer/readDatafromBinary.hpp b/include/grl/flatbuffer/readDatafromBinary.hpp index fb11e73..2738e09 100644 --- a/include/grl/flatbuffer/readDatafromBinary.hpp +++ b/include/grl/flatbuffer/readDatafromBinary.hpp @@ -243,7 +243,6 @@ namespace grl { auto marker_ID = marker->geometryID(); if(marker_ID == makerID){ - auto timeEvent = kukaiiwaFusionTrackMessage->timeEvent(); timeEventM(row,TimeType::local_receive_time) = timeEvent->local_receive_time(); timeEventM(row,TimeType::local_request_time) = timeEvent->local_request_time(); @@ -304,11 +303,11 @@ namespace grl { return allJointPosition; } template - void writeMatrixToCSV(const std::string& CSV_FileName, std::vector &labels, T1& timeM, T2& t2){ + void writeMatrixToCSV(const std::string& CSV_FileName, std::vector &labels, T1& timeM, T2& dataM){ std::size_t labels_size = labels.size(); - std::size_t cols_size = timeM.cols() + t2.cols(); + std::size_t cols_size = timeM.cols() + dataM.cols(); std::size_t time_rows_size = timeM.rows(); - std::size_t t2_rows_size = t2.rows(); + std::size_t t2_rows_size = dataM.rows(); assert(labels_size == cols_size && time_rows_size>0 && cols_size>0 && time_rows_size==t2_rows_size); auto time = timeM.col(TimeType::local_receive_time); assert(checkmonotonic(time)); @@ -324,101 +323,33 @@ namespace grl { for(int row_index=0; row_index0) { - device_time_step = device_time(i) - device_time(i-1); - receive_time_step = local_receive_time(i) - local_receive_time(i-1); - } - Eigen::RowVectorXd jointAngle = jointAngles.row(i); - // write the data to the output file - fs << local_receive_time(i) <<"," // B - << local_request_time(i)<< "," // A - << device_time(i) <<"," // C - << device_time(i) - local_receive_time(i) << "," - << receive_request(i) << "," - << device_time_step << "," - << receive_time_step << "," - << jointAngle[0] << "," - << jointAngle[1] << "," - << jointAngle[2] << "," - << jointAngle[3] << "," - << jointAngle[4] << "," - << jointAngle[5] << "," - << jointAngle[6] << std::endl; - } - fs.close(); - } - - - - void writeTimeEventToCSV( - - std::string & CSV_FileName, - grl::VectorXd device_time, - grl::VectorXd local_request_time, - grl::VectorXd local_receive_time){ - - std::size_t size = device_time.size(); - // auto initial_local_time = local_request_time(0); - auto initial_local_time = local_receive_time(0); - auto initial_device_time = device_time(0); - local_request_time = local_request_time - initial_local_time * grl::VectorXd::Ones(size); - local_receive_time = local_receive_time - initial_local_time * grl::VectorXd::Ones(size); - device_time = device_time - initial_device_time * grl::VectorXd::Ones(size); - grl::VectorXd receive_request = local_receive_time - local_request_time; + std::size_t time_size = timeEventM.rows(); + grl::VectorXd local_receive_timeV = timeEventM.col(local_receive_time); + grl::VectorXd local_request_timeV = timeEventM.col(local_request_time); + grl::VectorXd device_timeV = timeEventM.col(device_time); + grl::VectorXd receive_request = local_receive_timeV - local_request_timeV; // grl::VectorXd device_time_offset = local_receive_time - local_request_time; std::ofstream fs; // create a name for the file output fs.open( CSV_FileName, std::ofstream::out | std::ofstream::app); // write the file headers - fs << "local_request_time_offset," - << "local_receive_time_X," + fs << "local_receive_time_X," + << "local_request_time_offset," << "device_time_offset," << "Y," << "Receive-Request," @@ -427,16 +358,16 @@ namespace grl { << std::endl; int64_t device_time_step = 0; int64_t receive_time_step = 0; - for(int i=0; i0) { - device_time_step = device_time(i) - device_time(i-1); - receive_time_step = local_receive_time(i) - local_receive_time(i-1); + device_time_step = device_timeV(i) - device_timeV(i-1); + receive_time_step = local_receive_timeV(i) - local_receive_timeV(i-1); } // write the data to the output file - fs << local_request_time(i)<< "," // A - << local_receive_time(i) <<"," // B - << device_time(i) <<"," // C - << device_time(i) - local_receive_time(i) << "," + fs << local_receive_timeV(i) << "," // A + << local_request_timeV(i)<<"," // B + << device_timeV(i) <<"," // C + << device_timeV(i) - local_receive_timeV(i) << "," << receive_request(i) << "," << device_time_step << "," << receive_time_step << "," @@ -509,107 +440,44 @@ namespace grl { } fs.close(); } - void writeMarkerPoseToCSV( - std::string & CSV_FileName, - grl::MatrixXd &timeEventM, - Eigen::MatrixXd &markerPose){ - std::size_t time_size = timeEventM.rows(); - std::size_t marker_size = markerPose.rows(); - assert(time_size == marker_size); - - std::ofstream fs; - // create a name for the file output - fs.open( CSV_FileName, std::ofstream::out | std::ofstream::app); - // write the file headers - fs << "local_receive_time_X," - << "local_request_time_offset," - << "device_time_offset," - << "P_X," - << "P_Y," - << "P_Z," - << "Q_X," - << "Q_Y," - << "Q_Z," - << "Q_W" - << std::endl; - for(int i=0; i KUKAiiwaStatesRoot = - fbs_tk::open_root(kukaBinaryfile); - auto KUKA_states = KUKAiiwaStatesRoot->states(); - std::size_t kuka_state_size = KUKA_states->size(); - std::cout<< "------Kuka_state_size: "<< kuka_state_size << std::endl; - - grl::VectorXd kuka_device_time = grl::getTimeStamp(KUKAiiwaStatesRoot, grl::kuka_tag(), grl::TimeType::device_time); - grl::VectorXd kuka_local_request_time = grl::getTimeStamp(KUKAiiwaStatesRoot, grl::kuka_tag(), grl::TimeType::local_request_time); - grl::VectorXd kuka_local_receive_time = grl::getTimeStamp(KUKAiiwaStatesRoot, grl::kuka_tag(), grl::TimeType::local_receive_time); - writeTimeEventToCSV(KUKA_CSVfilename, kuka_device_time, kuka_local_request_time, kuka_local_receive_time); - - - - fbs_tk::Root logKUKAiiwaFusionTrackP = fbs_tk::open_root(fusiontrackBinaryfile); - auto FT_states = logKUKAiiwaFusionTrackP->states(); - std::size_t FT_state_size = FT_states->size(); - //std::cout<< "------FusionTrack State Size: "<< FT_state_size << std::endl; - grl::VectorXd FT_device_time = grl:: getTimeStamp(logKUKAiiwaFusionTrackP, grl::fusiontracker_tag(), grl::TimeType::device_time); - grl::VectorXd FT_local_request_time = grl::getTimeStamp(logKUKAiiwaFusionTrackP, grl::fusiontracker_tag(), grl::TimeType::local_request_time); - grl::VectorXd FT_local_receive_time = grl::getTimeStamp(logKUKAiiwaFusionTrackP, grl::fusiontracker_tag(), grl::TimeType::local_receive_time); - writeTimeEventToCSV(FT_CSVfilename, FT_device_time, FT_local_request_time, FT_local_receive_time); - uint32_t makerID = 22; - // Eigen::MatrixXd markerPose = grl::getMarkerPose(logKUKAiiwaFusionTrackP, makerID); - // std::cout <<"Rows: " << markerPose.rows() << std::endl; - //writeMarkerPoseToCSV(FT_PoseCSVfilename, FT_device_time, FT_local_request_time, FT_local_receive_time, markerPose); +void writeFTKUKATimeEventToCSV(std::string& FTKUKA_TimeEvent_CSVfilename, + const fbs_tk::Root &logKUKAiiwaFusionTrackP, + const fbs_tk::Root &kukaStatesP) { + grl::MatrixXd timeEventM_FT = grl::getTimeStamp(logKUKAiiwaFusionTrackP, grl::fusiontracker_tag()); + grl::MatrixXd timeEventM_Kuka = grl::getTimeStamp(kukaStatesP, grl::kuka_tag()); + std::size_t kuka_state_size = timeEventM_Kuka.rows(); + std::cout<< "------Kuka_state_size: "<< timeEventM_Kuka.rows() << std::endl; + std::size_t FT_state_size = timeEventM_FT.rows(); + std::cout<< "------FT_state_size: "<< timeEventM_FT.rows() << std::endl; int kuka_index = 0; int FT_index = 0; + // filter out the very beginning data, which can gurantee to record the data when the two devices work simultaniously. - while(kuka_local_receive_time(kuka_index) < FT_local_receive_time(FT_index)){ + while(kuka_index FT_local_receive_time(FT_index) && kuka_index == 0){ + while(FT_index timeEventM_FT(FT_index,local_receive_time) && kuka_index == 0){ FT_index++; } - auto initial_local_time = std::min(FT_local_receive_time(FT_index), kuka_local_receive_time(kuka_index)); - auto initial_device_time_kuka = kuka_device_time(kuka_index); - auto initial_device_time_FT = FT_device_time(FT_index); - FT_local_request_time = FT_local_request_time - initial_local_time * grl::VectorXd::Ones(FT_state_size); - FT_local_receive_time = FT_local_receive_time - initial_local_time * grl::VectorXd::Ones(FT_state_size); - FT_device_time = FT_device_time - initial_device_time_FT * grl::VectorXd::Ones(FT_state_size); - - kuka_local_request_time = kuka_local_request_time - initial_local_time * grl::VectorXd::Ones(kuka_state_size); - kuka_local_receive_time = kuka_local_receive_time - initial_local_time * grl::VectorXd::Ones(kuka_state_size); - kuka_device_time = kuka_device_time - initial_device_time_kuka * grl::VectorXd::Ones(kuka_state_size); + auto initial_local_time = std::min(timeEventM_FT(FT_index,local_receive_time), timeEventM_Kuka(kuka_index,local_receive_time)); + auto initial_device_time_kuka = timeEventM_Kuka(kuka_index,device_time); + auto initial_device_time_FT = timeEventM_FT(FT_index,device_time); + grl::VectorXd FT_local_request_time = timeEventM_FT.col(local_request_time) - initial_local_time * grl::VectorXd::Ones(FT_state_size); + grl::VectorXd FT_local_receive_time = timeEventM_FT.col(local_receive_time) - initial_local_time * grl::VectorXd::Ones(FT_state_size); + grl::VectorXd FT_device_time = timeEventM_FT.col(device_time) - initial_device_time_FT * grl::VectorXd::Ones(FT_state_size); + grl::VectorXd kuka_local_request_time = timeEventM_Kuka.col(local_request_time) - initial_local_time * grl::VectorXd::Ones(kuka_state_size); + grl::VectorXd kuka_local_receive_time = timeEventM_Kuka.col(local_receive_time) - initial_local_time * grl::VectorXd::Ones(kuka_state_size); + grl::VectorXd kuka_device_time = timeEventM_Kuka.col(device_time) - initial_device_time_kuka * grl::VectorXd::Ones(kuka_state_size); std::ofstream fs; // create a name for the file output - fs.open( FTKUKA_CSVfilename, std::ofstream::out | std::ofstream::app); + fs.open( FTKUKA_TimeEvent_CSVfilename, std::ofstream::out | std::ofstream::app); // write the file headers fs << "local_receive_time_offset_X," << "FT_local_request_time," @@ -661,91 +529,5 @@ namespace grl { fs.close(); } - -void AnalysizeFTKUKATimeEvent(grl::MatrixXd& timeEventM_FT, grl::MatrixXd& timeEventM_Kuka) -{ - - std::size_t kuka_state_size = timeEventM_Kuka.rows(); - std::cout<< "------Kuka_state_size: "<< timeEventM_Kuka.rows() << std::endl; - - std::size_t FT_state_size = timeEventM_FT.rows(); - std::cout<< "------FT_state_size: "<< timeEventM_FT.rows() << std::endl; - int kuka_index = 0; - int FT_index = 0; - - // filter out the very beginning data, which can gurantee to record the data when the two devices work simultaniously. - while(timeEventM_Kuka(kuka_index,0) < timeEventM_FT(FT_index,0)){ - kuka_index++; - } - while(timeEventM_Kuka(kuka_index,0) > timeEventM_FT(FT_index,0) && kuka_index == 0){ - FT_index++; - } - - auto initial_local_time = std::min(timeEventM_FT(FT_index,0), timeEventM_Kuka(kuka_index,0)); - auto initial_device_time_kuka = timeEventM_Kuka(kuka_index,2); - auto initial_device_time_FT = timeEventM_FT(FT_index,2); - grl::VectorXd FT_local_request_time = timeEventM_FT.col(1) - initial_local_time * grl::VectorXd::Ones(FT_state_size); - grl::VectorXd FT_local_receive_time = timeEventM_FT.col(0) - initial_local_time * grl::VectorXd::Ones(FT_state_size); - grl::VectorXd FT_device_time = timeEventM_FT.col(2) - initial_device_time_FT * grl::VectorXd::Ones(FT_state_size); - - grl::VectorXd kuka_local_request_time = timeEventM_Kuka.col(1) - initial_local_time * grl::VectorXd::Ones(kuka_state_size); - grl::VectorXd kuka_local_receive_time = timeEventM_Kuka.col(0) - initial_local_time * grl::VectorXd::Ones(kuka_state_size); - grl::VectorXd kuka_device_time = timeEventM_Kuka.col(2) - initial_device_time_kuka * grl::VectorXd::Ones(kuka_state_size); - - - // std::ofstream fs; - // // create a name for the file output - // fs.open( FTKUKA_CSVfilename, std::ofstream::out | std::ofstream::app); - // // write the file headers - // fs << "local_receive_time_offset_X," - // << "FT_local_request_time," - // << "KUKA_local_request_time," - // << "FT_device_time_offset," - // << "device_time_offset_kuka," - // << "Y_FT," - // << "Y_kuka" - // << std::endl; - - // int64_t kuka_diff = kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index); - // int64_t FT_diff = FT_device_time(FT_index) - FT_local_receive_time(FT_index); - // while ( kuka_index < kuka_state_size && FT_index < FT_state_size ) - // { - // if ( kuka_local_receive_time(kuka_index) < FT_local_receive_time(FT_index) ){ - // // write the data to the output file - // fs << kuka_local_receive_time(kuka_index) <<"," - // <<"," - // << kuka_local_request_time(kuka_index) << "," - // << "," - // << kuka_device_time(kuka_index) <<"," - // << "," - // << kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index) - kuka_diff - // << std::endl; - // kuka_index++; - // } else if( kuka_local_receive_time(kuka_index) > FT_local_receive_time(FT_index)) { - // // write the data to the output file - // fs << FT_local_receive_time(FT_index) <<"," - // << FT_local_request_time(FT_index) << "," - // <<"," - // << FT_device_time(FT_index) << "," - // << "," - // << FT_device_time(FT_index) - FT_local_receive_time(FT_index) - FT_diff << "," - // << std::endl; - // FT_index++; - // } else { - // fs << FT_local_receive_time(FT_index) <<"," - // << FT_local_request_time(FT_index) << "," - // << kuka_local_request_time(kuka_index) << "," - // << FT_device_time(FT_index) << "," - // << kuka_device_time(kuka_index) <<"," - // << FT_device_time(FT_index) - FT_local_receive_time(FT_index) - FT_diff << "," - // << kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index) - kuka_diff - // << std::endl; - // FT_index++; - // kuka_index++; - // } - // } - // fs.close(); -} - } #endif \ No newline at end of file diff --git a/test/KukaPoseEstimationTest.cpp b/test/KukaPoseEstimationTest.cpp index cbfc270..3323401 100644 --- a/test/KukaPoseEstimationTest.cpp +++ b/test/KukaPoseEstimationTest.cpp @@ -53,17 +53,29 @@ #include #include "kukaiiwaURDF.h" +#include + // const double PI = 3.14159265359; const double RadtoDegree = 180/3.14159265359; const double MeterToMM = 1000; std::string foldname = "/home/chunting/src/V-REP_PRO_EDU_V3_4_0_Linux/"; -std::string kukaBinaryfile = foldname + "2018_02_20_21_21_08_Kukaiiwa.iiwa"; -// std::string fusiontrackBinaryfile = foldname + "2018_02_20_20_27_23_FusionTrack.flik"; -// std::string FTKUKA_CSVfilename = foldname + current_date_and_time_string() + "_FTKUKA.csv"; -// std::string FT_CSVfilename = foldname + current_date_and_time_string() + "_FT.csv"; -std::string KUKA_CSVfilename = foldname + current_date_and_time_string() + "_KUKA.csv"; -std::string KUKA_CSVfilename_Joint = foldname + current_date_and_time_string() + "_KUKA_Joint.csv"; + +std::string foldtimestamp = current_date_and_time_string(); + +// mkdir(foldname+foldtimestamp, 0777); +std::string kukaBinaryfile = foldname + "2018_02_26_14_23_14_Kukaiiwa.iiwa"; +std::string KUKA_TimeEvent_CSVfilename = foldname + foldtimestamp + "/KUKA_TimeEvent.csv"; +std::string KUKA_Joint_CSVfilename = foldname + foldtimestamp + "/KUKA_Joint.csv"; + +std::string fusiontrackBinaryfile = foldname + "2018_02_26_14_19_55_FusionTrack.flik"; + +std::string FT_TimeEvent_CSVfilename = foldname + foldtimestamp + "/FT_TimeEvent.csv"; +std::string FT_Marker22_CSVfilename = foldname + foldtimestamp + "/FT_Pose_Marker22.csv"; +std::string FT_Marker55_CSVfilename = foldname + foldtimestamp + "/FT_Pose_Marker55.csv"; + +std::string FTKUKA_TimeEvent_CSVfilename = foldname + foldtimestamp + "/FTKUKA_TimeEvent.csv"; + int main(int argc, char* argv[]) { @@ -120,9 +132,9 @@ int main(int argc, char* argv[]) } // std::cout << poseEE << std::endl; - grl::writeEEPoseToCSV(KUKA_CSVfilename, kuka_device_time, kuka_local_request_time, kuka_local_receive_time, poseEE); + grl::writeEEPoseToCSV(KUKA_TimeEvent_CSVfilename, kuka_device_time, kuka_local_request_time, kuka_local_receive_time, poseEE); jointAngles = RadtoDegree*jointAngles; - grl::writeJointAngToCSV(KUKA_CSVfilename_Joint, kuka_device_time, kuka_local_request_time, kuka_local_receive_time, jointAngles); + // grl::writeJointAngToCSV(KUKA_Joint_CSVfilename, kuka_device_time, kuka_local_request_time, kuka_local_receive_time, jointAngles); diff --git a/test/readFRIFBTest.cpp b/test/readFRIFBTest.cpp index ad941af..cf3e22b 100644 --- a/test/readFRIFBTest.cpp +++ b/test/readFRIFBTest.cpp @@ -17,68 +17,82 @@ #include #include - -std::string foldname = "/home/chunting/src/V-REP_PRO_EDU_V3_4_0_Linux/"; -std::string kukaBinaryfile = foldname + "2018_02_26_14_23_14_Kukaiiwa.iiwa"; -std::string fusiontrackBinaryfile = foldname + "2018_02_26_14_30_17_FusionTrack.flik"; -std::string FTKUKA_CSVfilename = foldname + current_date_and_time_string() + "_FTKUKA.csv"; -std::string FT_CSVfilename = foldname + current_date_and_time_string() + "_FT.csv"; -std::string FT_Marker22_CSVfilename = foldname + current_date_and_time_string() + "_FT_Marker22.csv"; -std::string FT_Marker55_CSVfilename = foldname + current_date_and_time_string() + "_FT_Marker55.csv"; -std::string KUKA_CSVfilename = foldname + current_date_and_time_string() + "_KUKA.csv"; -std::string KUKA_CSVfilename_Joint = foldname + current_date_and_time_string() + "_KUKA_Joint.csv"; - +/// Boost to create an empty folder +#include int main(int argc, char* argv[]) { + std::string foldname = "/home/chunting/src/V-REP_PRO_EDU_V3_4_0_Linux/"; + std::string kukaBinaryfile = foldname + "2018_02_26_14_23_14_Kukaiiwa.iiwa"; + std::string fusiontrackBinaryfile = foldname + "2018_02_26_14_19_55_FusionTrack.flik"; + std::string foldtimestamp = current_date_and_time_string(); + boost::filesystem::path dir{foldname+foldtimestamp}; + + boost::filesystem::create_directory(dir); + // boost::filesystem::path dir_file{kukaBinaryfile}; + // boost::filesystem::copy_file(dir_file, dir); + // dir_file = boost::filesystem::path(fusiontrackBinaryfile); + // boost::filesystem::copy_file(dir_file, dir); + + + std::string KUKA_TimeEvent_CSVfilename = foldname + foldtimestamp + "/KUKA_TimeEvent.csv"; + std::string KUKA_Joint_CSVfilename = foldname + foldtimestamp + "/KUKA_Joint.csv"; + + std::string FT_TimeEvent_CSVfilename = foldname + foldtimestamp + "/FT_TimeEvent.csv"; + std::string FT_Marker22_CSVfilename = foldname + foldtimestamp + "/FT_Pose_Marker22.csv"; + std::string FT_Marker55_CSVfilename = foldname + foldtimestamp + "/FT_Pose_Marker55.csv"; + + std::string FTKUKA_TimeEvent_CSVfilename = foldname + foldtimestamp + "/FTKUKA_TimeEvent.csv"; /// Write FT data to CSV - fbs_tk::Root FusionTrackStatesRoot = fbs_tk::open_root(fusiontrackBinaryfile); - grl::MatrixXd timeEventM_FT = grl::getTimeStamp(FusionTrackStatesRoot, grl::fusiontracker_tag()); + fbs_tk::Root logKUKAiiwaFusionTrackP = fbs_tk::open_root(fusiontrackBinaryfile); + grl::MatrixXd timeEventM_FT = grl::getTimeStamp(logKUKAiiwaFusionTrackP, grl::fusiontracker_tag()); grl::regularizeTimeEvent(timeEventM_FT); + grl::writeTimeEventToCSV(FT_TimeEvent_CSVfilename, timeEventM_FT); std::size_t FT_size = timeEventM_FT.rows(); uint32_t makerID_22 = 22; uint32_t makerID_55 = 55; - grl::MatrixXd timeEventM_FT_22(FT_size, grl::col_timeEvent); - - Eigen::MatrixXd markerPose_22(FT_size, grl::col_FT_Pose); - int validsize_22 = grl::getMarkerPose(FusionTrackStatesRoot, makerID_22, timeEventM_FT_22, markerPose_22); - grl::regularizeTimeEvent(timeEventM_FT_22); - Eigen::MatrixXd markerPose_55(FT_size, grl::col_FT_Pose); - grl::MatrixXd timeEventM_FT_55(FT_size, grl::col_timeEvent); - int validsize_55 = grl::getMarkerPose(FusionTrackStatesRoot, makerID_55, timeEventM_FT_55, markerPose_55); - grl::regularizeTimeEvent(timeEventM_FT_55); - std::cout<<"validsize_22: " << validsize_22 << " validsize_55: " << validsize_55 << std::endl; + //timeEvent_FT = grl::MatrixXd::Zero(); + Eigen::MatrixXd markerPose(FT_size, grl::col_FT_Pose); + int validsize_22 = grl::getMarkerPose(logKUKAiiwaFusionTrackP, makerID_22, timeEventM_FT, markerPose); + + grl::regularizeTimeEvent(timeEventM_FT); std::vector FT_Labels_Pose = getLabels(grl::fusiontracker_tag()); if(validsize_22>2){ - grl::writeMatrixToCSV(FT_Marker22_CSVfilename, FT_Labels_Pose, timeEventM_FT_22, markerPose_22); + grl::writeMatrixToCSV(FT_Marker22_CSVfilename, FT_Labels_Pose, timeEventM_FT, markerPose); } + + timeEventM_FT.resize(FT_size, grl::col_timeEvent); + + markerPose.resize(FT_size, grl::col_FT_Pose); + //timeEvent_FT = grl::MatrixXd::Zero(); + //markerPose = Eigen::MatrixXd::Zero(); + int validsize_55 = grl::getMarkerPose(logKUKAiiwaFusionTrackP, makerID_55, timeEventM_FT, markerPose); + grl::regularizeTimeEvent(timeEventM_FT); + + std::cout<<"validsize_22: " << validsize_22 << " validsize_55: " << validsize_55 << std::endl; + + if(validsize_55>2) { - grl::writeMatrixToCSV(FT_Marker55_CSVfilename, FT_Labels_Pose, timeEventM_FT_55, markerPose_55); + grl::writeMatrixToCSV(FT_Marker55_CSVfilename, FT_Labels_Pose, timeEventM_FT, markerPose); } /// Write KUKA data to CSV - fbs_tk::Root KUKAiiwaStatesRoot = fbs_tk::open_root(kukaBinaryfile); - grl::MatrixXd timeEventM_Kuka = grl::getTimeStamp(KUKAiiwaStatesRoot,grl::kuka_tag()); + fbs_tk::Root kukaStatesP = fbs_tk::open_root(kukaBinaryfile); + grl::MatrixXd timeEventM_Kuka = grl::getTimeStamp(kukaStatesP,grl::kuka_tag()); grl::regularizeTimeEvent(timeEventM_Kuka); - - Eigen::MatrixXd jointAngles = grl::getAllJointAngles(KUKAiiwaStatesRoot); + grl::writeTimeEventToCSV(KUKA_TimeEvent_CSVfilename, timeEventM_Kuka); + Eigen::MatrixXd jointAngles = grl::getAllJointAngles(kukaStatesP); std::vector Kuka_Joint_Labels = getLabels(grl::kuka_tag()); if(jointAngles.rows() == timeEventM_Kuka.rows()){ - // Eigen::MatrixXd JointWithTime(jointAngles.rows(), jointAngles.cols()+timeEventM_Kuka.cols()); - // JointWithTime << timeEventM_Kuka.cast(), jointAngles; - grl::writeMatrixToCSV(KUKA_CSVfilename_Joint, Kuka_Joint_Labels, timeEventM_Kuka, jointAngles); + grl::writeMatrixToCSV(KUKA_Joint_CSVfilename, Kuka_Joint_Labels, timeEventM_Kuka, jointAngles); } - /// - - - // grl::writeFT_KUKATimeEventToCSV(fusiontrackBinaryfile, kukaBinaryfile, FTKUKA_CSVfilename, FT_CSVfilename, FT_PoseCSVfilename, KUKA_CSVfilename); - // grl::writeJointAngToCSV(kukaBinaryfile, KUKA_CSVfilename_Joint); + grl::writeFTKUKATimeEventToCSV(FTKUKA_TimeEvent_CSVfilename, logKUKAiiwaFusionTrackP, kukaStatesP); return 0; } From 735d1b454345b421fc940f705bf9259d6e82226f Mon Sep 17 00:00:00 2001 From: Chunting Date: Tue, 27 Feb 2018 21:20:16 -0500 Subject: [PATCH 053/102] Add some comments --- .../grl/flatbuffer/ParseflatbuffertoCSV.hpp | 496 ++++++++++++++++ include/grl/flatbuffer/kukaiiwaURDF.h | 482 ++++++++++++++++ include/grl/flatbuffer/readDatafromBinary.hpp | 533 ------------------ test/CMakeLists.txt | 13 +- test/KukaPoseEstimationTest.cpp | 126 ++--- ...adFRIFBTest.cpp => readFlatbufferTest.cpp} | 27 +- 6 files changed, 1064 insertions(+), 613 deletions(-) create mode 100644 include/grl/flatbuffer/ParseflatbuffertoCSV.hpp create mode 100644 include/grl/flatbuffer/kukaiiwaURDF.h delete mode 100644 include/grl/flatbuffer/readDatafromBinary.hpp rename test/{readFRIFBTest.cpp => readFlatbufferTest.cpp} (78%) diff --git a/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp b/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp new file mode 100644 index 0000000..90e8306 --- /dev/null +++ b/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp @@ -0,0 +1,496 @@ +#ifndef GRL_READ_DATA_FROM_BINARY +#define GRL_READ_DATA_FROM_BINARY + +#define FLATBUFFERS_DEBUG_VERIFICATION_FAILURE + +#include "flatbuffers/flatbuffers.h" +#include "grl/flatbuffer/JointState_generated.h" +#include "grl/flatbuffer/ArmControlState_generated.h" +#include "grl/flatbuffer/KUKAiiwa_generated.h" +#include "grl/flatbuffer/LinkObject_generated.h" +#include "grl/flatbuffer/Time_generated.h" +#include "grl/flatbuffer/flatbuffer.hpp" +#include "grl/flatbuffer/FusionTrack_generated.h" +#include "grl/flatbuffer/LogKUKAiiwaFusionTrack_generated.h" +#include +#include + +#include // For printing and file access. +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + + +#include +#include +#include +#include +#include +#include "camodocal/EigenUtils.h" + +//#include "grl/flatbuffer/readDatafromBinary.hpp" +#include "grl/time.hpp" + +// SpaceVecAlg +// https://github.com/jrl-umi3218/SpaceVecAlg +#include + +// RBDyn +// https://github.com/jrl-umi3218/RBDyn +#include +#include +#include +#include +#include +#include + +// mc_rbdyn_urdf +// https://github.com/jrl-umi3218/mc_rbdyn_urdf +#include +#include "kukaiiwaURDF.h" + +namespace grl { + /// Define some constants. + const double RadtoDegree = 180/3.14159265359; + const double MeterToMM = 1000; + /// Define the int64_t vector and matrix, which is used for time data. + typedef Eigen::Matrix VectorXd; + typedef Eigen::Matrix MatrixXd; + + std::vector Time_Labels = {"local_receive_time_X", "local_request_time_offset", "device_time_offset", "time_Y"}; + std::vector FT_Pose_Labels = {"Counter", "P_X", "P_Y", "P_Z", "Q_X", "Q_Y", "Q_Z", "Q_W"}; + std::vector Joint_Labels = {"Joint_1", "Joint_2", "Joint_3", "Joint_4", "Joint_5", "Joint_6", "Joint_7"}; + std::vector Kuka_Pose_Labels = {"X", "Y", "Z", "A", "B", "C"}; + struct kuka_tag {}; + struct fusiontracker_tag {}; + int col_timeEvent=4; + int col_Kuka_Joint = 7; + int col_FT_Pose = 8; + enum TimeType { local_receive_time = 0, local_request_time = 1, device_time = 2, time_Y = 3}; + enum LabelsType { FT_Pose = 0, Joint = 1, Kuka_Pose= 2}; + + /// Get CSV labels + /// @param label indicate the type of the label. + std::vector getLabels( LabelsType label){ + std::vector labels; + labels.insert(labels.end(), Time_Labels.begin(), Time_Labels.end()); + switch(label) { + case FT_Pose: + labels.insert(labels.end(), FT_Pose_Labels.begin(), FT_Pose_Labels.end()); + break; + case Joint: + labels.insert(labels.end(), Joint_Labels.begin(), Joint_Labels.end()); + break; + case Kuka_Pose: + labels.insert(labels.end(), Kuka_Pose_Labels.begin(), Kuka_Pose_Labels.end()); + break; + default: + std::cout<<"Only return Time_Labels..."< + bool checkmonotonic( T &time){ + for(int i=1; i &logKUKAiiwaFusionTrackP, fusiontracker_tag){ + + auto states = logKUKAiiwaFusionTrackP->states(); + std::size_t state_size = states->size(); + assert(state_size); + grl::MatrixXd timeEventM(state_size,4); + for(int i = 0; iGet(i); + auto timeEvent = kukaiiwaFusionTrackMessage->timeEvent(); + timeEventM(i,TimeType::local_receive_time) = timeEvent->local_receive_time(); + timeEventM(i,TimeType::local_request_time) = timeEvent->local_request_time(); + timeEventM(i,TimeType::device_time) = timeEvent->device_time(); + timeEventM(i,TimeType::time_Y) = timeEventM(i,TimeType::device_time) - timeEventM(i,TimeType::local_receive_time); + } + grl::VectorXd timeCol = timeEventM.col(TimeType::local_receive_time); + assert(checkmonotonic(timeCol)); + return timeEventM; + } + /// Return the original timeEvent of Kuka + /// @param kukaStatesP pointer of the root object for kuka. + /// @param kuka_tag tag dispatch + /// @return timeEventM timeEvent matrix which should have four columns + /// local_receive_time_offset = local_receive_time - initial_local_receive_time + /// local_request_time_offset = local_request_time - initial_local_request_time + /// device_time_offset = device_time - initial_device_time + /// time_Y = device_time_offset - local_receive_time_offset + grl::MatrixXd getTimeStamp(const fbs_tk::Root &kukaStatesP, kuka_tag){ + auto states = kukaStatesP->states(); + std::size_t state_size = states->size(); + assert(state_size); + grl::MatrixXd timeEventM(state_size,4); + for(int i = 0; iGet(i); + auto FRIMessage = KUKAiiwaState->FRIMessage(); + auto timeEvent = FRIMessage->timeStamp(); + timeEventM(i,TimeType::local_receive_time) = timeEvent->local_receive_time(); + timeEventM(i,TimeType::local_request_time) = timeEvent->local_request_time(); + timeEventM(i,TimeType::device_time) = timeEvent->device_time(); + timeEventM(i,TimeType::time_Y) = timeEventM(i,TimeType::device_time) - timeEventM(i,TimeType::local_receive_time); + } + grl::VectorXd timeCol = timeEventM.col(TimeType::local_receive_time); + assert(checkmonotonic(timeCol)); + return timeEventM; + } + /// Process the time data to get the time offset based on the starting time point. + /// See https://github.com/facontidavide/PlotJuggler/issues/68 + /// @param timeEventM timeEvent matrix which should have four columns + void regularizeTimeEvent(grl::MatrixXd& timeEventM){ + std::size_t time_size = timeEventM.rows(); + assert(time_size>0); + auto initial_local_time = timeEventM(0,TimeType::local_receive_time); + auto initial_device_time = timeEventM(0,TimeType::device_time); + timeEventM.col(TimeType::local_receive_time) = timeEventM.col(TimeType::local_receive_time) - initial_local_time * grl::VectorXd::Ones(time_size); + timeEventM.col(TimeType::local_request_time) = timeEventM.col(TimeType::local_request_time) - initial_local_time * grl::VectorXd::Ones(time_size); + + timeEventM.col(TimeType::device_time) = timeEventM.col(TimeType::device_time) - initial_device_time * grl::VectorXd::Ones(time_size); + timeEventM.col(TimeType::time_Y) = timeEventM.col(TimeType::device_time) - timeEventM.col(TimeType::local_receive_time); + grl::VectorXd timeCol = timeEventM.col(TimeType::local_receive_time); + assert(checkmonotonic(timeCol)); + } + + + /// Get the maker pose based on the markerID. The bad data, which means the frame doesn't have the indicated marker information, has been filtered out. + /// As for the bad data, both the marker pose and the corresponding timeEvent is skipped. + /// @param logKUKAiiwaFusionTrackP, pointer of the root object for fusiontracker. + /// @param makerID, the indicated marker. + /// @param timeEventM, timeEvent without bad data, which is filled out. + /// @param markerPose, the pose matrix, which is filled out. + /// @return row, the number of valid rows. + int getMarkerPose(const fbs_tk::Root &logKUKAiiwaFusionTrackP, + uint32_t &makerID, + grl::MatrixXd& timeEventM, + Eigen::MatrixXd& markerPose){ + auto states = logKUKAiiwaFusionTrackP->states(); + std::size_t state_size = states->size(); + assert(state_size>0); + // The first columne is counter + int row = 0; + // Eigen::MatrixXd markerPose(state_size, grl::col_FT_Pose); + int BadCount = 0; + for(int i = 0; iGet(i); + auto FT_Message = kukaiiwaFusionTrackMessage->deviceState_as_FusionTrackMessage(); + auto FT_Frame = FT_Message->frame(); + auto counter = FT_Frame->counter(); + auto Makers = FT_Frame->markers(); + /// In some frames, there are no markers, where we skip that line. Also we need to remove the corresponding line in TimeEvent. + if(Makers!=nullptr) { + auto makerSize = Makers->size(); + for(int markerIndex=0; markerIndexGet(markerIndex); + auto marker_ID = marker->geometryID(); + if(marker_ID == makerID){ + + auto timeEvent = kukaiiwaFusionTrackMessage->timeEvent(); + timeEventM(row,TimeType::local_receive_time) = timeEvent->local_receive_time(); + timeEventM(row,TimeType::local_request_time) = timeEvent->local_request_time(); + timeEventM(row,TimeType::device_time) = timeEvent->device_time(); + timeEventM(row,TimeType::time_Y) = timeEventM(row,TimeType::device_time) - timeEventM(row,TimeType::local_receive_time); + + auto Pose = marker->transform(); + auto position = Pose->position(); + auto orientationQtn = Pose->orientation(); + Eigen::VectorXd onePose(grl::col_FT_Pose); + onePose << counter,MeterToMM* position.x(), MeterToMM*position.y(), MeterToMM*position.z(), orientationQtn.x(), orientationQtn.y(), orientationQtn.z(), orientationQtn.w(); + markerPose.row(row++) = onePose.transpose(); + } + } + } + } + if(row < state_size) { + markerPose.conservativeResize(row, Eigen::NoChange_t{}); + timeEventM.conservativeResize(row, Eigen::NoChange_t{}); + } + std::cout <<"State size: " << state_size <<" markerPose size: " << markerPose.rows() << " timeEvent: " << timeEventM.rows() << " makerID: " << makerID < &kukaStatesP, int joint_index){ + + auto states = kukaStatesP->states(); + + std::size_t state_size = states->size(); + Eigen::VectorXf jointPosition(state_size); + for(int i = 0; iGet(i); + auto FRIMessage = KUKAiiwaState->FRIMessage(); + auto joints_Position = FRIMessage->measuredJointPosition(); // flatbuffers::Vector * + jointPosition(i) = joints_Position->Get(joint_index); + } + return jointPosition; + } + /// Get the all the joint angles. + /// @param kukaStatesP pointer of the root object for kuka. + /// @return allJointPosition, Eigen matrix which contains the positions of all the joints. + Eigen::MatrixXd getAllJointAngles(const fbs_tk::Root &kukaStatesP){ + + auto states = kukaStatesP->states(); + std::size_t state_size = states->size(); + Eigen::MatrixXd allJointPosition(state_size, 7); + for(int i = 0; iGet(i); + auto FRIMessage = KUKAiiwaState->FRIMessage(); + auto joints_Position = FRIMessage->measuredJointPosition(); // flatbuffers::Vector * + Eigen::VectorXd oneStateJointPosition(7); + for(int joint_index=0; joint_index<7; joint_index++){ + oneStateJointPosition(joint_index) = joints_Position->Get(joint_index); + } + allJointPosition.row(i) = oneStateJointPosition.transpose(); + } + return allJointPosition; + } + /// Write the data to CSV file + /// @param CSV_FileName, the file name. + /// @param labels, the labels for the data to write, which should be the first row of the csv file + /// @param timeM, by default we write all the data combined with the timestamp, whose type is grl::MatrixXd (int64_t). + /// @param dataM, the data to write, whose type is Eigen::MatrixXd + template + void writeMatrixToCSV(const std::string& CSV_FileName, std::vector &labels, T1& timeM, T2& dataM){ + std::size_t labels_size = labels.size(); + std::size_t cols_size = timeM.cols() + dataM.cols(); + std::size_t time_rows_size = timeM.rows(); + std::size_t data_rows_size = dataM.rows(); + assert(labels_size == cols_size && time_rows_size>0 && cols_size>0 && time_rows_size==data_rows_size); + auto time = timeM.col(TimeType::local_receive_time); + assert(checkmonotonic(time)); + // create an ofstream for the file output (see the link on streams for more info) + std::ofstream fs; + // create a name for the file output + fs.open(CSV_FileName, std::ofstream::out | std::ofstream::app); + // write the file labels + for(int col=0; col0) { + device_time_step = device_timeV(i) - device_timeV(i-1); + receive_time_step = local_receive_timeV(i) - local_receive_timeV(i-1); + } + // write the data to the output file + fs << local_receive_timeV(i) << "," // A + << local_request_timeV(i)<<"," // B + << device_timeV(i) <<"," // C + << device_timeV(i) - local_receive_timeV(i) << "," + << receive_request(i) << "," + << device_time_step << "," + << receive_time_step << std::endl; //D + } + fs.close(); + } + /// With the same method to process the time data from both Kuka and fusiontracker, combine it and write to a csv file. + /// @param FTKUKA_TimeEvent_CSVfilename, the csv file name + void writeFTKUKATimeEventToCSV(std::string& FTKUKA_TimeEvent_CSVfilename, + const fbs_tk::Root &logKUKAiiwaFusionTrackP, + const fbs_tk::Root &kukaStatesP) { + + grl::MatrixXd timeEventM_FT = grl::getTimeStamp(logKUKAiiwaFusionTrackP, grl::fusiontracker_tag()); + grl::MatrixXd timeEventM_Kuka = grl::getTimeStamp(kukaStatesP, grl::kuka_tag()); + std::size_t kuka_state_size = timeEventM_Kuka.rows(); + std::cout<< "------Kuka_state_size: "<< timeEventM_Kuka.rows() << std::endl; + + std::size_t FT_state_size = timeEventM_FT.rows(); + std::cout<< "------FT_state_size: "<< timeEventM_FT.rows() << std::endl; + int kuka_index = 0; + int FT_index = 0; + + // Filter out the very beginning data,since the tracker starts to work once the scene is loaded in vrep. + // But kuka starts to work only after clicking on the start button. + // To combine the time from two devices, they should share the same starting time point. + while(kuka_index timeEventM_FT(FT_index,local_receive_time) && kuka_index == 0){ + FT_index++; + } + + auto initial_local_time = std::min(timeEventM_FT(FT_index,local_receive_time), timeEventM_Kuka(kuka_index,local_receive_time)); + auto initial_device_time_kuka = timeEventM_Kuka(kuka_index,device_time); + auto initial_device_time_FT = timeEventM_FT(FT_index,device_time); + grl::VectorXd FT_local_request_time = timeEventM_FT.col(local_request_time) - initial_local_time * grl::VectorXd::Ones(FT_state_size); + grl::VectorXd FT_local_receive_time = timeEventM_FT.col(local_receive_time) - initial_local_time * grl::VectorXd::Ones(FT_state_size); + grl::VectorXd FT_device_time = timeEventM_FT.col(device_time) - initial_device_time_FT * grl::VectorXd::Ones(FT_state_size); + + grl::VectorXd kuka_local_request_time = timeEventM_Kuka.col(local_request_time) - initial_local_time * grl::VectorXd::Ones(kuka_state_size); + grl::VectorXd kuka_local_receive_time = timeEventM_Kuka.col(local_receive_time) - initial_local_time * grl::VectorXd::Ones(kuka_state_size); + grl::VectorXd kuka_device_time = timeEventM_Kuka.col(device_time) - initial_device_time_kuka * grl::VectorXd::Ones(kuka_state_size); + + + std::ofstream fs; + // create a name for the file output + fs.open( FTKUKA_TimeEvent_CSVfilename, std::ofstream::out | std::ofstream::app); + // write the file headers + fs << "local_receive_time_offset_X," + << "FT_local_request_time," + << "KUKA_local_request_time," + << "FT_device_time_offset," + << "device_time_offset_kuka," + << "Y_FT," + << "Y_kuka" + << std::endl; + + int64_t kuka_diff = kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index); + int64_t FT_diff = FT_device_time(FT_index) - FT_local_receive_time(FT_index); + while ( kuka_index < kuka_state_size && FT_index < FT_state_size ) + { + // If the row value is the kuka time, then the FT cells should be left blank. + if ( kuka_local_receive_time(kuka_index) < FT_local_receive_time(FT_index) ){ + // write the data to the output file + fs << kuka_local_receive_time(kuka_index) <<"," + <<"," + << kuka_local_request_time(kuka_index) << "," + << "," + << kuka_device_time(kuka_index) <<"," + << "," + << kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index) - kuka_diff + << std::endl; + kuka_index++; + } else if( kuka_local_receive_time(kuka_index) > FT_local_receive_time(FT_index)) { + // If the row value is the FT time, then the kuka cells should be left blank. + fs << FT_local_receive_time(FT_index) <<"," + << FT_local_request_time(FT_index) << "," + <<"," + << FT_device_time(FT_index) << "," + << "," + << FT_device_time(FT_index) - FT_local_receive_time(FT_index) - FT_diff << "," + << std::endl; + FT_index++; + } else { + // In case the time is extactly equivent with each other. + fs << FT_local_receive_time(FT_index) <<"," + << FT_local_request_time(FT_index) << "," + << kuka_local_request_time(kuka_index) << "," + << FT_device_time(FT_index) << "," + << kuka_device_time(kuka_index) <<"," + << FT_device_time(FT_index) - FT_local_receive_time(FT_index) - FT_diff << "," + << kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index) - kuka_diff + << std::endl; + FT_index++; + kuka_index++; + } + } + fs.close(); + } + /// Based on the RBDy and URDF model, get the cartesian pose of the end effector. + /// @param jointAngles, the joint angles matrix + /// @return poseEE, contain the translation and the Euler angle. + Eigen::MatrixXd getPoseEE(Eigen::MatrixXd& jointAngles){ + + namespace cst = boost::math::constants; + auto strRobot = mc_rbdyn_urdf::rbdyn_from_urdf(XYZSarmUrdf); + rbd::MultiBody mb = strRobot.mb; + rbd::MultiBodyConfig mbc(mb); + rbd::MultiBodyGraph mbg(strRobot.mbg); + std::size_t nrJoints = mbg.nrJoints(); + std::vector jointNames; + std::cout<<"Joint Size: "<< nrJoints << std::endl; + + std::size_t row_size = jointAngles.rows(); + std::size_t body_size = mbc.bodyPosW.size(); + /// The translation and Euler angles in world coordinate. + Eigen::MatrixXd poseEE(row_size,6); + + for(int rowIdx=0; rowIdx 0) { + mbc.q[jointIndex][0] = oneStateJointPosition[jointIdx]; + jointIdx++; + } + } + rbd::forwardKinematics(mb, mbc); + sva::PTransformd pos = mbc.bodyPosW[body_size-1]; + Eigen::Matrix3d E ; // rotation + + Eigen::Vector3d r ; // translation + E = pos.rotation(); + r = MeterToMM*pos.translation(); + Eigen::Vector3d eulerAngleEigen = RadtoDegree*E.eulerAngles(0,1,2); + Eigen::Quaterniond q(E); + Eigen::RowVectorXd pose(6); + pose << r.transpose(), eulerAngleEigen.transpose(); + poseEE.row(rowIdx) = pose; + // std::cout << "-----------------------------------" << std::endl; + // std::cout << "Rotation:\n " << E << std::endl + // << "Translation:\n" << r <. + */ +/** + * THIS IS A TEMPORAL FILE, AFTER FINISHING THE TEST, REMOVE IT. + * @CHUNTING + */ +#pragma once + +#include +#include + +#include "RBDyn/FK.h" + + +std::string XYZSarmUrdf( +R"( + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + /iiwa + + + + + Gazebo/Grey + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Grey + 0.2 + 0.2 + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + +)"); + +namespace mc_rbdyn_urdf +{ +bool operator==( const Geometry::Mesh& m1, const Geometry::Mesh& m2) +{ + return m1.scale == m2.scale && m1.filename == m2.filename; +} + +bool operator==( const Geometry::Box& b1, const Geometry::Box& b2) +{ + return b1.size == b2.size; +} + +bool operator==( const Geometry::Sphere& b1, const Geometry::Sphere& b2) +{ + return b1.radius == b2.radius; +} + +bool operator==( const Geometry::Cylinder& b1, const Geometry::Cylinder& b2) +{ + return b1.radius == b2.radius && b1.length == b2.length; +} + +bool operator==(const Geometry& g1, const Geometry& g2) +{ + return g1.type == g2.type && g1.data == g2.data; +} +bool operator==(const Visual& v1, const Visual& v2) { + return v1.name == v2.name && v1.origin == v2.origin && v1.geometry == v2.geometry; +} +} /* mc_rbdyn_urdf */ + + +const double TOL = 1e-6; \ No newline at end of file diff --git a/include/grl/flatbuffer/readDatafromBinary.hpp b/include/grl/flatbuffer/readDatafromBinary.hpp deleted file mode 100644 index 2738e09..0000000 --- a/include/grl/flatbuffer/readDatafromBinary.hpp +++ /dev/null @@ -1,533 +0,0 @@ -#ifndef GRL_READ_DATA_FROM_BINARY -#define GRL_READ_DATA_FROM_BINARY - -#define FLATBUFFERS_DEBUG_VERIFICATION_FAILURE - -#include "flatbuffers/flatbuffers.h" -#include "grl/flatbuffer/JointState_generated.h" -#include "grl/flatbuffer/ArmControlState_generated.h" -#include "grl/flatbuffer/KUKAiiwa_generated.h" -#include "grl/flatbuffer/LinkObject_generated.h" -#include "grl/flatbuffer/Time_generated.h" -#include "grl/flatbuffer/flatbuffer.hpp" -#include "grl/flatbuffer/FusionTrack_generated.h" -#include "grl/flatbuffer/LogKUKAiiwaFusionTrack_generated.h" -#include -#include - -#include // For printing and file access. -#include -#include - -#include -#include -#include -#include -#include -#include - - -namespace grl { - typedef Eigen::Matrix VectorXd; - typedef Eigen::Matrix MatrixXd; - - const char *Time_Labels[] = {"local_receive_time_X", "local_request_time_offset", "device_time_offset", "time_Y"}; - const char *FT_Pose_Labels[] = {"Counter", "P_X", "P_Y", "P_Z", "Q_X", "Q_Y", "Q_Z", "Q_W"}; - const char *Joint_Labels[] = {"Joint_1", "Joint_2", "Joint_3", "Joint_4", "Joint_5", "Joint_6", "Joint_7"}; - struct kuka_tag {}; - struct fusiontracker_tag {}; - int col_timeEvent=4; - int col_Kuka_Joint = 7; - int col_FT_Pose = 8; - enum TimeType { local_receive_time = 0, local_request_time = 1, device_time = 2, time_Y = 3}; - /// Help initialize a vector array of strings. - /// See https://stackoverflow.com/questions/4268886/initialize-a-vector-array-of-strings - template - T * end(T (&ra)[N]) { - return ra + N; - } - - - std::vector getLabels(fusiontracker_tag){ - std::vector Time_Labels_Vec(Time_Labels, grl::end(Time_Labels)); - std::vector FT_Pose_Labels_Vec(FT_Pose_Labels, grl::end(FT_Pose_Labels)); - std::vector FT_Labels_Pose; - FT_Labels_Pose.reserve(Time_Labels_Vec.size()+FT_Pose_Labels_Vec.size()); - FT_Labels_Pose.insert(FT_Labels_Pose.end(), Time_Labels_Vec.begin(), Time_Labels_Vec.end()); - FT_Labels_Pose.insert(FT_Labels_Pose.end(), FT_Pose_Labels_Vec.begin(), FT_Pose_Labels_Vec.end()); - return FT_Labels_Pose; - } - - std::vector getLabels(kuka_tag){ - std::vector Time_Labels_Vec(Time_Labels, grl::end(Time_Labels)); - std::vector Joint_Labels_Vec(Joint_Labels, grl::end(Joint_Labels)); - std::vector Kuka_Joint_Labels; - Kuka_Joint_Labels.reserve(Time_Labels_Vec.size()+Joint_Labels_Vec.size()); - Kuka_Joint_Labels.insert(Kuka_Joint_Labels.end(), Time_Labels_Vec.begin(), Time_Labels_Vec.end()); - Kuka_Joint_Labels.insert(Kuka_Joint_Labels.end(), Joint_Labels_Vec.begin(), Joint_Labels_Vec.end()); - return Kuka_Joint_Labels; - } - - /// Remove a certain row or colum for the given matrix in Eigen - /// See https://stackoverflow.com/questions/13290395/how-to-remove-a-certain-row-or-column-while-using-eigen-library-c - template - void removeRow(T& matrix, unsigned int rowToRemove) - { - unsigned int numRows = matrix.rows()-1; - unsigned int numCols = matrix.cols(); - - if( rowToRemove < numRows ) - matrix.block(rowToRemove,0,numRows-rowToRemove,numCols) = matrix.bottomRows(numRows-rowToRemove); - matrix.conservativeResize(numRows,numCols); - } - template - void removeColumn(T& matrix, unsigned int colToRemove) - { - unsigned int numRows = matrix.rows(); - unsigned int numCols = matrix.cols()-1; - - if( colToRemove < numCols ) - matrix.block(0,colToRemove,numRows,numCols-colToRemove) = matrix.rightCols(numCols-colToRemove); - - matrix.conservativeResize(numRows,numCols); - } - - template - bool checkmonotonic( T &time){ - for(int i=1; i &kukaStatesP, kuka_tag, TimeType time_type){ - auto states = kukaStatesP->states(); - std::size_t state_size = states->size(); - grl::VectorXd timeStamp(state_size); - for(int i = 0; iGet(i); - auto FRIMessage = KUKAiiwaState->FRIMessage(); - auto timeEvent = FRIMessage->timeStamp(); - switch(time_type) { - case device_time:{ - timeStamp(i) = timeEvent->device_time(); - break; - } - case local_request_time:{ - timeStamp(i) = timeEvent->local_request_time(); - break; - } - case local_receive_time:{ - timeStamp(i) = timeEvent->local_receive_time(); - break; - } - default:{ - std::perror("No time type!"); - } - } - } - return timeStamp; - } - /// Get the original time from LogKUKAiiwaFusionTrack - grl::VectorXd getTimeStamp(const fbs_tk::Root &logKUKAiiwaFusionTrackP, fusiontracker_tag, TimeType time_type){ - - auto states = logKUKAiiwaFusionTrackP->states(); - std::size_t state_size = states->size(); - grl::VectorXd timeStamp(state_size); - //Eigen::VectoXd time_stamp(state_size); - for(int i = 0; iGet(i); - auto timeEvent = kukaiiwaFusionTrackMessage->timeEvent(); - switch(time_type) { - case device_time:{ - timeStamp(i) = timeEvent->device_time(); - break; - } - case local_request_time:{ - timeStamp(i) = timeEvent->local_request_time(); - break; - } - case local_receive_time:{ - timeStamp(i) = timeEvent->local_receive_time(); - break; - } - default:{ - std::perror("No time type!"); - } - } - } - return timeStamp; - } - /// Return the original timeEvent - grl::MatrixXd getTimeStamp(const fbs_tk::Root &logKUKAiiwaFusionTrackP, fusiontracker_tag){ - - auto states = logKUKAiiwaFusionTrackP->states(); - std::size_t state_size = states->size(); - assert(state_size); - grl::MatrixXd timeEventM(state_size,4); - for(int i = 0; iGet(i); - auto timeEvent = kukaiiwaFusionTrackMessage->timeEvent(); - timeEventM(i,TimeType::local_receive_time) = timeEvent->local_receive_time(); - timeEventM(i,TimeType::local_request_time) = timeEvent->local_request_time(); - timeEventM(i,TimeType::device_time) = timeEvent->device_time(); - timeEventM(i,TimeType::time_Y) = timeEventM(i,TimeType::device_time) - timeEventM(i,TimeType::local_receive_time); - } - grl::VectorXd timeCol = timeEventM.col(TimeType::local_receive_time); - assert(checkmonotonic(timeCol)); - return timeEventM; - } - /// Return the original timeEvent - grl::MatrixXd getTimeStamp(const fbs_tk::Root &kukaStatesP, kuka_tag){ - auto states = kukaStatesP->states(); - std::size_t state_size = states->size(); - assert(state_size); - grl::MatrixXd timeEventM(state_size,4); - for(int i = 0; iGet(i); - auto FRIMessage = KUKAiiwaState->FRIMessage(); - auto timeEvent = FRIMessage->timeStamp(); - timeEventM(i,TimeType::local_receive_time) = timeEvent->local_receive_time(); - timeEventM(i,TimeType::local_request_time) = timeEvent->local_request_time(); - timeEventM(i,TimeType::device_time) = timeEvent->device_time(); - timeEventM(i,TimeType::time_Y) = timeEventM(i,TimeType::device_time) - timeEventM(i,TimeType::local_receive_time); - } - grl::VectorXd timeCol = timeEventM.col(TimeType::local_receive_time); - assert(checkmonotonic(timeCol)); - return timeEventM; - } - - void regularizeTimeEvent(grl::MatrixXd& timeEventM){ - std::size_t time_size = timeEventM.rows(); - assert(time_size>0); - auto initial_local_time = timeEventM(0,TimeType::local_receive_time); - auto initial_device_time = timeEventM(0,TimeType::device_time); - timeEventM.col(TimeType::local_receive_time) = timeEventM.col(TimeType::local_receive_time) - initial_local_time * grl::VectorXd::Ones(time_size); - timeEventM.col(TimeType::local_request_time) = timeEventM.col(TimeType::local_request_time) - initial_local_time * grl::VectorXd::Ones(time_size); - - timeEventM.col(TimeType::device_time) = timeEventM.col(TimeType::device_time) - initial_device_time * grl::VectorXd::Ones(time_size); - timeEventM.col(TimeType::time_Y) = timeEventM.col(TimeType::device_time) - timeEventM.col(TimeType::local_receive_time); - grl::VectorXd timeCol = timeEventM.col(TimeType::local_receive_time); - assert(checkmonotonic(timeCol)); - } - - - - int getMarkerPose(const fbs_tk::Root &logKUKAiiwaFusionTrackP, - uint32_t &makerID, - grl::MatrixXd& timeEventM, - Eigen::MatrixXd& markerPose){ - auto states = logKUKAiiwaFusionTrackP->states(); - std::size_t state_size = states->size(); - assert(state_size>0); - // The first columne is counter - int row = 0; - // Eigen::MatrixXd markerPose(state_size, grl::col_FT_Pose); - int BadCount = 0; - for(int i = 0; iGet(i); - auto FT_Message = kukaiiwaFusionTrackMessage->deviceState_as_FusionTrackMessage(); - auto FT_Frame = FT_Message->frame(); - auto counter = FT_Frame->counter(); - auto Makers = FT_Frame->markers(); - /// In some frames, there are no markers, where we skip that line. Also we need to remove the corresponding line in TimeEvent. - if(Makers!=nullptr) { - auto makerSize = Makers->size(); - for(int markerIndex=0; markerIndexGet(markerIndex); - auto marker_ID = marker->geometryID(); - if(marker_ID == makerID){ - - auto timeEvent = kukaiiwaFusionTrackMessage->timeEvent(); - timeEventM(row,TimeType::local_receive_time) = timeEvent->local_receive_time(); - timeEventM(row,TimeType::local_request_time) = timeEvent->local_request_time(); - timeEventM(row,TimeType::device_time) = timeEvent->device_time(); - timeEventM(row,TimeType::time_Y) = timeEventM(row,TimeType::device_time) - timeEventM(row,TimeType::local_receive_time); - - auto Pose = marker->transform(); - auto position = Pose->position(); - auto orientationQtn = Pose->orientation(); - Eigen::VectorXd onePose(grl::col_FT_Pose); - onePose << counter,1000* position.x(), 1000*position.y(), 1000*position.z(), orientationQtn.x(), orientationQtn.y(), orientationQtn.z(), orientationQtn.w(); - markerPose.row(row++) = onePose.transpose(); - } - } - } - } - if(row < state_size) { - markerPose.conservativeResize(row, Eigen::NoChange_t{}); - timeEventM.conservativeResize(row, Eigen::NoChange_t{}); - } - std::cout <<"State size: " << state_size <<" markerPose size: " << markerPose.rows() << " timeEvent: " << timeEventM.rows() << " makerID: " << makerID < &kukaStatesP, int joint_index){ - - auto states = kukaStatesP->states(); - - std::size_t state_size = states->size(); - Eigen::VectorXf jointPosition(state_size); - for(int i = 0; iGet(i); - auto FRIMessage = KUKAiiwaState->FRIMessage(); - auto joints_Position = FRIMessage->measuredJointPosition(); // flatbuffers::Vector * - jointPosition(i) = joints_Position->Get(joint_index); - } - return jointPosition; - } - /// Reture all joint angles - Eigen::MatrixXd getAllJointAngles(const fbs_tk::Root &kukaStatesP){ - - auto states = kukaStatesP->states(); - std::size_t state_size = states->size(); - Eigen::MatrixXd allJointPosition(state_size, 7); - for(int i = 0; iGet(i); - auto FRIMessage = KUKAiiwaState->FRIMessage(); - auto joints_Position = FRIMessage->measuredJointPosition(); // flatbuffers::Vector * - Eigen::VectorXd oneStateJointPosition(7); - for(int joint_index=0; joint_index<7; joint_index++){ - oneStateJointPosition(joint_index) = joints_Position->Get(joint_index); - } - allJointPosition.row(i) = oneStateJointPosition.transpose(); - } - return allJointPosition; - } - template - void writeMatrixToCSV(const std::string& CSV_FileName, std::vector &labels, T1& timeM, T2& dataM){ - std::size_t labels_size = labels.size(); - std::size_t cols_size = timeM.cols() + dataM.cols(); - std::size_t time_rows_size = timeM.rows(); - std::size_t t2_rows_size = dataM.rows(); - assert(labels_size == cols_size && time_rows_size>0 && cols_size>0 && time_rows_size==t2_rows_size); - auto time = timeM.col(TimeType::local_receive_time); - assert(checkmonotonic(time)); - // create an ofstream for the file output (see the link on streams for more info) - std::ofstream fs; - // create a name for the file output - fs.open(CSV_FileName, std::ofstream::out | std::ofstream::app); - // write the file labels - for(int col=0; col0) { - device_time_step = device_timeV(i) - device_timeV(i-1); - receive_time_step = local_receive_timeV(i) - local_receive_timeV(i-1); - } - // write the data to the output file - fs << local_receive_timeV(i) << "," // A - << local_request_timeV(i)<<"," // B - << device_timeV(i) <<"," // C - << device_timeV(i) - local_receive_timeV(i) << "," - << receive_request(i) << "," - << device_time_step << "," - << receive_time_step << "," - << std::endl; //D - } - fs.close(); - } - - - - void writeEEPoseToCSV( - std::string & CSV_FileName, - grl::VectorXd device_time, - grl::VectorXd local_request_time, - grl::VectorXd local_receive_time, - Eigen::MatrixXd &poseEE){ - - std::size_t time_size = device_time.size(); - std::size_t row_size = poseEE.rows(); - assert(time_size == row_size); - - // auto initial_local_time = local_request_time(0); - auto initial_local_time = local_receive_time(0); - auto initial_device_time = device_time(0); - local_request_time = local_request_time - initial_local_time * grl::VectorXd::Ones(time_size); - local_receive_time = local_receive_time - initial_local_time * grl::VectorXd::Ones(time_size); - device_time = device_time - initial_device_time * grl::VectorXd::Ones(time_size); - grl::VectorXd receive_request = local_receive_time - local_request_time; - // grl::VectorXd device_time_offset = local_receive_time - local_request_time; - std::ofstream fs; - // create a name for the file output - fs.open( CSV_FileName, std::ofstream::out | std::ofstream::app); - // write the file headers - fs << "local_receive_time_X," - << "local_request_time_offset," - << "device_time_offset," - << "time_Y," - << "Receive-Request," - << "device_time_step," - << "receive_time_step," - << "X," - << "Y," - << "Z," - << "A," - << "B," - << "C" - << std::endl; - int64_t device_time_step = 0; - int64_t receive_time_step = 0; - for(int i=0; i0) { - device_time_step = device_time(i) - device_time(i-1); - receive_time_step = local_receive_time(i) - local_receive_time(i-1); - } - Eigen::RowVectorXd pose = poseEE.row(i); - // write the data to the output file - fs << local_receive_time(i) <<"," // B - << local_request_time(i)<< "," // A - << device_time(i) <<"," // C - << device_time(i) - local_receive_time(i) << "," - << receive_request(i) << "," - << device_time_step << "," - << receive_time_step << "," - << pose(0) << "," - << pose(1) << "," - << pose(2) << "," - << pose(3) << "," - << pose(4) << "," - << pose(5) << std::endl; //D - } - fs.close(); - } - -void writeFTKUKATimeEventToCSV(std::string& FTKUKA_TimeEvent_CSVfilename, - const fbs_tk::Root &logKUKAiiwaFusionTrackP, - const fbs_tk::Root &kukaStatesP) { - - grl::MatrixXd timeEventM_FT = grl::getTimeStamp(logKUKAiiwaFusionTrackP, grl::fusiontracker_tag()); - grl::MatrixXd timeEventM_Kuka = grl::getTimeStamp(kukaStatesP, grl::kuka_tag()); - std::size_t kuka_state_size = timeEventM_Kuka.rows(); - std::cout<< "------Kuka_state_size: "<< timeEventM_Kuka.rows() << std::endl; - - std::size_t FT_state_size = timeEventM_FT.rows(); - std::cout<< "------FT_state_size: "<< timeEventM_FT.rows() << std::endl; - int kuka_index = 0; - int FT_index = 0; - - // filter out the very beginning data, which can gurantee to record the data when the two devices work simultaniously. - while(kuka_index timeEventM_FT(FT_index,local_receive_time) && kuka_index == 0){ - FT_index++; - } - - auto initial_local_time = std::min(timeEventM_FT(FT_index,local_receive_time), timeEventM_Kuka(kuka_index,local_receive_time)); - auto initial_device_time_kuka = timeEventM_Kuka(kuka_index,device_time); - auto initial_device_time_FT = timeEventM_FT(FT_index,device_time); - grl::VectorXd FT_local_request_time = timeEventM_FT.col(local_request_time) - initial_local_time * grl::VectorXd::Ones(FT_state_size); - grl::VectorXd FT_local_receive_time = timeEventM_FT.col(local_receive_time) - initial_local_time * grl::VectorXd::Ones(FT_state_size); - grl::VectorXd FT_device_time = timeEventM_FT.col(device_time) - initial_device_time_FT * grl::VectorXd::Ones(FT_state_size); - - grl::VectorXd kuka_local_request_time = timeEventM_Kuka.col(local_request_time) - initial_local_time * grl::VectorXd::Ones(kuka_state_size); - grl::VectorXd kuka_local_receive_time = timeEventM_Kuka.col(local_receive_time) - initial_local_time * grl::VectorXd::Ones(kuka_state_size); - grl::VectorXd kuka_device_time = timeEventM_Kuka.col(device_time) - initial_device_time_kuka * grl::VectorXd::Ones(kuka_state_size); - - - std::ofstream fs; - // create a name for the file output - fs.open( FTKUKA_TimeEvent_CSVfilename, std::ofstream::out | std::ofstream::app); - // write the file headers - fs << "local_receive_time_offset_X," - << "FT_local_request_time," - << "KUKA_local_request_time," - << "FT_device_time_offset," - << "device_time_offset_kuka," - << "Y_FT," - << "Y_kuka" - << std::endl; - - int64_t kuka_diff = kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index); - int64_t FT_diff = FT_device_time(FT_index) - FT_local_receive_time(FT_index); - while ( kuka_index < kuka_state_size && FT_index < FT_state_size ) - { - if ( kuka_local_receive_time(kuka_index) < FT_local_receive_time(FT_index) ){ - // write the data to the output file - fs << kuka_local_receive_time(kuka_index) <<"," - <<"," - << kuka_local_request_time(kuka_index) << "," - << "," - << kuka_device_time(kuka_index) <<"," - << "," - << kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index) - kuka_diff - << std::endl; - kuka_index++; - } else if( kuka_local_receive_time(kuka_index) > FT_local_receive_time(FT_index)) { - // write the data to the output file - fs << FT_local_receive_time(FT_index) <<"," - << FT_local_request_time(FT_index) << "," - <<"," - << FT_device_time(FT_index) << "," - << "," - << FT_device_time(FT_index) - FT_local_receive_time(FT_index) - FT_diff << "," - << std::endl; - FT_index++; - } else { - fs << FT_local_receive_time(FT_index) <<"," - << FT_local_request_time(FT_index) << "," - << kuka_local_request_time(kuka_index) << "," - << FT_device_time(FT_index) << "," - << kuka_device_time(kuka_index) <<"," - << FT_device_time(FT_index) - FT_local_receive_time(FT_index) - FT_diff << "," - << kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index) - kuka_diff - << std::endl; - FT_index++; - kuka_index++; - } - } - fs.close(); -} - -} -#endif \ No newline at end of file diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 374153f..8db2b25 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -95,11 +95,18 @@ if(TARGET KukaFRIClient OR FRI_Client_SDK_Cpp_FOUND) basis_add_executable(FlatbuffersExampleWithKukaIiwaTest.cpp)# ${GRL_FLATBUFFERS_OUTPUTS}) basis_target_link_libraries(FlatbuffersExampleWithKukaIiwaTest ${Boost_LIBRARIES} ${Boost_REGEX_LIBRARY} ${FRI_Client_SDK_Cpp_LIBRARIES} ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} KukaFRIClient ${FLATBUFFERS_STATIC_LIB}) - basis_add_executable(readFRIFBTest.cpp) - basis_target_link_libraries(readFRIFBTest ${Boost_LIBRARIES} ${Boost_REGEX_LIBRARY} ${FRI_Client_SDK_Cpp_LIBRARIES} ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} KukaFRIClient ${FLATBUFFERS_STATIC_LIB} -lboost_iostreams -lboost_system -lboost_filesystem) + basis_add_executable(readFlatbufferTest.cpp) + basis_target_link_libraries(readFlatbufferTest ${Boost_LIBRARIES} ${Boost_REGEX_LIBRARY} ${FRI_Client_SDK_Cpp_LIBRARIES} ${Nanopb_LIBRARIES} + ${CMAKE_THREAD_LIBS_INIT} KukaFRIClient ${FLATBUFFERS_STATIC_LIB} -lboost_iostreams -lboost_system -lboost_filesystem + mc_rbdyn_urdf + SpaceVecAlg #TODO: re-enable when exported targets are correct again + sch-core + RBDyn + Tasks) basis_add_executable(KukaPoseEstimationTest.cpp) - basis_target_link_libraries(KukaPoseEstimationTest ${Boost_LIBRARIES} ${Boost_REGEX_LIBRARY} ${FRI_Client_SDK_Cpp_LIBRARIES} ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} KukaFRIClient ${FLATBUFFERS_STATIC_LIB} -lboost_iostreams -lboost_system -lboost_filesystem + basis_target_link_libraries(KukaPoseEstimationTest ${Boost_LIBRARIES} ${Boost_REGEX_LIBRARY} ${FRI_Client_SDK_Cpp_LIBRARIES} + ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} KukaFRIClient ${FLATBUFFERS_STATIC_LIB} -lboost_iostreams -lboost_system -lboost_filesystem mc_rbdyn_urdf SpaceVecAlg #TODO: re-enable when exported targets are correct again sch-core diff --git a/test/KukaPoseEstimationTest.cpp b/test/KukaPoseEstimationTest.cpp index 3323401..1400b92 100644 --- a/test/KukaPoseEstimationTest.cpp +++ b/test/KukaPoseEstimationTest.cpp @@ -32,7 +32,7 @@ #include #include "camodocal/EigenUtils.h" -#include "grl/flatbuffer/readDatafromBinary.hpp" +#include "grl/flatbuffer/ParseflatbuffertoCSV.hpp" #include "grl/time.hpp" // SpaceVecAlg @@ -63,80 +63,76 @@ std::string foldname = "/home/chunting/src/V-REP_PRO_EDU_V3_4_0_Linux/"; std::string foldtimestamp = current_date_and_time_string(); -// mkdir(foldname+foldtimestamp, 0777); std::string kukaBinaryfile = foldname + "2018_02_26_14_23_14_Kukaiiwa.iiwa"; std::string KUKA_TimeEvent_CSVfilename = foldname + foldtimestamp + "/KUKA_TimeEvent.csv"; std::string KUKA_Joint_CSVfilename = foldname + foldtimestamp + "/KUKA_Joint.csv"; +std::string KUKA_Pose_CSVfilename = foldname + foldtimestamp + "/KUKA_Pose.csv"; -std::string fusiontrackBinaryfile = foldname + "2018_02_26_14_19_55_FusionTrack.flik"; -std::string FT_TimeEvent_CSVfilename = foldname + foldtimestamp + "/FT_TimeEvent.csv"; -std::string FT_Marker22_CSVfilename = foldname + foldtimestamp + "/FT_Pose_Marker22.csv"; -std::string FT_Marker55_CSVfilename = foldname + foldtimestamp + "/FT_Pose_Marker55.csv"; - -std::string FTKUKA_TimeEvent_CSVfilename = foldname + foldtimestamp + "/FTKUKA_TimeEvent.csv"; int main(int argc, char* argv[]) { - using namespace Eigen; - using namespace sva; - using namespace rbd; - namespace cst = boost::math::constants; - auto strRobot = mc_rbdyn_urdf::rbdyn_from_urdf(XYZSarmUrdf); - MultiBody mb = strRobot.mb; - MultiBodyConfig mbc(mb); - MultiBodyGraph mbg(strRobot.mbg); - std::size_t nrJoints = mbg.nrJoints(); - std::vector jointNames; - std::cout<<"Joint Size: "<< nrJoints << std::endl; - fbs_tk::Root KUKAiiwaStatesRoot = fbs_tk::open_root(kukaBinaryfile); - grl::VectorXd kuka_device_time = grl::getTimeStamp(KUKAiiwaStatesRoot, grl::kuka_tag(), grl::TimeType::device_time); - grl::VectorXd kuka_local_request_time = grl::getTimeStamp(KUKAiiwaStatesRoot, grl::kuka_tag(), grl::TimeType::local_request_time); - grl::VectorXd kuka_local_receive_time = grl::getTimeStamp(KUKAiiwaStatesRoot, grl::kuka_tag(), grl::TimeType::local_receive_time); - - Eigen::MatrixXd jointAngles = grl::getAllJointAngles(KUKAiiwaStatesRoot); - std::size_t row_size = jointAngles.rows(); - std::size_t body_size = mbc.bodyPosW.size(); - /// The translation and Euler angles in world coordinate. - Eigen::MatrixXd poseEE(row_size,6); - - for(int rowIdx=0; rowIdx 0) { - mbc.q[jointIndex][0] = oneStateJointPosition[jointIdx]; - jointIdx++; - } - } - forwardKinematics(mb, mbc); - sva::PTransformd pos = mbc.bodyPosW[body_size-1]; - Eigen::Matrix3d E ; // rotation - - Eigen::Vector3d r ; // translation - E = pos.rotation(); - r = MeterToMM*pos.translation(); - Eigen::Vector3d eulerAngleEigen = RadtoDegree*E.eulerAngles(0,1,2); - Eigen::Quaterniond q(E); - Eigen::RowVectorXd pose(6); - pose << r.transpose(), eulerAngleEigen.transpose(); - poseEE.row(rowIdx) = pose; - // std::cout << "-----------------------------------" << std::endl; - // std::cout << "Rotation:\n " << E << std::endl - // << "Translation:\n" << r < jointNames; + // std::cout<<"Joint Size: "<< nrJoints << std::endl; + // fbs_tk::Root kukaStatesP = fbs_tk::open_root(kukaBinaryfile); + + // grl::MatrixXd timeEventM_Kuka = grl::getTimeStamp(kukaStatesP,grl::kuka_tag()); + // grl::regularizeTimeEvent(timeEventM_Kuka); + + // Eigen::MatrixXd jointAngles = grl::getAllJointAngles(kukaStatesP); + // std::vector Kuka_Joint_Labels = grl::getLabels(grl::LabelsType::Joint); + // jointAngles = RadtoDegree*jointAngles; + // if(jointAngles.rows() == timeEventM_Kuka.rows()){ + // grl::writeMatrixToCSV(KUKA_Joint_CSVfilename, Kuka_Joint_Labels, timeEventM_Kuka, jointAngles); + // } + + + // std::size_t row_size = jointAngles.rows(); + // std::size_t body_size = mbc.bodyPosW.size(); + // /// The translation and Euler angles in world coordinate. + // Eigen::MatrixXd poseEE(row_size,6); + + // for(int rowIdx=0; rowIdx 0) { + // mbc.q[jointIndex][0] = oneStateJointPosition[jointIdx]; + // jointIdx++; + // } + // } + // rbd::forwardKinematics(mb, mbc); + // sva::PTransformd pos = mbc.bodyPosW[body_size-1]; + // Eigen::Matrix3d E ; // rotation + + // Eigen::Vector3d r ; // translation + // E = pos.rotation(); + // r = MeterToMM*pos.translation(); + // Eigen::Vector3d eulerAngleEigen = RadtoDegree*E.eulerAngles(0,1,2); + // Eigen::Quaterniond q(E); + // Eigen::RowVectorXd pose(6); + // pose << r.transpose(), eulerAngleEigen.transpose(); + // poseEE.row(rowIdx) = pose; + // // std::cout << "-----------------------------------" << std::endl; + // // std::cout << "Rotation:\n " << E << std::endl + // // << "Translation:\n" << r < #include "grl/flatbuffer/FusionTrack_generated.h" #include "grl/flatbuffer/Time_generated.h" @@ -24,20 +24,17 @@ int main(int argc, char* argv[]) { std::string foldname = "/home/chunting/src/V-REP_PRO_EDU_V3_4_0_Linux/"; - std::string kukaBinaryfile = foldname + "2018_02_26_14_23_14_Kukaiiwa.iiwa"; - std::string fusiontrackBinaryfile = foldname + "2018_02_26_14_19_55_FusionTrack.flik"; + std::string kukaTimeStamp = "2018_02_26_14_23_14_Kukaiiwa"; + std::string FTTimeStamp = "2018_02_26_14_19_55_FusionTrack"; + std::string kukaBinaryfile = foldname + kukaTimeStamp+ ".iiwa"; + std::string fusiontrackBinaryfile = foldname + FTTimeStamp +".flik"; std::string foldtimestamp = current_date_and_time_string(); boost::filesystem::path dir{foldname+foldtimestamp}; - boost::filesystem::create_directory(dir); - // boost::filesystem::path dir_file{kukaBinaryfile}; - // boost::filesystem::copy_file(dir_file, dir); - // dir_file = boost::filesystem::path(fusiontrackBinaryfile); - // boost::filesystem::copy_file(dir_file, dir); - std::string KUKA_TimeEvent_CSVfilename = foldname + foldtimestamp + "/KUKA_TimeEvent.csv"; std::string KUKA_Joint_CSVfilename = foldname + foldtimestamp + "/KUKA_Joint.csv"; + std::string KUKA_Pose_CSVfilename = foldname + foldtimestamp + "/KUKA_Pose.csv"; std::string FT_TimeEvent_CSVfilename = foldname + foldtimestamp + "/FT_TimeEvent.csv"; std::string FT_Marker22_CSVfilename = foldname + foldtimestamp + "/FT_Pose_Marker22.csv"; @@ -45,6 +42,8 @@ int main(int argc, char* argv[]) std::string FTKUKA_TimeEvent_CSVfilename = foldname + foldtimestamp + "/FTKUKA_TimeEvent.csv"; + + /// Write FT data to CSV fbs_tk::Root logKUKAiiwaFusionTrackP = fbs_tk::open_root(fusiontrackBinaryfile); grl::MatrixXd timeEventM_FT = grl::getTimeStamp(logKUKAiiwaFusionTrackP, grl::fusiontracker_tag()); @@ -59,7 +58,7 @@ int main(int argc, char* argv[]) int validsize_22 = grl::getMarkerPose(logKUKAiiwaFusionTrackP, makerID_22, timeEventM_FT, markerPose); grl::regularizeTimeEvent(timeEventM_FT); - std::vector FT_Labels_Pose = getLabels(grl::fusiontracker_tag()); + std::vector FT_Labels_Pose = grl::getLabels(grl::LabelsType::FT_Pose); if(validsize_22>2){ grl::writeMatrixToCSV(FT_Marker22_CSVfilename, FT_Labels_Pose, timeEventM_FT, markerPose); } @@ -85,10 +84,14 @@ int main(int argc, char* argv[]) grl::regularizeTimeEvent(timeEventM_Kuka); grl::writeTimeEventToCSV(KUKA_TimeEvent_CSVfilename, timeEventM_Kuka); Eigen::MatrixXd jointAngles = grl::getAllJointAngles(kukaStatesP); - std::vector Kuka_Joint_Labels = getLabels(grl::kuka_tag()); + std::vector Kuka_Joint_Labels = grl::getLabels(grl::LabelsType::Joint); if(jointAngles.rows() == timeEventM_Kuka.rows()){ - grl::writeMatrixToCSV(KUKA_Joint_CSVfilename, Kuka_Joint_Labels, timeEventM_Kuka, jointAngles); + grl::writeMatrixToCSV(KUKA_Joint_CSVfilename, Kuka_Joint_Labels, timeEventM_Kuka, jointAngles); + std::vector Kuka_Pose_Labels = grl::getLabels(grl::LabelsType::Kuka_Pose); + Eigen::MatrixXd PoseEE = grl::getPoseEE(jointAngles); + grl::writeMatrixToCSV(KUKA_Pose_CSVfilename, Kuka_Pose_Labels, timeEventM_Kuka, PoseEE); + } From 528d5a47875c8a5e5c1cd6eddee434edcc0fe194 Mon Sep 17 00:00:00 2001 From: Chunting Date: Wed, 28 Feb 2018 00:06:36 -0500 Subject: [PATCH 054/102] Skip the bad data --- .../grl/flatbuffer/ParseflatbuffertoCSV.hpp | 60 +++++++++++++------ test/readFlatbufferTest.cpp | 12 ++-- 2 files changed, 50 insertions(+), 22 deletions(-) diff --git a/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp b/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp index 90e8306..4032bb7 100644 --- a/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp +++ b/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp @@ -65,16 +65,17 @@ namespace grl { typedef Eigen::Matrix VectorXd; typedef Eigen::Matrix MatrixXd; - std::vector Time_Labels = {"local_receive_time_X", "local_request_time_offset", "device_time_offset", "time_Y"}; - std::vector FT_Pose_Labels = {"Counter", "P_X", "P_Y", "P_Z", "Q_X", "Q_Y", "Q_Z", "Q_W"}; + std::vector Time_Labels = {"local_receive_time_X", "local_request_time_offset", "device_time_offset", "time_Y", "counter"}; + std::vector FT_Pose_Labels = {"P_X", "P_Y", "P_Z", "Q_X", "Q_Y", "Q_Z", "Q_W"}; std::vector Joint_Labels = {"Joint_1", "Joint_2", "Joint_3", "Joint_4", "Joint_5", "Joint_6", "Joint_7"}; std::vector Kuka_Pose_Labels = {"X", "Y", "Z", "A", "B", "C"}; struct kuka_tag {}; struct fusiontracker_tag {}; - int col_timeEvent=4; - int col_Kuka_Joint = 7; - int col_FT_Pose = 8; - enum TimeType { local_receive_time = 0, local_request_time = 1, device_time = 2, time_Y = 3}; + int col_timeEvent=Time_Labels.size(); + int col_Kuka_Joint = Joint_Labels.size(); + int col_FT_Pose = FT_Pose_Labels.size(); + int col_Kuka_Pose = Kuka_Pose_Labels.size(); + enum TimeType { local_receive_time = 0, local_request_time = 1, device_time = 2, time_Y = 3, counterIdx=4}; enum LabelsType { FT_Pose = 0, Joint = 1, Kuka_Pose= 2}; /// Get CSV labels @@ -120,14 +121,34 @@ namespace grl { auto states = logKUKAiiwaFusionTrackP->states(); std::size_t state_size = states->size(); assert(state_size); - grl::MatrixXd timeEventM(state_size,4); + int row = 0; + grl::MatrixXd timeEventM(state_size,col_timeEvent); for(int i = 0; iGet(i); - auto timeEvent = kukaiiwaFusionTrackMessage->timeEvent(); - timeEventM(i,TimeType::local_receive_time) = timeEvent->local_receive_time(); - timeEventM(i,TimeType::local_request_time) = timeEvent->local_request_time(); - timeEventM(i,TimeType::device_time) = timeEvent->device_time(); - timeEventM(i,TimeType::time_Y) = timeEventM(i,TimeType::device_time) - timeEventM(i,TimeType::local_receive_time); + auto FT_Message = kukaiiwaFusionTrackMessage->deviceState_as_FusionTrackMessage(); + auto FT_Frame = FT_Message->frame(); + auto counter = FT_Frame->counter(); + auto Makers = FT_Frame->markers(); + if(Makers!=nullptr) { + auto makerSize = Makers->size(); + for(int markerIndex=0; markerIndexGet(markerIndex); + auto marker_ID = marker->geometryID(); + if(marker_ID == 22 || marker_ID == 55){ + auto timeEvent = kukaiiwaFusionTrackMessage->timeEvent(); + timeEventM(row,TimeType::local_receive_time) = timeEvent->local_receive_time(); + timeEventM(row,TimeType::local_request_time) = timeEvent->local_request_time(); + timeEventM(row,TimeType::device_time) = timeEvent->device_time(); + timeEventM(row,TimeType::time_Y) = timeEventM(row,TimeType::device_time) - timeEventM(row,TimeType::local_receive_time); + timeEventM(row,TimeType::counterIdx) = counter; + row++; + break; + } + } + } + } + if(row < state_size) { + timeEventM.conservativeResize(row, Eigen::NoChange_t{}); } grl::VectorXd timeCol = timeEventM.col(TimeType::local_receive_time); assert(checkmonotonic(timeCol)); @@ -145,15 +166,17 @@ namespace grl { auto states = kukaStatesP->states(); std::size_t state_size = states->size(); assert(state_size); - grl::MatrixXd timeEventM(state_size,4); + grl::MatrixXd timeEventM(state_size,col_timeEvent); for(int i = 0; iGet(i); auto FRIMessage = KUKAiiwaState->FRIMessage(); + auto identifier = FRIMessage->messageIdentifier(); auto timeEvent = FRIMessage->timeStamp(); timeEventM(i,TimeType::local_receive_time) = timeEvent->local_receive_time(); timeEventM(i,TimeType::local_request_time) = timeEvent->local_request_time(); timeEventM(i,TimeType::device_time) = timeEvent->device_time(); timeEventM(i,TimeType::time_Y) = timeEventM(i,TimeType::device_time) - timeEventM(i,TimeType::local_receive_time); + timeEventM(i,TimeType::counterIdx) = identifier; } grl::VectorXd timeCol = timeEventM.col(TimeType::local_receive_time); assert(checkmonotonic(timeCol)); @@ -214,12 +237,12 @@ namespace grl { timeEventM(row,TimeType::local_request_time) = timeEvent->local_request_time(); timeEventM(row,TimeType::device_time) = timeEvent->device_time(); timeEventM(row,TimeType::time_Y) = timeEventM(row,TimeType::device_time) - timeEventM(row,TimeType::local_receive_time); - + timeEventM(row,TimeType::counterIdx) = counter; auto Pose = marker->transform(); auto position = Pose->position(); auto orientationQtn = Pose->orientation(); Eigen::VectorXd onePose(grl::col_FT_Pose); - onePose << counter,MeterToMM* position.x(), MeterToMM*position.y(), MeterToMM*position.z(), orientationQtn.x(), orientationQtn.y(), orientationQtn.z(), orientationQtn.w(); + onePose << MeterToMM*position.x(), MeterToMM*position.y(), MeterToMM*position.z(), orientationQtn.x(), orientationQtn.y(), orientationQtn.z(), orientationQtn.w(); markerPose.row(row++) = onePose.transpose(); } } @@ -317,6 +340,7 @@ namespace grl { grl::VectorXd local_receive_timeV = timeEventM.col(local_receive_time); grl::VectorXd local_request_timeV = timeEventM.col(local_request_time); grl::VectorXd device_timeV = timeEventM.col(device_time); + grl::VectorXd counter = timeEventM.col(counterIdx); grl::VectorXd receive_request = local_receive_timeV - local_request_timeV; // grl::VectorXd device_time_offset = local_receive_time - local_request_time; std::ofstream fs; @@ -329,7 +353,8 @@ namespace grl { << "Y," << "Receive-Request," << "device_time_step," - << "receive_time_step" + << "receive_time_step," + << "counter" << std::endl; int64_t device_time_step = 0; int64_t receive_time_step = 0; @@ -345,7 +370,8 @@ namespace grl { << device_timeV(i) - local_receive_timeV(i) << "," << receive_request(i) << "," << device_time_step << "," - << receive_time_step << std::endl; //D + << receive_time_step << "," + << counter(i) << std::endl; //D } fs.close(); } diff --git a/test/readFlatbufferTest.cpp b/test/readFlatbufferTest.cpp index 757be49..ff4f0c3 100644 --- a/test/readFlatbufferTest.cpp +++ b/test/readFlatbufferTest.cpp @@ -20,6 +20,7 @@ /// Boost to create an empty folder #include +/// split -C 307m --numeric-suffixes 2018_02_26_14_19_55_FusionTrack.json 2018_02_26_14_19_55_FusionTrack int main(int argc, char* argv[]) { @@ -47,8 +48,8 @@ int main(int argc, char* argv[]) /// Write FT data to CSV fbs_tk::Root logKUKAiiwaFusionTrackP = fbs_tk::open_root(fusiontrackBinaryfile); grl::MatrixXd timeEventM_FT = grl::getTimeStamp(logKUKAiiwaFusionTrackP, grl::fusiontracker_tag()); - grl::regularizeTimeEvent(timeEventM_FT); - grl::writeTimeEventToCSV(FT_TimeEvent_CSVfilename, timeEventM_FT); + //grl::regularizeTimeEvent(timeEventM_FT); + std::size_t FT_size = timeEventM_FT.rows(); uint32_t makerID_22 = 22; @@ -60,7 +61,8 @@ int main(int argc, char* argv[]) grl::regularizeTimeEvent(timeEventM_FT); std::vector FT_Labels_Pose = grl::getLabels(grl::LabelsType::FT_Pose); if(validsize_22>2){ - grl::writeMatrixToCSV(FT_Marker22_CSVfilename, FT_Labels_Pose, timeEventM_FT, markerPose); + grl::writeTimeEventToCSV(FT_TimeEvent_CSVfilename, timeEventM_FT); + grl::writeMatrixToCSV(FT_Marker22_CSVfilename, FT_Labels_Pose, timeEventM_FT, markerPose); } timeEventM_FT.resize(FT_size, grl::col_timeEvent); @@ -78,7 +80,7 @@ int main(int argc, char* argv[]) grl::writeMatrixToCSV(FT_Marker55_CSVfilename, FT_Labels_Pose, timeEventM_FT, markerPose); } - /// Write KUKA data to CSV + // /// Write KUKA data to CSV fbs_tk::Root kukaStatesP = fbs_tk::open_root(kukaBinaryfile); grl::MatrixXd timeEventM_Kuka = grl::getTimeStamp(kukaStatesP,grl::kuka_tag()); grl::regularizeTimeEvent(timeEventM_Kuka); @@ -95,7 +97,7 @@ int main(int argc, char* argv[]) } - grl::writeFTKUKATimeEventToCSV(FTKUKA_TimeEvent_CSVfilename, logKUKAiiwaFusionTrackP, kukaStatesP); + // grl::writeFTKUKATimeEventToCSV(FTKUKA_TimeEvent_CSVfilename, logKUKAiiwaFusionTrackP, kukaStatesP); return 0; } From 06c54593e52a41b353134d54ec962da687bc68c6 Mon Sep 17 00:00:00 2001 From: Chunting Date: Wed, 28 Feb 2018 22:20:02 -0500 Subject: [PATCH 055/102] [NOT FINISHED] Do the Calibration --- .../grl/flatbuffer/ParseflatbuffertoCSV.hpp | 65 +++++++++++++++---- .../grl/sensor/FusionTrackToFlatbuffer.hpp | 10 +-- src/lua/robone.lua | 24 +++++++ test/readFlatbufferTest.cpp | 26 ++++++-- 4 files changed, 102 insertions(+), 23 deletions(-) diff --git a/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp b/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp index 4032bb7..d0cc7ac 100644 --- a/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp +++ b/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp @@ -468,10 +468,44 @@ namespace grl { } fs.close(); } + /// Get the transformation matrix from LBR_iiwa_14_R820_link8 to Fiducial#22. + /// See the physical relationship in vrep. + void getEEToFudicial22Matrix(std::vector &EEPose){ + Eigen::Vector3d trans; + trans << -0.10710206627846, 0.060432970523834, 0.11829662322998; + Eigen::Matrix3d rot; + rot << -0.078461408615112, 0.13236111402512, -0.98809123039246, + -0.99495536088943, -0.072545289993286, 0.069288671016693, + -0.062510251998901, 0.98854321241379, 0.13738548755646; + sva::PTransformd relativeTransform(rot, trans); + for(int i=0; i& FudicialPose){ + Eigen::Vector3d trans; + trans << -0.39689552783966, 0.34754598140717, 2.2256722450256; + Eigen::Matrix3d rot; + rot << 0.28129482269287, 0.95735365152359, 0.065933525562286, + 0.026909112930298, 0.060811519622803, -0.99778652191162, + -0.95924407243729, 0.28244641423225, -0.0086555480957031; + sva::PTransformd relativeTransform(rot, trans); + for(int i=0; i getPoseEE(Eigen::MatrixXd& jointAngles){ namespace cst = boost::math::constants; auto strRobot = mc_rbdyn_urdf::rbdyn_from_urdf(XYZSarmUrdf); @@ -485,7 +519,8 @@ namespace grl { std::size_t row_size = jointAngles.rows(); std::size_t body_size = mbc.bodyPosW.size(); /// The translation and Euler angles in world coordinate. - Eigen::MatrixXd poseEE(row_size,6); + /// + std::vector EEpose; for(int rowIdx=0; rowIdx& Pose ) { + std::size_t pose_size = Pose.size(); + Eigen::MatrixXd PKPose(pose_size,6); + for(int i=0; i toFlatBuffer(flatbuffers::FlatBufferBuilder &fbb, const grl::sensor::FusionTrack::Frame &frame, - bool writeRegionOfInterest = true, - bool writeFiducials = true, + bool writeRegionOfInterest = false, + bool writeFiducials = false, bool writeMarkers = true) { /// @todo TODO(ahundt) IN PROGRESS Here we should get the markers'name @@ -472,9 +472,9 @@ flatbuffers::Offset toFlatBuffer(flatbuffers::FlatBufferBuilder &fbb, const grl::sensor::FusionTrack &fusiontrack, const grl::sensor::FusionTrack::Frame &frame, - bool writeParameters = true, - bool writeRegionOfInterest = true, - bool writeFiducials = true, + bool writeParameters = false, + bool writeRegionOfInterest = false, + bool writeFiducials = false, bool writeMarkers = true) { static const double microsecToSec = 1 / 1000000; diff --git a/src/lua/robone.lua b/src/lua/robone.lua index 47db797..0c4adce 100644 --- a/src/lua/robone.lua +++ b/src/lua/robone.lua @@ -446,6 +446,7 @@ end ------------------------------------------ -- Configure the Optical Tracker -- -- Call in a "Customization Script" -- +-- The functions in Customization Script are executed all the time: when simulation is running, as well as when simulation is not running. ------------------------------------------ robone.configureOpticalTracker=function() @@ -508,7 +509,30 @@ robone.configureOpticalTracker=function() -- Test the below one -- Enable the recordDataScript() to call the method below. simExtAtracsysFusionTrackRecordWhileSimulationIsRunning(true) + -------------------------------------------------- + -- Get the relative transformation matrix between Fiducial#22 and LBR_iiwa_14_R820_link8 + + fiducial22handl = simGetObjectHandle ('Fiducial#22') + link8handl = simGetObjectHandle ('LBR_iiwa_14_R820_link8') + relativeTransformationMatrix = simGetObjectMatrix(fiducial22handl, link8handl) + print('The relative transformation matrix between Fiducial#22 and LBR_iiwa_14_R820_link8') + for i=1,12,1 do + print(relativeTransformationMatrix[i].." ") + end + + robothandle = simGetObjectHandle ('LBR_iiwa_14_R820#0') + trankerhandle = simGetObjectHandle ('OpticalTrackerBase#0') + robotToTrakerM = simGetObjectMatrix(robothandle, trankerhandle) + print('The relative transformation matrix between LBR_iiwa_14_R820#0 and OpticalTrackerBase#0') + for i=1,12,1 do + print(robotToTrakerM[i].." ") + end + fudicialToRobotM = simGetObjectMatrix(fiducial22handl, robothandle) + print('The relative transformation matrix between Fiducial#22 and LBR_iiwa_14_R820#0') + for i=1,12,1 do + print(fudicialToRobotM[i].." ") + end end -- By default we disable customization script execution during simulation, in order diff --git a/test/readFlatbufferTest.cpp b/test/readFlatbufferTest.cpp index ff4f0c3..ed3845c 100644 --- a/test/readFlatbufferTest.cpp +++ b/test/readFlatbufferTest.cpp @@ -20,13 +20,13 @@ /// Boost to create an empty folder #include -/// split -C 307m --numeric-suffixes 2018_02_26_14_19_55_FusionTrack.json 2018_02_26_14_19_55_FusionTrack +/// split -C 200m --numeric-suffixes 2018_02_28_16_39_13_FusionTrack.json 2018_02_28_16_39_13_FusionTrack int main(int argc, char* argv[]) { std::string foldname = "/home/chunting/src/V-REP_PRO_EDU_V3_4_0_Linux/"; - std::string kukaTimeStamp = "2018_02_26_14_23_14_Kukaiiwa"; - std::string FTTimeStamp = "2018_02_26_14_19_55_FusionTrack"; + std::string kukaTimeStamp = "2018_02_28_20_43_50_Kukaiiwa"; + std::string FTTimeStamp = "2018_02_28_20_43_16_FusionTrack"; std::string kukaBinaryfile = foldname + kukaTimeStamp+ ".iiwa"; std::string fusiontrackBinaryfile = foldname + FTTimeStamp +".flik"; std::string foldtimestamp = current_date_and_time_string(); @@ -36,6 +36,8 @@ int main(int argc, char* argv[]) std::string KUKA_TimeEvent_CSVfilename = foldname + foldtimestamp + "/KUKA_TimeEvent.csv"; std::string KUKA_Joint_CSVfilename = foldname + foldtimestamp + "/KUKA_Joint.csv"; std::string KUKA_Pose_CSVfilename = foldname + foldtimestamp + "/KUKA_Pose.csv"; + std::string FudicialToRobotPose_CSVfilename = foldname + foldtimestamp + "/FudicialToRobot_Pose.csv"; + std::string FudicialToFTPose_CSVfilename = foldname + foldtimestamp + "/FudicialToFT_Pose.csv"; std::string FT_TimeEvent_CSVfilename = foldname + foldtimestamp + "/FT_TimeEvent.csv"; std::string FT_Marker22_CSVfilename = foldname + foldtimestamp + "/FT_Pose_Marker22.csv"; @@ -80,7 +82,7 @@ int main(int argc, char* argv[]) grl::writeMatrixToCSV(FT_Marker55_CSVfilename, FT_Labels_Pose, timeEventM_FT, markerPose); } - // /// Write KUKA data to CSV + /// Write KUKA data to CSV fbs_tk::Root kukaStatesP = fbs_tk::open_root(kukaBinaryfile); grl::MatrixXd timeEventM_Kuka = grl::getTimeStamp(kukaStatesP,grl::kuka_tag()); grl::regularizeTimeEvent(timeEventM_Kuka); @@ -91,8 +93,20 @@ int main(int argc, char* argv[]) if(jointAngles.rows() == timeEventM_Kuka.rows()){ grl::writeMatrixToCSV(KUKA_Joint_CSVfilename, Kuka_Joint_Labels, timeEventM_Kuka, jointAngles); std::vector Kuka_Pose_Labels = grl::getLabels(grl::LabelsType::Kuka_Pose); - Eigen::MatrixXd PoseEE = grl::getPoseEE(jointAngles); - grl::writeMatrixToCSV(KUKA_Pose_CSVfilename, Kuka_Pose_Labels, timeEventM_Kuka, PoseEE); + std::vector PoseEE = grl::getPoseEE(jointAngles); + + Eigen::MatrixXd PKPose = grl::getPluckerPose(PoseEE); + grl::writeMatrixToCSV(KUKA_Pose_CSVfilename, Kuka_Pose_Labels, timeEventM_Kuka, PKPose); + + grl::getEEToFudicial22Matrix(PoseEE); + // PKPose.Zero(); + PKPose = grl::getPluckerPose(PoseEE); + grl::writeMatrixToCSV(FudicialToRobotPose_CSVfilename, Kuka_Pose_Labels, timeEventM_Kuka, PKPose); + + grl::getRobotToTrackerMatrix(PoseEE); + // PKPose = Eigen::MatrixXd::Zero(); + PKPose = grl::getPluckerPose(PoseEE); + grl::writeMatrixToCSV(FudicialToFTPose_CSVfilename, Kuka_Pose_Labels, timeEventM_Kuka, PKPose); } From a42196adc83762cc380ed66f7a995af40abb1323 Mon Sep 17 00:00:00 2001 From: Chunting Date: Wed, 7 Mar 2018 22:12:25 -0500 Subject: [PATCH 056/102] Get the inverse pose of the marker --- include/grl/flatbuffer/FlatbufferToEigen.hpp | 103 ++++++++++++ .../grl/flatbuffer/ParseflatbuffertoCSV.hpp | 150 +++++++++++------- include/grl/flatbuffer/kukaiiwaURDF.h | 10 ++ include/grl/vrep/Eigen.hpp | 37 +++-- include/grl/vrep/Vrep.hpp | 2 + test/CMakeLists.txt | 36 +++-- test/readFlatbufferTest.cpp | 111 +++++++------ 7 files changed, 310 insertions(+), 139 deletions(-) create mode 100644 include/grl/flatbuffer/FlatbufferToEigen.hpp diff --git a/include/grl/flatbuffer/FlatbufferToEigen.hpp b/include/grl/flatbuffer/FlatbufferToEigen.hpp new file mode 100644 index 0000000..203a50a --- /dev/null +++ b/include/grl/flatbuffer/FlatbufferToEigen.hpp @@ -0,0 +1,103 @@ +#ifndef GRL_FLATBUFFER_TO_EIGEN +#define GRL_FLATBUFFER_TO_EIGEN + +#define FLATBUFFERS_DEBUG_VERIFICATION_FAILURE +// // grl + +/// Can't include the vrep header files. +// #include "grl/vrep/Vrep.hpp" +// #include "grl/vrep/VrepRobotArmDriver.hpp" +// #include "grl/vrep/VrepRobotArmJacobian.hpp" + +// #include "grl/vrep/Eigen.hpp" +// #include "grl/vrep/Vrep.hpp" +// #include "camodocal/calib/HandEyeCalibration.h" + +// #include "v_repLib.h" +// FusionTrack Libraries + +#include +#include +#include "camodocal/EigenUtils.h" + + + + +namespace grl { + + Eigen::Affine3f MarkerPoseToAffine3f(const Eigen::VectorXd& markerPose){ + + assert(markerPose.size() == 7); + Eigen::Affine3f markerToCameraTransform; + markerToCameraTransform.translation().x() = markerPose[0]; + markerToCameraTransform.translation().y() = markerPose[1]; + markerToCameraTransform.translation().z() = markerPose[2]; + Eigen::Quaterniond q(markerPose[3], markerPose[4], markerPose[5], markerPose[6]); + Eigen::Matrix3d R = q.normalized().toRotationMatrix(); + markerToCameraTransform.matrix().block<3,3>(0,0) = R.cast(); + return markerToCameraTransform; + } + Eigen::VectorXd Affine3fToMarkerPose(const Eigen::Affine3f& markerToCameraTransform){ + Eigen::VectorXd markerPose(7); + + markerPose[0] = markerToCameraTransform.translation().x(); + markerPose[1] = markerToCameraTransform.translation().y(); + markerPose[2] = markerToCameraTransform.translation().z(); + Eigen::Matrix3f R = markerToCameraTransform.matrix().block<3,3>(0,0); + Eigen::Quaterniond q( R.cast()); + markerPose[3] = q.w(); + markerPose[4] = q.x(); + markerPose[5] = q.y(); + markerPose[6] = q.z(); + + return markerPose; + } + + Eigen::MatrixXd getPluckerPose(std::vector& Pose ) { + std::size_t pose_size = Pose.size(); + Eigen::MatrixXd PKPose(pose_size,6); + for(int i=0; i invertPose(const std::vector& poseTransformd){ + std::size_t size = poseTransformd.size(); + std::vector inversePose(size); + + for(int i=0; i + +#include "grl/vector_ostream.hpp" #include "flatbuffers/flatbuffers.h" #include "grl/flatbuffer/JointState_generated.h" #include "grl/flatbuffer/ArmControlState_generated.h" @@ -13,6 +28,8 @@ #include "grl/flatbuffer/FusionTrack_generated.h" #include "grl/flatbuffer/LogKUKAiiwaFusionTrack_generated.h" #include + + #include #include // For printing and file access. @@ -36,13 +53,13 @@ #include #include "camodocal/EigenUtils.h" -//#include "grl/flatbuffer/readDatafromBinary.hpp" #include "grl/time.hpp" // SpaceVecAlg // https://github.com/jrl-umi3218/SpaceVecAlg #include + // RBDyn // https://github.com/jrl-umi3218/RBDyn #include @@ -57,27 +74,31 @@ #include #include "kukaiiwaURDF.h" + + namespace grl { + + /// Define some constants. - const double RadtoDegree = 180/3.14159265359; - const double MeterToMM = 1000; + const double RadtoDegree =1;// 180/3.14159265359; + const double MeterToMM = 1; /// Define the int64_t vector and matrix, which is used for time data. typedef Eigen::Matrix VectorXd; typedef Eigen::Matrix MatrixXd; std::vector Time_Labels = {"local_receive_time_X", "local_request_time_offset", "device_time_offset", "time_Y", "counter"}; - std::vector FT_Pose_Labels = {"P_X", "P_Y", "P_Z", "Q_X", "Q_Y", "Q_Z", "Q_W"}; + // std::vector PK_Pose_Labels = {"P_X", "P_Y", "P_Z", "Q_X", "Q_Y", "Q_Z", "Q_W"}; std::vector Joint_Labels = {"Joint_1", "Joint_2", "Joint_3", "Joint_4", "Joint_5", "Joint_6", "Joint_7"}; - std::vector Kuka_Pose_Labels = {"X", "Y", "Z", "A", "B", "C"}; + std::vector PK_Pose_Labels = {"X", "Y", "Z", "A", "B", "C"}; struct kuka_tag {}; struct fusiontracker_tag {}; int col_timeEvent=Time_Labels.size(); int col_Kuka_Joint = Joint_Labels.size(); - int col_FT_Pose = FT_Pose_Labels.size(); - int col_Kuka_Pose = Kuka_Pose_Labels.size(); + int col_Pose = PK_Pose_Labels.size(); enum TimeType { local_receive_time = 0, local_request_time = 1, device_time = 2, time_Y = 3, counterIdx=4}; enum LabelsType { FT_Pose = 0, Joint = 1, Kuka_Pose= 2}; + /// Get CSV labels /// @param label indicate the type of the label. std::vector getLabels( LabelsType label){ @@ -85,13 +106,13 @@ namespace grl { labels.insert(labels.end(), Time_Labels.begin(), Time_Labels.end()); switch(label) { case FT_Pose: - labels.insert(labels.end(), FT_Pose_Labels.begin(), FT_Pose_Labels.end()); + labels.insert(labels.end(), PK_Pose_Labels.begin(), PK_Pose_Labels.end()); break; case Joint: labels.insert(labels.end(), Joint_Labels.begin(), Joint_Labels.end()); break; case Kuka_Pose: - labels.insert(labels.end(), Kuka_Pose_Labels.begin(), Kuka_Pose_Labels.end()); + labels.insert(labels.end(), PK_Pose_Labels.begin(), PK_Pose_Labels.end()); break; default: std::cout<<"Only return Time_Labels..."< &logKUKAiiwaFusionTrackP, uint32_t &makerID, grl::MatrixXd& timeEventM, - Eigen::MatrixXd& markerPose){ + Eigen::MatrixXd& markerPose, + bool markerFrame = false){ auto states = logKUKAiiwaFusionTrackP->states(); std::size_t state_size = states->size(); assert(state_size>0); // The first columne is counter int row = 0; - // Eigen::MatrixXd markerPose(state_size, grl::col_FT_Pose); + // Eigen::MatrixXd markerPose(state_size, grl::col_Pose); int BadCount = 0; for(int i = 0; iGet(i); @@ -239,11 +261,27 @@ namespace grl { timeEventM(row,TimeType::time_Y) = timeEventM(row,TimeType::device_time) - timeEventM(row,TimeType::local_receive_time); timeEventM(row,TimeType::counterIdx) = counter; auto Pose = marker->transform(); - auto position = Pose->position(); - auto orientationQtn = Pose->orientation(); - Eigen::VectorXd onePose(grl::col_FT_Pose); - onePose << MeterToMM*position.x(), MeterToMM*position.y(), MeterToMM*position.z(), orientationQtn.x(), orientationQtn.y(), orientationQtn.z(), orientationQtn.w(); - markerPose.row(row++) = onePose.transpose(); + auto FB_r = Pose->position(); + auto FB_q = Pose->orientation(); + // Convert the flatbuffer type to Eigen type + Eigen::Vector3d r(FB_r.x(), FB_r.y(), FB_r.z()); + Eigen::Quaterniond q(FB_q.w(), FB_q.x(), FB_q.y(), FB_q.z()); + Eigen::Matrix3d E = q.normalized().toRotationMatrix(); + Eigen::Vector3d eulerAngleEigen = RadtoDegree*E.eulerAngles(0,1,2); + Eigen::RowVectorXd pose(grl::col_Pose); + pose << r.transpose(), eulerAngleEigen.transpose(); + + + if(markerFrame == true) { + Eigen::VectorXd onePose(7); + onePose << r, q.w(), q.x(), q.y(), q.z(); + auto markerToCameraTransform = grl::MarkerPoseToAffine3f(onePose); + auto cameraTomarkerTransform = markerToCameraTransform.inverse(); + auto _markerPose = grl::Affine3fToMarkerPose(cameraTomarkerTransform); + pose = grl::getPluckerPose(_markerPose); + + } + markerPose.row(row++) = pose; } } } @@ -290,7 +328,7 @@ namespace grl { auto joints_Position = FRIMessage->measuredJointPosition(); // flatbuffers::Vector * Eigen::VectorXd oneStateJointPosition(7); for(int joint_index=0; joint_index<7; joint_index++){ - oneStateJointPosition(joint_index) = joints_Position->Get(joint_index); + oneStateJointPosition(joint_index) = joints_Position->Get(joint_index); } allJointPosition.row(i) = oneStateJointPosition.transpose(); } @@ -314,7 +352,7 @@ namespace grl { std::ofstream fs; // create a name for the file output fs.open(CSV_FileName, std::ofstream::out | std::ofstream::app); - // write the file labels + // write the file labels; for(int col=0; col &EEPose){ - Eigen::Vector3d trans; - trans << -0.10710206627846, 0.060432970523834, 0.11829662322998; - Eigen::Matrix3d rot; - rot << -0.078461408615112, 0.13236111402512, -0.98809123039246, - -0.99495536088943, -0.072545289993286, 0.069288671016693, - -0.062510251998901, 0.98854321241379, 0.13738548755646; - sva::PTransformd relativeTransform(rot, trans); - for(int i=0; i& FudicialPose){ + std::vector getRobotToTrackerMatrix(std::vector& FudicialPose){ Eigen::Vector3d trans; - trans << -0.39689552783966, 0.34754598140717, 2.2256722450256; + trans << 0.20652678608894 , -0.70480275154114 ,1.4510889053345 ; Eigen::Matrix3d rot; - rot << 0.28129482269287, 0.95735365152359, 0.065933525562286, - 0.026909112930298, 0.060811519622803, -0.99778652191162, - -0.95924407243729, 0.28244641423225, -0.0086555480957031; + rot << -0.19507896900177, -0.8963907957077, -0.39802974462509 , + 0.9409122467041, -0.28804504871368, 0.17808490991592, + -0.28378140926361, -0.33688074350357, 0.89776360988617 ; sva::PTransformd relativeTransform(rot, trans); + std::vector to(FudicialPose.size()); for(int i=0; i + /// This is gotten from VREP, the oritation of the marker dummy is identical with the flange ('Fiducial#22' and 'RobotFlangeTip'). + /// SEE kukaiiwaURDF.h /// @param jointAngles, the joint angles matrix /// @return poseEE, contain the translation and the Euler angle. - std::vector getPoseEE(Eigen::MatrixXd& jointAngles){ + std::vector getPoseEE(Eigen::MatrixXd& jointAngles, bool markerPose = false){ + using namespace Eigen; + using namespace sva; + using namespace rbd; namespace cst = boost::math::constants; auto strRobot = mc_rbdyn_urdf::rbdyn_from_urdf(XYZSarmUrdf); rbd::MultiBody mb = strRobot.mb; rbd::MultiBodyConfig mbc(mb); rbd::MultiBodyGraph mbg(strRobot.mbg); - std::size_t nrJoints = mbg.nrJoints(); + std::size_t nrJoints = mb.nrJoints(); + std::size_t nrBodies = strRobot.mb.nrBodies(); std::vector jointNames; std::cout<<"Joint Size: "<< nrJoints << std::endl; @@ -536,25 +566,23 @@ namespace grl { } rbd::forwardKinematics(mb, mbc); // Pose of the end effector relative to robot base frame. - EEpose.push_back(mbc.bodyPosW[body_size-1]); + if(markerPose){ + EEpose.push_back(mbc.bodyPosW[body_size-1]); + } else{ + EEpose.push_back(mbc.bodyPosW[body_size-2]); + } } - return EEpose; + // std::cout<< "---------------------" << std::endl; + // for(int i=0; i& Pose ) { - std::size_t pose_size = Pose.size(); - Eigen::MatrixXd PKPose(pose_size,6); - for(int i=0; i + + + + + + + + + + diff --git a/include/grl/vrep/Eigen.hpp b/include/grl/vrep/Eigen.hpp index 2facfea..92e430c 100644 --- a/include/grl/vrep/Eigen.hpp +++ b/include/grl/vrep/Eigen.hpp @@ -3,6 +3,9 @@ #ifndef _VREP_EIGEN_HPP_ #define _VREP_EIGEN_HPP_ +/// BOOST_THROW_EXCEPTION +#include + #include #include #include @@ -23,7 +26,7 @@ Eigen::Quaterniond vrepToEigenQuaternion(InputIterator vrepQuat){ // 0123 // vrep is ordered xyzw, // eigen is ordered wxyz - + Eigen::Quaterniond quat(vrepQuat[vrepquat::w],vrepQuat[vrepquat::x],vrepQuat[vrepquat::y],vrepQuat[vrepquat::z]); return quat; } @@ -31,7 +34,7 @@ Eigen::Quaterniond vrepToEigenQuaternion(InputIterator vrepQuat){ template std::array EigenToVrepQuaternion(const Q& q){ std::array qa; - + // 0123 // vrep is ordered xyzw, // eigen is ordered wxyz @@ -39,7 +42,7 @@ std::array EigenToVrepQuaternion(const Q& q){ qa[vrepquat::y] = q.y(); qa[vrepquat::z] = q.z(); qa[vrepquat::w] = q.w(); - + return qa; } @@ -49,7 +52,7 @@ std::array EigenToVrepPosition(const P& p){ qv[0] = p.x(); qv[1] = p.y(); qv[2] = p.z(); - + return qv; } @@ -94,7 +97,7 @@ Eigen::Affine3d vrepToEigenTransform(const std::array& vrepTransform) std::pair getAxisAngleAndTranslation(int ObjectHandle, int BaseFrameObjectHandle){ - + std::array simTipPosition; //std::array simTipOrientation; std::array simTipQuaternion; @@ -103,12 +106,12 @@ std::pair getAxisAngleAndTranslation(int Object if(ret==-1) BOOST_THROW_EXCEPTION(std::runtime_error("HandEyeCalibrationVrepPlugin: Could not get position")); ret = simGetObjectQuaternion(ObjectHandle, BaseFrameObjectHandle, simTipQuaternion.begin()); if(ret==-1) BOOST_THROW_EXCEPTION(std::runtime_error("HandEyeCalibrationVrepPlugin: Could not get quaternion")); - + auto simTipAngleAxis = vrepQuatToEigenVector3dAngleAxis(simTipQuaternion.begin()); auto simTipVec = vrepToEigenVector3d(simTipPosition.begin()); - - - + + + return std::make_pair(simTipAngleAxis,simTipVec); } @@ -121,11 +124,11 @@ void setObjectTransform(int objectHandle, int relativeToObjectHandle, T& transfo std::array vrepQuat = EigenToVrepQuaternion(eigenQuat); int ret = simSetObjectQuaternion(objectHandle,relativeToObjectHandle,vrepQuat.begin()); if(ret==-1) BOOST_THROW_EXCEPTION(std::runtime_error("setObjectTransform: Could not set Quaternion")); - + std::array vrepPos = EigenToVrepPosition(transform.translation()); ret = simSetObjectPosition(objectHandle,relativeToObjectHandle,vrepPos.begin()); if(ret==-1) BOOST_THROW_EXCEPTION(std::runtime_error("setObjectTransform: Could not set Quaternion")); - + } /// @param objectHandle The V-Rep object handle for which the transform is needed @@ -138,17 +141,17 @@ Eigen::Affine3d getObjectTransform(int objectHandle, int relativeToObjectHandle int ret = simGetObjectQuaternion(objectHandle,relativeToObjectHandle,vrepQuat.begin()); Eigen::Quaterniond eigenQuat(vrepToEigenQuaternion(vrepQuat)); if(ret==-1) BOOST_THROW_EXCEPTION(std::runtime_error("getObjectTransform: Could not get Quaternion")); - + std::array vrepPos; ret = simGetObjectPosition(objectHandle,relativeToObjectHandle,vrepPos.begin()); if(ret==-1) BOOST_THROW_EXCEPTION(std::runtime_error("getObjectTransform: Could not get Position")); Eigen::Vector3d eigenPos(vrepToEigenVector3d(vrepPos)); - + Eigen::Affine3d transform; - + transform = eigenQuat; transform.translation() = eigenPos; - + return transform; } @@ -163,12 +166,12 @@ std::pair getObjectTransformQuaternionTransl int ret = simGetObjectQuaternion(objectHandle,relativeToObjectHandle,vrepQuat.begin()); if(ret==-1) BOOST_THROW_EXCEPTION(std::runtime_error("getObjectTransformQuaternionTranslationPair: Could not get Quaternion")); Eigen::Quaterniond eigenQuat(vrepToEigenQuaternion(vrepQuat)); - + std::array vrepPos; ret = simGetObjectPosition(objectHandle,relativeToObjectHandle,vrepPos.begin()); if(ret==-1) BOOST_THROW_EXCEPTION(std::runtime_error("getObjectTransformQuaternionTranslationPair: Could not get Position")); Eigen::Vector3d eigenPos(vrepToEigenVector3d(vrepPos)); - + return std::make_pair(eigenQuat,eigenPos); } diff --git a/include/grl/vrep/Vrep.hpp b/include/grl/vrep/Vrep.hpp index 048e620..e0da032 100644 --- a/include/grl/vrep/Vrep.hpp +++ b/include/grl/vrep/Vrep.hpp @@ -2,10 +2,12 @@ #ifndef GRL_VREP_HPP_ #define GRL_VREP_HPP_ +#include #include #include #include "v_repLib.h" + namespace grl { namespace vrep { /// @brief Get the handle of a single object, otherwise throw an exception diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 8db2b25..988d223 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -78,7 +78,8 @@ endif () # For KUKA IIWA FRI Libraries if(TARGET KukaFRIClient OR FRI_Client_SDK_Cpp_FOUND) - basis_include_directories(${FRI_Client_SDK_Cpp_PROJECT_INCLUDE_DIRS} ${Boost_REGEX_LIBRARY} ${FRI_Client_SDK_Cpp_INCLUDE_DIRS} ${PROJECT_INCLUDE_DIR}/thirdparty/vrep/include + basis_include_directories(${FRI_Client_SDK_Cpp_PROJECT_INCLUDE_DIRS} ${Boost_REGEX_LIBRARY} + ${FRI_Client_SDK_Cpp_INCLUDE_DIRS} ${PROJECT_INCLUDE_DIR}/thirdparty/vrep/include ${FLATBUFFERS_INCLUDE_DIRS} ${FUSIONTRACK_INCLUDE_DIRS}) basis_add_executable(KukaFRITest.cpp)# ${GRL_FLATBUFFERS_OUTPUTS}) @@ -96,22 +97,31 @@ if(TARGET KukaFRIClient OR FRI_Client_SDK_Cpp_FOUND) basis_target_link_libraries(FlatbuffersExampleWithKukaIiwaTest ${Boost_LIBRARIES} ${Boost_REGEX_LIBRARY} ${FRI_Client_SDK_Cpp_LIBRARIES} ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} KukaFRIClient ${FLATBUFFERS_STATIC_LIB}) basis_add_executable(readFlatbufferTest.cpp) - basis_target_link_libraries(readFlatbufferTest ${Boost_LIBRARIES} ${Boost_REGEX_LIBRARY} ${FRI_Client_SDK_Cpp_LIBRARIES} ${Nanopb_LIBRARIES} - ${CMAKE_THREAD_LIBS_INIT} KukaFRIClient ${FLATBUFFERS_STATIC_LIB} -lboost_iostreams -lboost_system -lboost_filesystem + basis_target_link_libraries(readFlatbufferTest + ${Boost_LIBRARIES} + ${Boost_REGEX_LIBRARY} + ${FRI_Client_SDK_Cpp_LIBRARIES} + ${Nanopb_LIBRARIES} + ${CMAKE_THREAD_LIBS_INIT} + ${FLATBUFFERS_STATIC_LIB} + -lboost_iostreams -lboost_system -lboost_filesystem + -ldl + KukaFRIClient mc_rbdyn_urdf SpaceVecAlg #TODO: re-enable when exported targets are correct again sch-core RBDyn - Tasks) - - basis_add_executable(KukaPoseEstimationTest.cpp) - basis_target_link_libraries(KukaPoseEstimationTest ${Boost_LIBRARIES} ${Boost_REGEX_LIBRARY} ${FRI_Client_SDK_Cpp_LIBRARIES} - ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} KukaFRIClient ${FLATBUFFERS_STATIC_LIB} -lboost_iostreams -lboost_system -lboost_filesystem - mc_rbdyn_urdf - SpaceVecAlg #TODO: re-enable when exported targets are correct again - sch-core - RBDyn - Tasks) + Tasks + v_repLib) + + # basis_add_executable(KukaPoseEstimationTest.cpp) + # basis_target_link_libraries(KukaPoseEstimationTest ${Boost_LIBRARIES} ${Boost_REGEX_LIBRARY} ${FRI_Client_SDK_Cpp_LIBRARIES} + # ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} KukaFRIClient ${FLATBUFFERS_STATIC_LIB} -lboost_iostreams -lboost_system -lboost_filesystem + # mc_rbdyn_urdf + # SpaceVecAlg #TODO: re-enable when exported targets are correct again + # sch-core + # RBDyn + # Tasks) endif() diff --git a/test/readFlatbufferTest.cpp b/test/readFlatbufferTest.cpp index ed3845c..6b7ace0 100644 --- a/test/readFlatbufferTest.cpp +++ b/test/readFlatbufferTest.cpp @@ -24,94 +24,109 @@ int main(int argc, char* argv[]) { + /// Define the file names std::string foldname = "/home/chunting/src/V-REP_PRO_EDU_V3_4_0_Linux/"; - std::string kukaTimeStamp = "2018_02_28_20_43_50_Kukaiiwa"; - std::string FTTimeStamp = "2018_02_28_20_43_16_FusionTrack"; + std::string kukaTimeStamp = "2018_03_05_10_26_21_Kukaiiwa"; + std::string FTTimeStamp = "2018_03_05_10_26_40_FusionTrack"; std::string kukaBinaryfile = foldname + kukaTimeStamp+ ".iiwa"; std::string fusiontrackBinaryfile = foldname + FTTimeStamp +".flik"; std::string foldtimestamp = current_date_and_time_string(); boost::filesystem::path dir{foldname+foldtimestamp}; boost::filesystem::create_directory(dir); - std::string KUKA_TimeEvent_CSVfilename = foldname + foldtimestamp + "/KUKA_TimeEvent.csv"; - std::string KUKA_Joint_CSVfilename = foldname + foldtimestamp + "/KUKA_Joint.csv"; - std::string KUKA_Pose_CSVfilename = foldname + foldtimestamp + "/KUKA_Pose.csv"; - std::string FudicialToRobotPose_CSVfilename = foldname + foldtimestamp + "/FudicialToRobot_Pose.csv"; - std::string FudicialToFTPose_CSVfilename = foldname + foldtimestamp + "/FudicialToFT_Pose.csv"; - - std::string FT_TimeEvent_CSVfilename = foldname + foldtimestamp + "/FT_TimeEvent.csv"; - std::string FT_Marker22_CSVfilename = foldname + foldtimestamp + "/FT_Pose_Marker22.csv"; - std::string FT_Marker55_CSVfilename = foldname + foldtimestamp + "/FT_Pose_Marker55.csv"; - - std::string FTKUKA_TimeEvent_CSVfilename = foldname + foldtimestamp + "/FTKUKA_TimeEvent.csv"; + std::string KUKA_TimeEvent_CSV = foldname + foldtimestamp + "/KUKA_TimeEvent.csv"; + std::string KUKA_Joint_CSV = foldname + foldtimestamp + "/KUKA_Joint.csv"; + std::string KUKA_Pose_CSV = foldname + foldtimestamp + "/KUKA_Pose.csv"; + std::string KUKA_Inverse_Pose_CSV = foldname + foldtimestamp + "/Inverse_KUKA_Pose.csv"; + std::string FudicialToRobotPose_CSV = foldname + foldtimestamp + "/FudicialToRobot_Pose.csv"; + std::string FudicialToFTPose_CSV = foldname + foldtimestamp + "/FudicialToFT_Pose.csv"; + std::string FT_TimeEvent_CSV = foldname + foldtimestamp + "/FT_TimeEvent.csv"; + std::string FT_Marker22_CSV = foldname + foldtimestamp + "/FT_Pose_Marker22.csv"; + std::string FT_Marker55_CSV = foldname + foldtimestamp + "/FT_Pose_Marker55.csv"; + std::string FTKUKA_TimeEvent_CSV = foldname + foldtimestamp + "/FTKUKA_TimeEvent.csv"; + // Put all the data into the same coordinate system --- Marker frame + bool inMarkerFrame = false; // Indicate the marker pose is in marker frame or tracker frame. /// Write FT data to CSV fbs_tk::Root logKUKAiiwaFusionTrackP = fbs_tk::open_root(fusiontrackBinaryfile); grl::MatrixXd timeEventM_FT = grl::getTimeStamp(logKUKAiiwaFusionTrackP, grl::fusiontracker_tag()); - //grl::regularizeTimeEvent(timeEventM_FT); + grl::regularizeTimeEvent(timeEventM_FT); std::size_t FT_size = timeEventM_FT.rows(); - uint32_t makerID_22 = 22; - uint32_t makerID_55 = 55; - //timeEvent_FT = grl::MatrixXd::Zero(); - Eigen::MatrixXd markerPose(FT_size, grl::col_FT_Pose); - int validsize_22 = grl::getMarkerPose(logKUKAiiwaFusionTrackP, makerID_22, timeEventM_FT, markerPose); + uint32_t markerID_22 = 22; + uint32_t markerID_55 = 55; + timeEventM_FT = grl::MatrixXd::Zero(FT_size, timeEventM_FT.cols()); + Eigen::MatrixXd markerPose(FT_size, grl::col_Pose); + // Get the pose of the marker 22. + // Since the bad data is skipped, the timeEventM_FT also needs to be filled out with the orinial time stamp. - grl::regularizeTimeEvent(timeEventM_FT); + int validsize_22 = grl::getMarkerPose(logKUKAiiwaFusionTrackP, markerID_22, timeEventM_FT, markerPose, inMarkerFrame); + std::cout<<"validsize_22: " << validsize_22 << std::endl; std::vector FT_Labels_Pose = grl::getLabels(grl::LabelsType::FT_Pose); if(validsize_22>2){ - grl::writeTimeEventToCSV(FT_TimeEvent_CSVfilename, timeEventM_FT); - grl::writeMatrixToCSV(FT_Marker22_CSVfilename, FT_Labels_Pose, timeEventM_FT, markerPose); - } + grl::regularizeTimeEvent(timeEventM_FT); + grl::writeTimeEventToCSV(FT_TimeEvent_CSV, timeEventM_FT); + grl::writeMatrixToCSV(FT_Marker22_CSV, FT_Labels_Pose, timeEventM_FT, markerPose); + } + // Change it back to the original size timeEventM_FT.resize(FT_size, grl::col_timeEvent); + markerPose.resize(FT_size, grl::col_Pose); + // Get the pose of the bone marker + int validsize_55 = grl::getMarkerPose(logKUKAiiwaFusionTrackP, markerID_55, timeEventM_FT, markerPose); - markerPose.resize(FT_size, grl::col_FT_Pose); - //timeEvent_FT = grl::MatrixXd::Zero(); - //markerPose = Eigen::MatrixXd::Zero(); - int validsize_55 = grl::getMarkerPose(logKUKAiiwaFusionTrackP, makerID_55, timeEventM_FT, markerPose); - grl::regularizeTimeEvent(timeEventM_FT); - - std::cout<<"validsize_22: " << validsize_22 << " validsize_55: " << validsize_55 << std::endl; - - + std::cout<<"validsize_55: " << validsize_55 << std::endl; if(validsize_55>2) { - grl::writeMatrixToCSV(FT_Marker55_CSVfilename, FT_Labels_Pose, timeEventM_FT, markerPose); + grl::regularizeTimeEvent(timeEventM_FT); + grl::writeMatrixToCSV(FT_Marker55_CSV, FT_Labels_Pose, timeEventM_FT, markerPose); } + ///////////////////////////////////////////////////////////////////////////////////////////////////// /// Write KUKA data to CSV + //////////////////////////////////////////////////////////////////////////////////////////////////// fbs_tk::Root kukaStatesP = fbs_tk::open_root(kukaBinaryfile); grl::MatrixXd timeEventM_Kuka = grl::getTimeStamp(kukaStatesP,grl::kuka_tag()); grl::regularizeTimeEvent(timeEventM_Kuka); - grl::writeTimeEventToCSV(KUKA_TimeEvent_CSVfilename, timeEventM_Kuka); + grl::writeTimeEventToCSV(KUKA_TimeEvent_CSV, timeEventM_Kuka); Eigen::MatrixXd jointAngles = grl::getAllJointAngles(kukaStatesP); std::vector Kuka_Joint_Labels = grl::getLabels(grl::LabelsType::Joint); if(jointAngles.rows() == timeEventM_Kuka.rows()){ - grl::writeMatrixToCSV(KUKA_Joint_CSVfilename, Kuka_Joint_Labels, timeEventM_Kuka, jointAngles); - std::vector Kuka_Pose_Labels = grl::getLabels(grl::LabelsType::Kuka_Pose); - std::vector PoseEE = grl::getPoseEE(jointAngles); - + // Write the joint angles into csv + grl::writeMatrixToCSV(KUKA_Joint_CSV, Kuka_Joint_Labels, timeEventM_Kuka, jointAngles); + std::vector PK_Pose_Labels = grl::getLabels(grl::LabelsType::Kuka_Pose); + // forward kinematic to get the of the end effector + // markerPose = true; // Get the marker pose directly, or get the end effector pose. + bool markerPose = false; + std::vector PoseEE = grl::getPoseEE(jointAngles, markerPose); + // convert the sva::PTransformd to Plucker coordinate. Eigen::MatrixXd PKPose = grl::getPluckerPose(PoseEE); - grl::writeMatrixToCSV(KUKA_Pose_CSVfilename, Kuka_Pose_Labels, timeEventM_Kuka, PKPose); + // Write the plucker coordinate into csv. + grl::writeMatrixToCSV(KUKA_Pose_CSV, PK_Pose_Labels, timeEventM_Kuka, PKPose); + // Invert the EE pose + std::vector inversePose = grl::invertPose(PoseEE); + // convert the sva::PTransformd to Plucker coordinate. + PKPose = Eigen::MatrixXd::Zero(PKPose.rows(), PKPose.cols()); + PKPose = grl::getPluckerPose(inversePose); + // Write the plucker coordinate into csv. + + grl::writeMatrixToCSV(KUKA_Inverse_Pose_CSV, PK_Pose_Labels, timeEventM_Kuka, PKPose); + - grl::getEEToFudicial22Matrix(PoseEE); - // PKPose.Zero(); - PKPose = grl::getPluckerPose(PoseEE); - grl::writeMatrixToCSV(FudicialToRobotPose_CSVfilename, Kuka_Pose_Labels, timeEventM_Kuka, PKPose); + // // Pose of end-effector againt robot base frame + // std::vector Fudicial_Robot_Pose = grl::getEEToFudicial22Matrix(PoseEE, inMarkerFrame); - grl::getRobotToTrackerMatrix(PoseEE); - // PKPose = Eigen::MatrixXd::Zero(); - PKPose = grl::getPluckerPose(PoseEE); - grl::writeMatrixToCSV(FudicialToFTPose_CSVfilename, Kuka_Pose_Labels, timeEventM_Kuka, PKPose); + // PKPose = Eigen::MatrixXd::Zero(PKPose.rows(), PKPose.cols()); + // PKPose = grl::getPluckerPose(Fudicial_Robot_Pose); + // grl::writeMatrixToCSV(FudicialToRobotPose_CSV, PK_Pose_Labels, timeEventM_Kuka, PKPose); } - // grl::writeFTKUKATimeEventToCSV(FTKUKA_TimeEvent_CSVfilename, logKUKAiiwaFusionTrackP, kukaStatesP); + // grl::writeFTKUKATimeEventToCSV(FTKUKA_TimeEvent_CSV, logKUKAiiwaFusionTrackP, kukaStatesP); return 0; } From 134753ebadeb2cc74a7fa7ae0df14665558f7641 Mon Sep 17 00:00:00 2001 From: Chunting Date: Thu, 8 Mar 2018 14:57:12 -0500 Subject: [PATCH 057/102] Modifications based on review --- include/grl/flatbuffer/FusionTrack.fbs | 8 +- include/grl/flatbuffer/KUKAiiwa.fbs | 2 +- .../grl/sensor/FusionTrackToFlatbuffer.hpp | 12 +- include/thirdparty/fbs_tk/fbs_tk.hpp | 150 +++++++++--------- src/java/grl/src/grl/FRIMode.java | 3 +- 5 files changed, 85 insertions(+), 90 deletions(-) diff --git a/include/grl/flatbuffer/FusionTrack.fbs b/include/grl/flatbuffer/FusionTrack.fbs index 0363e20..9aa5bdd 100644 --- a/include/grl/flatbuffer/FusionTrack.fbs +++ b/include/grl/flatbuffer/FusionTrack.fbs @@ -75,13 +75,13 @@ table FusionTrackFrame { height:uint; imageStrideInBytes:int; imageHeaderVersion:uint; - imageHeaderStatus:ftkQueryStatus; + imageHeaderStatus:int; imageLeftPixels:string; imageLeftPixelsVersion:uint; - imageLeftStatus:ftkQueryStatus; + imageLeftStatus:int; imageRightPixels:string; imageRightPixelsVersion:uint; - imageRightStatus:ftkQueryStatus; + imageRightStatus:int; regionsOfInterestLeft:[ftkRegionOfInterest]; regionsOfInterestLeftVersion:uint; regionsOfInterestLeftStatus:int; @@ -94,7 +94,7 @@ table FusionTrackFrame { markers:[ftkMarker]; markersVersion:uint; markersStatus:int; - deviceType:ftkDeviceType; + deviceType:int; ftkError:long; } diff --git a/include/grl/flatbuffer/KUKAiiwa.fbs b/include/grl/flatbuffer/KUKAiiwa.fbs index 634a74d..e4a40c0 100644 --- a/include/grl/flatbuffer/KUKAiiwa.fbs +++ b/include/grl/flatbuffer/KUKAiiwa.fbs @@ -240,7 +240,7 @@ table KUKAiiwaMonitorConfiguration { } // Get state data for the arm (things that change often) -// This table isn't used any more, since the main information of this table is the same with +// This table isn't used any more, since the main information of this table is the same with table FRIMessageLog table KUKAiiwaMonitorState { // this is the measured current state of the arm joints // as seen in monitorState and _FRIMonitoringMessage diff --git a/include/grl/sensor/FusionTrackToFlatbuffer.hpp b/include/grl/sensor/FusionTrackToFlatbuffer.hpp index 75abd4b..a24a9f2 100644 --- a/include/grl/sensor/FusionTrackToFlatbuffer.hpp +++ b/include/grl/sensor/FusionTrackToFlatbuffer.hpp @@ -365,16 +365,14 @@ toFlatBuffer(flatbuffers::FlatBufferBuilder &fbb, int32_t imageStrideInBytes = frame.imageHeader.imageStrideInBytes; uint32_t imageHeaderVersion = frame.FrameQueryP->imageHeaderVersionSize.Version; - grl::flatbuffer::ftkQueryStatus imageHeaderStatus = toFlatBuffer(frame.FrameQueryP->imageHeaderStat); + int32_t imageHeaderStatus = frame.FrameQueryP->imageHeaderStat; flatbuffers::Offset imageLeftPixels = frame.CameraImageLeftP ? fbb.CreateString(reinterpret_cast(frame.CameraImageLeftP->begin()), sizeof(frame.CameraImageLeftP)) : 0; uint32_t imageLeftPixelsVersion = frame.FrameQueryP->imageLeftVersionSize.Version; - // int32_t imageLeftStatus = frame.FrameQueryP->imageLeftStat; + int32_t imageLeftStatus = frame.FrameQueryP->imageLeftStat; - grl::flatbuffer::ftkQueryStatus imageLeftStatus = toFlatBuffer(frame.FrameQueryP->imageLeftStat); flatbuffers::Offset imageRightPixels = frame.CameraImageRightP ? fbb.CreateString(reinterpret_cast(frame.CameraImageRightP->begin()), sizeof(frame.CameraImageRightP)) : 0; uint32_t imageRightPixelsVersion = frame.FrameQueryP->imageRightVersionSize.Version; - // int32_t imageRightStatus = frame.FrameQueryP->imageRightStat; - grl::flatbuffer::ftkQueryStatus imageRightStatus = toFlatBuffer(frame.FrameQueryP->imageRightStat); + int32_t imageRightStatus = frame.FrameQueryP->imageRightStat; flatbuffers::Offset>> regionsOfInterestLeft = (writeRegionOfInterest && frame.ImageRegionOfInterestBoxesLeft.size()) ? toFlatBuffer(fbb, frame.ImageRegionOfInterestBoxesLeft): 0; @@ -393,9 +391,7 @@ toFlatBuffer(flatbuffers::FlatBufferBuilder &fbb, (writeMarkers && frame.Markers.size()) ? toFlatBuffer(fbb, frame.Markers, markerIndexToMarkerNames) : 0; uint32_t markersVersion = frame.FrameQueryP->markersVersionSize.Version; int32_t markersStatus = frame.FrameQueryP->markersStat; - // int32_t deviceType = frame.DeviceType; - auto deviceType = toFlatBuffer(frame.DeviceType, grl::flatbuffer::ftkDeviceType::DEV_UNKNOWN_DEVICE); - // auto deviceType = grl::flatbuffer::ftkDeviceType::DEV_SIMULATOR; + int32_t deviceType = frame.DeviceType; int64_t ftkError = frame.Error; return grl::flatbuffer::CreateFusionTrackFrame( diff --git a/include/thirdparty/fbs_tk/fbs_tk.hpp b/include/thirdparty/fbs_tk/fbs_tk.hpp index 40164be..4eb332e 100644 --- a/include/thirdparty/fbs_tk/fbs_tk.hpp +++ b/include/thirdparty/fbs_tk/fbs_tk.hpp @@ -349,81 +349,81 @@ struct hash> { }; } // namespace std -// #include "thirdparty/fbs_tk/fbs_tk.hpp" -// #include - -// using namespace std; -// using namespace flatbuffers; - -// /// This fbs_tk is different from the above on, since this one is located in namestd; -// namespace fbs_tk { - -// bool json_to_bin(flatbuffers::Parser &parser, const char *js, Buffer &bin) { - -// if (!parser.Parse(static_cast(js))) { -// return false; -// } -// copy_from(bin, parser.builder_); -// return true; -// } - -// bool json_to_fbs(flatbuffers::Parser &parser, std::istream &in, std::ostream &out) { -// string js; -// if (!load_buffer(in, &js)) { -// return false; -// } -// Buffer bin; -// if (!json_to_bin(parser, js.c_str(), bin)) { -// return false; -// } -// bin.write_data(out); -// return true; -// } - -// string bin_to_json(flatbuffers::Parser &parser, const Buffer &bin) { -// string buffer; -// GenerateText(parser, reinterpret_cast(bin.get_data().data()), &buffer); -// return buffer; -// } - -// // converts a binary FBS into a json object -// bool fbs_to_json(flatbuffers::Parser &parser, std::istream &in, std::ostream &out) { -// Buffer bin; -// bin.read_all_data(in); -// if (in.bad()) { -// return false; -// } -// out << bin_to_json(parser, bin); -// return out.good(); -// } - -// bool fbs_stream_to_jsonl(const string &schema, istream &in, ostream &out) { -// flatbuffers::Parser parser; -// parser.Parse(schema.c_str()); -// for (const auto & buff : range(in)) { -// out << bin_to_json(parser, buff) << endl; -// } -// return in.eof(); -// } - -// bool jsonl_to_fbs_stream(const string &schema, istream &in, ostream &out) { -// flatbuffers::Parser parser; -// parser.Parse(schema.c_str()); -// string js; -// Buffer bin; -// while(getline(in, js)) { -// if (!json_to_bin(parser, js.c_str(), bin)) { -// return false; -// } -// out << bin; -// if (out.bad()) { -// return false; -// } -// } -// return in.eof(); -// } - -// } // NAMESPACE +#include "thirdparty/fbs_tk/fbs_tk.hpp" +#include + +using namespace std; +using namespace flatbuffers; + + +namespace fbs_tk { + +bool json_to_bin(flatbuffers::Parser &parser, const char *js, Buffer &bin) { + + if (!parser.Parse(static_cast(js))) { + return false; + } + copy_from(bin, parser.builder_); + return true; +} + +bool json_to_fbs(flatbuffers::Parser &parser, std::istream &in, std::ostream &out) { + string js; + if (!load_buffer(in, &js)) { + return false; + } + Buffer bin; + if (!json_to_bin(parser, js.c_str(), bin)) { + return false; + } + bin.write_data(out); + return true; +} + +string bin_to_json(flatbuffers::Parser &parser, const Buffer &bin) { + string buffer; + GenerateText(parser, reinterpret_cast(bin.get_data().data()), &buffer); + return buffer; +} + +// converts a binary FBS into a json object +bool fbs_to_json(flatbuffers::Parser &parser, std::istream &in, std::ostream &out) { + Buffer bin; + bin.read_all_data(in); + if (in.bad()) { + return false; + } + out << bin_to_json(parser, bin); + return out.good(); +} + +bool fbs_stream_to_jsonl(const string &schema, istream &in, ostream &out) { + flatbuffers::Parser parser; + parser.Parse(schema.c_str()); + for (const auto & buff : range(in)) { + out << bin_to_json(parser, buff) << endl; + } + return in.eof(); +} + +bool jsonl_to_fbs_stream(const string &schema, istream &in, ostream &out) { + flatbuffers::Parser parser; + parser.Parse(schema.c_str()); + string js; + Buffer bin; + while(getline(in, js)) { + if (!json_to_bin(parser, js.c_str(), bin)) { + return false; + } + out << bin; + if (out.bad()) { + return false; + } + } + return in.eof(); +} + +} // NAMESPACE diff --git a/src/java/grl/src/grl/FRIMode.java b/src/java/grl/src/grl/FRIMode.java index 35d6118..3cadc2b 100755 --- a/src/java/grl/src/grl/FRIMode.java +++ b/src/java/grl/src/grl/FRIMode.java @@ -81,10 +81,9 @@ public boolean isCommandingWaitOrActive() { boolean ret; /** - * The method getFRIChannelInformation() can be used to poll the following information + * The method getFRIChannelInformation() can be used to poll the following information, the FRI connection and FRI state, * and save it in a variable of type FRIChannelInformation. * Type: FRIChannelInformation - * Variable in which the data for the FRI connection and FRI state are saved */ synchronized (this) { From e539c0b8f04ea3717115a72199c617a8e314891f Mon Sep 17 00:00:00 2001 From: Chunting Date: Wed, 14 Mar 2018 23:05:20 -0400 Subject: [PATCH 058/102] Add new marker --- include/grl/sensor/FusionTrack.hpp | 2 + .../grl/vrep/HandEyeCalibrationVrepPlugin.hpp | 32 +- src/lua/robone.lua | 324 +++++++++++++----- .../v_repExtAtracsysFusionTrack.cpp | 11 +- .../v_repExtHandEyeCalibration.cpp | 23 +- .../v_repExtKukaLBRiiwa.cpp | 21 +- test/CMakeLists.txt | 31 +- test/KinematicsCalibration_test.cpp | 167 +++++++++ test/kukaiiwaURDF.h | 10 + test/readFlatbufferTest.cpp | 19 +- 10 files changed, 505 insertions(+), 135 deletions(-) create mode 100644 test/KinematicsCalibration_test.cpp diff --git a/include/grl/sensor/FusionTrack.hpp b/include/grl/sensor/FusionTrack.hpp index c875434..1e4eaea 100644 --- a/include/grl/sensor/FusionTrack.hpp +++ b/include/grl/sensor/FusionTrack.hpp @@ -181,6 +181,8 @@ class FusionTrack std::vector geometries; geometries.push_back("geometry0022.ini"); geometries.push_back("geometry0055.ini"); + // Add the new marker attached to the frame + geometries.push_back("geometry50000.ini"); params.geometryFilenames = geometries; return params; } diff --git a/include/grl/vrep/HandEyeCalibrationVrepPlugin.hpp b/include/grl/vrep/HandEyeCalibrationVrepPlugin.hpp index 40fe622..9084859 100644 --- a/include/grl/vrep/HandEyeCalibrationVrepPlugin.hpp +++ b/include/grl/vrep/HandEyeCalibrationVrepPlugin.hpp @@ -83,7 +83,7 @@ void construct(){ void addFrame() { - logger_->info( "Adding hand eye calibration frame #", ++frameCount); + logger_->info( "Adding hand eye calibration frame {}", ++frameCount); auto robotTipInRobotBase = getObjectTransform(robotTip,robotBase); auto fiducialInOpticalTrackerBase = getObjectTransform(opticalTrackerDetectedObjectName,opticalTrackerBase); @@ -94,8 +94,8 @@ void addFrame() { isFirstFrame = false; } - auto robotTipInFirstTipBase = firstRobotTipInRobotBaseInverse * robotTipInRobotBase; // A_0_Inv * A_i - auto fiducialInFirstFiducialBase = firstFiducialInOpticalTrackerBaseInverse * fiducialInOpticalTrackerBase; // B_0_Inv * B_i + auto robotTipInFirstTipBase = firstRobotTipInRobotBaseInverse * robotTipInRobotBase; // B_0_Inv * B_i + auto fiducialInFirstFiducialBase = firstFiducialInOpticalTrackerBaseInverse * fiducialInOpticalTrackerBase; // A_0_Inv * A_i rvecsArm.push_back( eigenRotToEigenVector3dAngleAxis(robotTipInFirstTipBase.rotation() )); @@ -107,11 +107,11 @@ void addFrame() { if(debug){ - logger_->info( "\nrobotTipInRobotBase:\n", poseString(robotTipInRobotBase)); - logger_->info( "\nfiducialInOpticalTrackerBase:\n", poseString(fiducialInOpticalTrackerBase)); + logger_->info( "\nrobotTipInRobotBase: \n{}", poseString(robotTipInRobotBase)); + logger_->info( "\nfiducialInOpticalTrackerBase: \n{}", poseString(fiducialInOpticalTrackerBase)); - logger_->info( "\nrobotTipInFirstTipBase:\n", poseString(robotTipInFirstTipBase)); - logger_->info( "\nfiducialInFirstFiducialBase:\n", poseString(fiducialInFirstFiducialBase)); + logger_->info( "\nrobotTipInFirstTipBase: \n{}", poseString(robotTipInFirstTipBase)); + logger_->info( "\nfiducialInFirstFiducialBase: \n{}", poseString(fiducialInFirstFiducialBase)); // print simulation transfrom from tip to fiducial Eigen::Affine3d RobotTipToFiducial = getObjectTransform(opticalTrackerDetectedObjectName,robotTip); @@ -129,8 +129,8 @@ void addFrame() { /// @todo evaluate if applyEstimate should not be called by this void estimateHandEyeScrew(){ - logger_->info( "Running Hand Eye Screw Estimate with the following numbers of entries in each category: rvecsFiducial",rvecsFiducial.size(), - " tvecsFiducial: ", tvecsFiducial.size(), " rvecsArm: ", rvecsArm.size(), " tvecsArm: ", tvecsArm.size()); + logger_->info( "Running Hand Eye Screw Estimate with the following numbers of entries in each category: rvecsFiducial {} , tvecsFiducial: {} , rvecsArm: {} , tvecsArm: {}" + ,rvecsFiducial.size(), tvecsFiducial.size(), rvecsArm.size(), tvecsArm.size()); BOOST_VERIFY(allHandlesSet); @@ -140,7 +140,8 @@ void estimateHandEyeScrew(){ tvecsArm, rvecsFiducial, tvecsFiducial, - transformEstimate.matrix() + transformEstimate.matrix(), + false //PlanarMotion ); @@ -153,18 +154,21 @@ void estimateHandEyeScrew(){ if(debug){ // print simulation transfrom from tip to fiducial Eigen::Affine3d RobotTipToFiducial = getObjectTransform(opticalTrackerDetectedObjectName,robotTip); - logger_->info( "\n", poseString(RobotTipToFiducial,"expected RobotTipToFiducial (simulation only): ")); + logger_->info( "\n{}", poseString(RobotTipToFiducial,"expected RobotTipToFiducial (simulation only): ")); } - logger_->info( "\n", poseString(transformEstimate,"estimated RobotTipToFiducial:")); + logger_->info( "\n{}", poseString(transformEstimate,"estimated RobotTipToFiducial:")); applyEstimate(); // print results Eigen::Quaterniond eigenQuat(transformEstimate.rotation()); - logger_->info( "Hand Eye Screw Estimate quat wxyz\n: ", eigenQuat.w(), " ", eigenQuat.x(), " ", eigenQuat.y(), " ", eigenQuat.z(), " ", " translation xyz: ", transformEstimate.translation().x(), " ", transformEstimate.translation().y(), " ", transformEstimate.translation().z(), " "); + logger_->info( "Hand Eye Screw Estimate quat wxyz\n: {} , {} , {} , {} \n translation xyz: {} {} {}", + eigenQuat.w(), eigenQuat.x(), eigenQuat.y(),eigenQuat.z(), transformEstimate.translation().x(), transformEstimate.translation().y(), transformEstimate.translation().z()); - logger_->info( "Optical Tracker Base Measured quat wxyz\n: ", detectedObjectQuaternion[0], " ", detectedObjectQuaternion[1], " ", detectedObjectQuaternion[2], " ", detectedObjectQuaternion[3], " ", " translation xyz: ", detectedObjectPosition[0], " ", detectedObjectPosition[1], " ", detectedObjectPosition[2], " "); + logger_->info( "Optical Tracker Base Measured quat wxyz\n: {} , {} , {} , {} \n translation xyz: {} , {}, {}", + detectedObjectQuaternion[0], detectedObjectQuaternion[1], detectedObjectQuaternion[2], detectedObjectQuaternion[3], + detectedObjectPosition[0], detectedObjectPosition[1], detectedObjectPosition[2]); } diff --git a/src/lua/robone.lua b/src/lua/robone.lua index 0c4adce..f6ba601 100644 --- a/src/lua/robone.lua +++ b/src/lua/robone.lua @@ -59,70 +59,74 @@ robone.cutBoneScript=function() --maxVel = 0.04 --maxForce = 30 - while true do - if runFollowPathMode then - -- Moves an object to the position/orientation of another moving object (target object) - -- by performing interpolations (i.e. the object will effectiviely follow the target object). - simMoveToObject(target,CreatedPathHandle,3,0,0.5,0.02) - -- http://www.coppeliarobotics.com/helpFiles/en/apiFunctions.htm#simFollowPath - simFollowPath(target,CreatedPathHandle,3,0,0.01,0.01) - --simSwitchThread() - - else - -- Calculate path length - pathLength = simGetPathLength(CreatedPathHandle) - -- Move to start of path - - simMoveToObject(target,CreatedPathHandle,3,0,0.2,0.02) - newPosition = 0 - - - - -- Create empty vector for tool tip forces (x,y,z,alpha,beta,gamma,joint dependence(?)) - --toolTipForces = matrix(7,1,0) - toolTipForces = {0,0,0,0,0,0,0} - newPos = simGetPositionOnPath(CreatedPathHandle,0) - -- While not at end of path, traverse path - while newPosition < pathLength do - print("VREP JOINT POSITIONS") - for i=1,7,1 do - print(simGetJointPosition(jointHandles[i])) - end - externalJointTorque = getExternalJointTorque(measuredJointHandles, externalTorqueHandle) - - measuredForce = calcToolTipForce(IKGroupHandle, externalJointTorque) - print("MEASURED FORCE: "..measuredForce) - - position = simGetPathPosition(CreatedPathHandle) - offset = 0.001 - newPosition = position+offset - - simSetPathPosition(CreatedPathHandle, newPosition) - nextPos = simGetPositionOnPath(CreatedPathHandle, newPosition/pathLength) - nextEul = simGetOrientationOnPath(CreatedPathHandle, newPosition/pathLength) - nextQuat = simGetQuaternionFromMatrix(simBuildMatrix({0,0,0},nextEul)) - nextVel = {0,0,0,0} - for i=1,3,1 do - nextVel[i] = 3*(nextPos[i]-newPos[i]) - end - --timeBefore = simGetSimulationTime() - --print("Time nonRML takes: ".. timeBefore-timeAfter) - --print("Time before RML: "..timeBefore) - result,newPos,newQuat,currentVel,currentAccel,timeLeft=simRMLMoveToPosition(target,-1,-1,currentVel,currentAccel,maxVel,maxAccel,maxJerk,nextPos,nextQuat,nextVel) - --timeAfter = simGetSimulationTime() - --print("Time after RML: "..timeAfter) - --print("Time RML takes: ".. timeAfter-timeBefore) - --print("Time left: "..timeLeft) - - - --fcv(IKGroupHandle, CreatedPathHandle, maxVel, maxForce, measuredForce, pathLength) - - --simSwitchThread() - - end - simSetPathPosition(CreatedPathHandle, 0) - end - end + while true do + if runFollowPathMode then + -- Moves an object ('RobotMillTipTarget') to the position/orientation of another moving object (target object, 'MillHipCutPath' or 'Straingtline') + -- by performing interpolations (i.e. the object will effectiviely follow the target object). + simMoveToObject(target,CreatedPathHandle,3,0,0.5,0.02) + -- http://www.coppeliarobotics.com/helpFiles/en/apiFunctions.htm#simFollowPath + simFollowPath(target,CreatedPathHandle,3,0,0.01,0.01) + --simSwitchThread() + + else + -- Calculate path length + -- Retrieves the length of a path object. + pathLength = simGetPathLength(CreatedPathHandle) + -- Move to start of path + + simMoveToObject(target,CreatedPathHandle,3,0,0.2,0.02) + newPosition = 0 + + + + -- Create empty vector for tool tip forces (x,y,z,alpha,beta,gamma,joint dependence(?)) + -- toolTipForces = matrix(7,1,0) + toolTipForces = {0,0,0,0,0,0,0} + -- Retrieves the absolute interpolated position of a point along a path object. + -- 0 means the beginning of the path + newPos = simGetPositionOnPath(CreatedPathHandle,0) + -- While not at end of path, traverse path + while newPosition < pathLength do + print("VREP JOINT POSITIONS") + for i=1,7,1 do + print(simGetJointPosition(jointHandles[i])) + end + externalJointTorque = getExternalJointTorque(measuredJointHandles, externalTorqueHandle) + + measuredForce = calcToolTipForce(IKGroupHandle, externalJointTorque) + print("MEASURED FORCE: "..measuredForce) + -- Retrieves the intrinsic position of a path object (a distance along the path). + position = simGetPathPosition(CreatedPathHandle) + offset = 0.001 -- step size + newPosition = position+offset + -- Sets the intrinsic position of a path object (i.e. the position along the path). + simSetPathPosition(CreatedPathHandle, newPosition) + nextPos = simGetPositionOnPath(CreatedPathHandle, newPosition/pathLength) + nextEul = simGetOrientationOnPath(CreatedPathHandle, newPosition/pathLength) + nextQuat = simGetQuaternionFromMatrix(simBuildMatrix({0,0,0},nextEul)) + nextVel = {0,0,0,0} + for i=1,3,1 do + nextVel[i] = 3*(nextPos[i]-newPos[i]) + end + --timeBefore = simGetSimulationTime() + --print("Time nonRML takes: ".. timeBefore-timeAfter) + --print("Time before RML: "..timeBefore) + --[[Moves an object to a given position and/or orientation using the Reflexxes Motion Library type II or IV. This function can only be called from child scripts running in a thread --]] + result,newPos,newQuat,currentVel,currentAccel,timeLeft=simRMLMoveToPosition(target,-1,-1,currentVel,currentAccel,maxVel,maxAccel,maxJerk,nextPos,nextQuat,nextVel) + --timeAfter = simGetSimulationTime() + --print("Time after RML: "..timeAfter) + --print("Time RML takes: ".. timeAfter-timeBefore) + --print("Time left: "..timeLeft) + + + --fcv(IKGroupHandle, CreatedPathHandle, maxVel, maxForce, measuredForce, pathLength) + + --simSwitchThread() + + end + simSetPathPosition(CreatedPathHandle, 0) + end + end --Path0P,Path0O=grl.getTransformToPathPointInWorldFrame(BallJointPath,0) --pathPos=simGetObjectPosition(BallJointPath,targetBase) @@ -225,6 +229,7 @@ robone.cutBoneScript=function() end function fcv(ikGroupHandle, CreatedPathHandle, maxVelocity, maxForce, measuredForce, pathLength) + print("run in fcv............... ") -- selection parameter for simRMLPos for default behavior selection = {1,1,1,1,1,1,1} @@ -265,7 +270,7 @@ robone.cutBoneScript=function() bone=simGetObjectHandle('FemurBone') table=simGetObjectHandle('highTable') - testStraightLine = false + testStraightLine = true CreatedPathHandle=simGetObjectHandle('MillHipCutPath') @@ -343,8 +348,8 @@ robone.handEyeCalibScript=function() maxJerk={jerk,jerk,jerk,jerk*DtoR} targetVel={targetV,targetV,targetV,targetV*DtoR} - target=simGetObjectHandle('RobotMillTipTarget') - -- target=simGetObjectHandle('RobotFlangeTipTarget') + -- target=simGetObjectHandle('RobotMillTipTarget') + target=simGetObjectHandle('RobotFlangeTipTarget') targetBase=simGetObjectHandle('Robotiiwa') path=simGetObjectHandle('HandEyeCalibPath') circleCalib = simGetObjectHandle('CircleCalibPath') @@ -355,7 +360,7 @@ robone.handEyeCalibScript=function() -- Enable/Disable custom IK useGrlInverseKinematics=true - simExtHandEyeCalibStart('Robotiiwa' , 'RobotMillTipTarget', 'OpticalTrackerBase', 'Fiducial#22') + --simExtHandEyeCalibStart('Robotiiwa' , 'RobotFlangeTipTarget', 'OpticalTrackerBase', 'Fiducial#22') if (grl.isModuleLoaded('GrlInverseKinematics') and useGrlInverseKinematics) then simExtGrlInverseKinematicsStart() @@ -367,8 +372,10 @@ robone.handEyeCalibScript=function() -- Run the hand eye calibration - simExtHandEyeCalibStart() - + -- simExtHandEyeCalibStart() + -- call handEyeCalibrationPG->construct(); + simExtHandEyeCalibStart('Robotiiwa' , 'RobotFlangeTipTarget', 'OpticalTrackerBase', 'Fiducial#22') + -- Fill out the vectors, and then pass these vectors to for i=0,1,1/numSteps do p,o=grl.getPathPointInWorldFrame(path,i) simRMLMoveToPosition(target,-1,-1,currentVel,currentAccel,maxVel,maxAccel,maxJerk,p,o,nil) @@ -441,6 +448,62 @@ robone.startRealArmDriverScript=function() else simDisplayDialog('Error','KukaLBRiiwa plugin was not found. (v_repExtKukaLBRiiwa.dll)&&nSimulation will run without hardware',sim_dlgstyle_ok,true,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1}) end +end + +robone.coordinateCalibrateionScript=function() + print("coordinateCalibrateionScript--------------") +-- Check if the required plugin is there: + if (grl.isModuleLoaded('KukaLBRiiwa')) then + + -- Now disable IK and control one individual joint: + -- simSetExplicitHandling(ikGroupHandle,0) -- this disables automatic handling of the ik group + + + testJointAngles = { -0.06, -0.03, 0.01, -0.06, -0.04, -0.02, 0.1} + jointHandles={-1,-1,-1,-1,-1,-1,-1} + jointHandles[1]=simGetObjectHandle('LBR_iiwa_14_R820_joint1') + for i=2,7,1 do + jointHandles[i]=simGetObjectHandle('LBR_iiwa_14_R820_joint'..i) + result = simSetJointPosition(jointHandles[i],0) + simWait(0.5) + print(result.."======--------------") + + + + relativePosition = simGetObjectPosition(jointHandles[i], jointHandles[i-1]) + print('XYZ') + for i=1,3,1 do + print(relativePosition[i].." ") + end + + relativeOrientation = simGetObjectOrientation(RobotFlangeTip, robothandle) + print('RPY') + for i=1,3,1 do + print(relativeOrientation[i]*180/math.pi.." ") + end + + end + simWait(50) + --robotFlangeTip = simGetObjectHandle ('RobotFlangeTip') + --robothandle = simGetObjectHandle ('LBR_iiwa_14_R820') + --Position = simGetObjectPosition(robotFlangeTip, robothandle) + --print('The Position of RobotFlangeTip in RobotBase Frame') + --for i=1,3,1 do + -- print(Position[i].." ") + --end + --Orientation = simGetObjectOrientation(robotFlangeTip, robothandle) + --print('The Orientation of RobotFlangeTip in RobotBase Frame') + --for i=1,3,1 do + -- print(Orientation[i].." ") + --end + + else + simDisplayDialog('Error','KukaLBRiiwa plugin was not found. (v_repExtKukaLBRiiwa.dll)&&nSimulation will run without hardware',sim_dlgstyle_ok,true,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1}) + end + + + + end ------------------------------------------ @@ -471,6 +534,10 @@ robone.configureOpticalTracker=function() simExtAtracsysFusionTrackAddGeometry('geometry0022.ini') simAddStatusbarMessage('robone.configureOpticalTracker() + v_repExtAtracsysFusionTrackVrepPlugin: loading geometry0055.ini') simExtAtracsysFusionTrackAddGeometry('geometry0055.ini') + -- Add the new geometry attached to the frame. + -- geometry50000.ini + simAddStatusbarMessage('robone.configureOpticalTracker() + v_repExtAtracsysFusionTrackVrepPlugin: loading geometry50000.ini') + simExtAtracsysFusionTrackAddGeometry('geometry50000.ini') -------------------------------------------------- @@ -487,8 +554,19 @@ robone.configureOpticalTracker=function() 'Fiducial#22', -- ObjectBeingMeasured '22' -- GeometryID ) + + + + -- Set the new marker in vrep based on the Fiducial#22, the relative pose is read from FusionTracker + simExtAtracsysFusionTrackAddObject('Fiducial#50000', -- ObjectToMove + 'OpticalTrackerBase#0', -- FrameInWhichToMoveObject + 'Fiducial#50000', -- ObjectBeingMeasured + '50000' -- GeometryID + ) + end + -------------------------------------------------- -- Move the Bone -- true enables moving the bone, false disables it @@ -502,6 +580,13 @@ robone.configureOpticalTracker=function() 'Fiducial#55', -- ObjectBeingMeasured '55' -- GeometryID ) + robothandle = simGetObjectHandle ('LBR_iiwa_14_R820#0') + --trankerhandle = simGetObjectHandle ('OpticalTrackerBase#0') + robotToWorldM = simGetObjectMatrix(robothandle, -1) + print('The absolute transformation matrix of LBR_iiwa_14_R820#0') + for i=1,12,1 do + print(robotToWorldM[i].." ") + end end -- Start collecting data from the optical tracker @@ -511,30 +596,91 @@ robone.configureOpticalTracker=function() simExtAtracsysFusionTrackRecordWhileSimulationIsRunning(true) -------------------------------------------------- -- Get the relative transformation matrix between Fiducial#22 and LBR_iiwa_14_R820_link8 + --[[ + if (moveTracker) then - fiducial22handl = simGetObjectHandle ('Fiducial#22') - link8handl = simGetObjectHandle ('LBR_iiwa_14_R820_link8') - relativeTransformationMatrix = simGetObjectMatrix(fiducial22handl, link8handl) - print('The relative transformation matrix between Fiducial#22 and LBR_iiwa_14_R820_link8') - for i=1,12,1 do - print(relativeTransformationMatrix[i].." ") - end - robothandle = simGetObjectHandle ('LBR_iiwa_14_R820#0') - trankerhandle = simGetObjectHandle ('OpticalTrackerBase#0') - robotToTrakerM = simGetObjectMatrix(robothandle, trankerhandle) - print('The relative transformation matrix between LBR_iiwa_14_R820#0 and OpticalTrackerBase#0') - for i=1,12,1 do - print(robotToTrakerM[i].." ") - end + -- testJointAngles = { -0.06, -0.03, 0.01, -0.06, -0.04, -0.02, 0.1} + jointHandles={-1,-1,-1,-1,-1,-1,-1} + jointHandles[1]=simGetObjectHandle('LBR_iiwa_14_R820_joint1') + for i=1,7,1 do + jointHandles[i]=simGetObjectHandle('LBR_iiwa_14_R820_joint'..i) + -- result = simSetJointPosition(jointHandles[i],testJointAngles[i]) + end - fudicialToRobotM = simGetObjectMatrix(fiducial22handl, robothandle) - print('The relative transformation matrix between Fiducial#22 and LBR_iiwa_14_R820#0') - for i=1,12,1 do - print(fudicialToRobotM[i].." ") - end + + + RobotFlangeTip = simGetObjectHandle ('RobotFlangeTip') + robothandle = simGetObjectHandle ('LBR_iiwa_14_R820') + fiducial22handl = simGetObjectHandle ('Fiducial#22') + relativeTransformationMatrix = simGetObjectMatrix(fiducial22handl, RobotFlangeTip) + print('The relative transformation matrix between Fiducial#22 and RobotFlangeTip') + for i=1,12,1 do + print(relativeTransformationMatrix[i].." ") + end + + + trankerhandle = simGetObjectHandle ('OpticalTrackerBase#0') + robotToTrakerM = simGetObjectMatrix(robothandle, trankerhandle) + print('The relative transformation matrix between LBR_iiwa_14_R820#0 and OpticalTrackerBase#0') + for i=1,12,1 do + print(robotToTrakerM[i].." ") + end + + fudicialToRobotM = simGetObjectMatrix(fiducial22handl, robothandle) + print('The relative transformation matrix between Fiducial#22 and LBR_iiwa_14_R820') + for i=1,12,1 do + print(fudicialToRobotM[i].." ") + end + + relativePosition = simGetObjectPosition(RobotFlangeTip, robothandle) + print('The relative position between RobotFlangeTip and LBR_iiwa_14_R820 ') + for i=1,3,1 do + print(relativePosition[i].." ") + end + + relativeOrientation = simGetObjectOrientation(RobotFlangeTip, robothandle) + print('The relative orientation between RobotFlangeTip and LBR_iiwa_14_R820') + for i=1,3,1 do + print(relativeOrientation[i]*180/math.pi.." ") + end + + + print("======--------------") + relativePosition = simGetObjectPosition(jointHandles[1], -1) + print('XYZ'..1) + for i=1,3,1 do + print(relativePosition[i].." ") + end + + relativeOrientation = simGetObjectOrientation(jointHandles[1], -1) + print('RPY') + for i=1,3,1 do + print(relativeOrientation[i]*180/math.pi.." ") + end + for i=2,7,1 do + --result = simSetJointPosition(jointHandles[i],0) + --simWait(0.5) + print("======--------------") + relativePosition = simGetObjectPosition(jointHandles[i], jointHandles[i-1]) + print('XYZ'..i) + for j=1,3,1 do + print(relativePosition[j].." ") + end + + relativeOrientation = simGetObjectOrientation(jointHandles[i], jointHandles[i-1]) + print('RPY'..i) + for j=1,3,1 do + print(relativeOrientation[j]*180/math.pi.." ") + end + + end + + end + --]] end + -- By default we disable customization script execution during simulation, in order -- to run simulations faster: simSetScriptAttribute(sim_handle_self,sim_customizationscriptattribute_activeduringsimulation,false) diff --git a/src/v_repExtAtracsysFusionTrack/v_repExtAtracsysFusionTrack.cpp b/src/v_repExtAtracsysFusionTrack/v_repExtAtracsysFusionTrack.cpp index 128026e..b01ba30 100755 --- a/src/v_repExtAtracsysFusionTrack/v_repExtAtracsysFusionTrack.cpp +++ b/src/v_repExtAtracsysFusionTrack/v_repExtAtracsysFusionTrack.cpp @@ -187,6 +187,7 @@ void LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_OBJECT(SLuaCallBack *p) std::string(" GeometryID: ") + inData->at(3).stringData[0]; // GeometryID loggerPG->info(output); // convert the tuple of strings to a tuple of ints + // convert vrep string names to vrep handle integer ids auto newObjectToTrackIDs = grl::VrepStringToLogAndTrackMotionConfigParams(newObjectToTrack); fusionTrackParamsG.MotionConfigParamsVector.push_back(newObjectToTrackIDs); @@ -697,7 +698,7 @@ VREP_DLLEXPORT void *v_repMessage(int message, int *auxiliaryData, void *customD } if (message == sim_message_eventcallback_simulationabouttostart) - { // Simulation is about to start + { // Simulation is about to start. CLick the start button in VREP ///////////////////////// // PUT OBJECT STARTUP CODE HERE @@ -719,11 +720,11 @@ VREP_DLLEXPORT void *v_repMessage(int message, int *auxiliaryData, void *customD // LoggerPG->error( initerr); // } - // if(fusionTrackPG && recordWhileSimulationIsRunningG) { - // fusionTrackPG->start_recording(); - // } + if(fusionTrackPG && recordWhileSimulationIsRunningG) { + fusionTrackPG->start_recording(); + } } - + // simulation has ended if (message == sim_message_eventcallback_simulationended) { // Simulation just ended diff --git a/src/v_repExtHandEyeCalibration/v_repExtHandEyeCalibration.cpp b/src/v_repExtHandEyeCalibration/v_repExtHandEyeCalibration.cpp index ee677aa..6004a44 100755 --- a/src/v_repExtHandEyeCalibration/v_repExtHandEyeCalibration.cpp +++ b/src/v_repExtHandEyeCalibration/v_repExtHandEyeCalibration.cpp @@ -40,6 +40,7 @@ LIBRARY vrepLib; // the V-REP library that we will dynamically load and bind #define CONCAT(x,y,z) x y z #define strConCat(x,y,z) CONCAT(x,y,z) #define LUA_GET_SENSOR_DATA_COMMAND "simExtSkeleton_getSensorData" +#define LUA_SIM_EXT_HAND_EYE_CALIB_START_COMMAND "simExtHandEyeCalibStart" @@ -66,8 +67,8 @@ const int inArgs_HAND_EYE_CALIB_START[]={ sim_lua_arg_string,0, // "Robotiiwa" , // OpticalTrackerBaseName, sim_lua_arg_string,0, // "tcp://0.0.0.0:30010" , // OpticalTrackerTipName }; - -std::string LUA_SIM_EXT_HAND_EYE_CALIB_START_CALL_TIP("number result=simExtHandEyeCalibStart(string RobotBaseName , string RobotTipName, string OpticalTrackerBaseName, string OpticalTrackerDetectedObjectName) -- KukaCommandMode options are JAVA and FRI"); +// -- KukaCommandMode options are JAVA and FRI +std::string LUA_SIM_EXT_HAND_EYE_CALIB_START_CALL_TIP("number result=simExtHandEyeCalibStart(string RobotBaseName , string RobotTipName, string OpticalTrackerBaseName, string OpticalTrackerDetectedObjectName)"); void LUA_SIM_EXT_HAND_EYE_CALIB_START(SLuaCallBack* p) @@ -78,7 +79,7 @@ void LUA_SIM_EXT_HAND_EYE_CALIB_START(SLuaCallBack* p) CLuaFunctionData data; - if (data.readDataFromLua(p,inArgs_HAND_EYE_CALIB_START,inArgs_HAND_EYE_CALIB_START[0],"simExtHandEyeCalibStart")) + if (data.readDataFromLua(p,inArgs_HAND_EYE_CALIB_START,inArgs_HAND_EYE_CALIB_START[0], LUA_SIM_EXT_HAND_EYE_CALIB_START_COMMAND)) { std::vector* inData=data.getInDataPtr(); std::string RobotBaseName((inData->at(0 ).stringData[0])); @@ -86,7 +87,7 @@ void LUA_SIM_EXT_HAND_EYE_CALIB_START(SLuaCallBack* p) std::string OpticalTrackerBaseName((inData->at(2 ).stringData[0])); std::string OpticalTrackerDetectedObjectName(inData->at(3 ).stringData[0]); handEyeCalibrationPG=std::make_shared( - std::make_tuple(RobotBaseName , RobotTipName, OpticalTrackerBaseName, OpticalTrackerDetectedObjectName) + // std::make_tuple(RobotBaseName , RobotTipName, OpticalTrackerBaseName, OpticalTrackerDetectedObjectName) ); handEyeCalibrationPG->construct(); @@ -205,9 +206,21 @@ VREP_DLLEXPORT unsigned char v_repStart(void* reservedPointer,int reservedInt) } // ****************************************** + std::vector inArgs; + + CLuaFunctionData::getInputDataForFunctionRegistration(inArgs_HAND_EYE_CALIB_START,inArgs); + simRegisterCustomLuaFunction + ( + LUA_SIM_EXT_HAND_EYE_CALIB_START_COMMAND, + LUA_SIM_EXT_HAND_EYE_CALIB_START_CALL_TIP.c_str(), + &inArgs[0], + LUA_SIM_EXT_HAND_EYE_CALIB_START + ); + int noArgs[]={0}; // no input arguments - simRegisterCustomLuaFunction("simExtHandEyeCalibStart","number result=simExtHandEyeCalibStart()",noArgs,LUA_SIM_EXT_HAND_EYE_CALIB_START); + //simRegisterCustomLuaFunction("simExtHandEyeCalibStart","number result=simExtHandEyeCalibStart()",noArgs,LUA_SIM_EXT_HAND_EYE_CALIB_START); + //simRegisterCustomLuaFunction("simExtHandEyeCalibStart","number result=simExtHandEyeCalibStart()",inArgs_HAND_EYE_CALIB_START,LUA_SIM_EXT_HAND_EYE_CALIB_START); simRegisterCustomLuaFunction("simExtHandEyeCalibStop","number result=simExtHandEyeCalibStop()",noArgs,LUA_SIM_EXT_HAND_EYE_CALIB_STOP); simRegisterCustomLuaFunction("simExtHandEyeCalibReset","number result=simExtHandEyeCalibReset()",noArgs,LUA_SIM_EXT_HAND_EYE_CALIB_RESET); simRegisterCustomLuaFunction("simExtHandEyeCalibAddFrame","number result=simExtHandEyeCalibAddFrame()",noArgs,LUA_SIM_EXT_HAND_EYE_CALIB_ADD_FRAME); diff --git a/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp b/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp index e07a152..6798395 100755 --- a/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp +++ b/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp @@ -515,18 +515,21 @@ VREP_DLLEXPORT void* v_repMessage(int message,int* auxiliaryData,void* customDat //////////////////// loggerPG->error("Ending KUKA LBR iiwa plugin connection to Kuka iiwa\n" ); loggerPG->info("recordWhileSimulationIsRunningG: {}", recordWhileSimulationIsRunningG); - loggerPG->info("kukaVrepPluginPG->is_recording(): {}", kukaVrepPluginPG->is_recording()); + if (kukaVrepPluginPG.get() != nullptr) { - if(kukaVrepPluginPG && recordWhileSimulationIsRunningG && kukaVrepPluginPG->is_recording()) { - bool success = kukaVrepPluginPG->save_recording(); - if(success) { - loggerPG->info("Vrep quits successfully..." ); - } + loggerPG->info("kukaVrepPluginPG->is_recording(): {}", kukaVrepPluginPG->is_recording()); + + if(kukaVrepPluginPG && recordWhileSimulationIsRunningG && kukaVrepPluginPG->is_recording()) { + bool success = kukaVrepPluginPG->save_recording(); + if(success) { + loggerPG->info("Vrep quits successfully..." ); + } - kukaVrepPluginPG->stop_recording(); + kukaVrepPluginPG->stop_recording(); + } + kukaVrepPluginPG->clear_recording(); + kukaVrepPluginPG.reset(); } - kukaVrepPluginPG->clear_recording(); - kukaVrepPluginPG.reset(); } diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 988d223..a050418 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -90,8 +90,13 @@ if(TARGET KukaFRIClient OR FRI_Client_SDK_Cpp_FOUND) endif() basis_add_test(KukaLBRiiwaVrepPluginTest.cpp) - basis_target_link_libraries(KukaLBRiiwaVrepPluginTest ${Boost_LIBRARIES} ${Boost_REGEX_LIBRARY} ${Boost_UNIT_TEST_FRAMEWORK_LIBRARY} ${FRI_Client_SDK_Cpp_LIBRARIES} ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} - ${FLATBUFFERS_STATIC_LIB} v_repLib KukaFRIClient ${LINUX_ONLY_LIBS} ) + basis_target_link_libraries(KukaLBRiiwaVrepPluginTest + ${Boost_LIBRARIES} + ${Boost_REGEX_LIBRARY} + ${Boost_UNIT_TEST_FRAMEWORK_LIBRARY} + ${FRI_Client_SDK_Cpp_LIBRARIES} + ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} + ${FLATBUFFERS_STATIC_LIB} v_repLib KukaFRIClient ${LINUX_ONLY_LIBS} ) basis_add_executable(FlatbuffersExampleWithKukaIiwaTest.cpp)# ${GRL_FLATBUFFERS_OUTPUTS}) basis_target_link_libraries(FlatbuffersExampleWithKukaIiwaTest ${Boost_LIBRARIES} ${Boost_REGEX_LIBRARY} ${FRI_Client_SDK_Cpp_LIBRARIES} ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} KukaFRIClient ${FLATBUFFERS_STATIC_LIB}) @@ -114,15 +119,19 @@ if(TARGET KukaFRIClient OR FRI_Client_SDK_Cpp_FOUND) Tasks v_repLib) - # basis_add_executable(KukaPoseEstimationTest.cpp) - # basis_target_link_libraries(KukaPoseEstimationTest ${Boost_LIBRARIES} ${Boost_REGEX_LIBRARY} ${FRI_Client_SDK_Cpp_LIBRARIES} - # ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} KukaFRIClient ${FLATBUFFERS_STATIC_LIB} -lboost_iostreams -lboost_system -lboost_filesystem - # mc_rbdyn_urdf - # SpaceVecAlg #TODO: re-enable when exported targets are correct again - # sch-core - # RBDyn - # Tasks) - endif() + basis_add_executable(KinematicsCalibration_test.cpp) + basis_target_link_libraries(KinematicsCalibration_test + ${Boost_LIBRARIES} ${Boost_REGEX_LIBRARY} ${FRI_Client_SDK_Cpp_LIBRARIES} + ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} ${FLATBUFFERS_STATIC_LIB} + -lboost_iostreams -lboost_system -lboost_filesystem + -ldl + mc_rbdyn_urdf + SpaceVecAlg #TODO: re-enable when exported targets are correct again + sch-core + RBDyn + Tasks + v_repLib) +endif() if(CERES_FOUND OR USE_INTERNAL_CERES) diff --git a/test/KinematicsCalibration_test.cpp b/test/KinematicsCalibration_test.cpp new file mode 100644 index 0000000..729995a --- /dev/null +++ b/test/KinematicsCalibration_test.cpp @@ -0,0 +1,167 @@ +#define BOOST_TEST_DYN_LINK +#define BOOST_TEST_MODULE KinematicsCalibration_test +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +// SpaceVecAlg +// https://github.com/jrl-umi3218/SpaceVecAlg +#include + + +// RBDyn +// https://github.com/jrl-umi3218/RBDyn +#include +#include +#include +#include +#include +#include + +// mc_rbdyn_urdf +// https://github.com/jrl-umi3218/mc_rbdyn_urdf +#include + +//// local includes +// #include "grl/vrep/KukaLBRiiwaVrepPlugin.hpp" + +#include "grl/flatbuffer/FlatbufferToEigen.hpp" +#include "grl/vrep/SpaceVecAlg.hpp" + + +#include "kukaiiwaURDF.h" + +const double degreesToRadians = M_PI/180.0; +const double radiansTodegrees = 180/M_PI; + +BOOST_AUTO_TEST_SUITE(KinematicsCalibration_test) + + +namespace cst = boost::math::constants; +auto strRobot = mc_rbdyn_urdf::rbdyn_from_urdf(XYZSarmUrdf); +rbd::MultiBody mb = strRobot.mb; +rbd::MultiBodyConfig mbc(mb); +rbd::MultiBodyGraph mbg(strRobot.mbg); +std::size_t nrJoints = mb.nrJoints(); +std::size_t nrBodies = strRobot.mb.nrBodies(); + + +BOOST_AUTO_TEST_CASE(ForwardKinematics) +{ + Eigen::MatrixXd testJointAngles(5,7); // Degree + testJointAngles << -0.06, -0.03, 0.01, -0.06, -0.04, -0.02, 0.1, + -0.06, 90.09, 0.01, -0.07, -0.04, -0.01, 0.1, + -0.06, -0.06, 90.06, -0.06, -0.04, -0.02, 0.1, + 90.03, -0.06, 0.06, -0.06, -0.04, -0.02, 0.1, + 0.05, -0.06, 0.06, -91.87, 0, 45.48, 0.1; + + testJointAngles = degreesToRadians*testJointAngles; + + Eigen::MatrixXd cartesianPositions(5,6); // Degree and mm + cartesianPositions << 0.02, 0, 1306, 0, 0.01, 0, + 946, -1.04, 357.97, 154.15, 89.84, 154.22, + -1, 0.5, 1306, 90.06, 0.04, -0.06, + 0, -0.5, 1306, 90.14, -0.02, 0.06, + 484.81, 0.85, 674.72, 179.97, 42.7, 179.91; + + std::size_t testSize = testJointAngles.rows(); + std::size_t jointNum = testJointAngles.cols(); + std::size_t poseDim = cartesianPositions.cols(); + std::vector jointNames; + std::vector EEpose; + + for(int testIdx=0; testIdx 0) { + mbc.q[jointIndex][0] = oneStateJointPosition[jointIdx]; + jointIdx++; + } + std::cout << "Rotation: " << std::endl << transfroms[jointIndex].rotation() << std::endl; + std::cout << "Translation: " << std::endl << transfroms[jointIndex].translation() << std::endl; + } + rbd::forwardKinematics(mb, mbc); + // Pose of the end effector relative to robot base frame. + // body_size-1: end-effector with marker + // body_size-2: Flange, here this should be used. + EEpose.push_back(mbc.bodyPosW[nrBodies-2]); + + + for(int i=0; i(0,0) = 1000*pluckerpose.block<5,3>(0,0); + pluckerpose.block<5,3>(0,3) = radiansTodegrees*pluckerpose.block<5,3>(0,3); + std::cout << pluckerpose << std::endl; + std::cout << "---------------------------------" << std::endl; + + +///////////////////////////////////////////////////////////////////////////////////////// +cartesianPositions.block<5,3>(0,3) = degreesToRadians*cartesianPositions.block<5,3>(0,3); +std::vector PTransform; +for(int testIdx=0; testIdx + + + + + + + + + + diff --git a/test/readFlatbufferTest.cpp b/test/readFlatbufferTest.cpp index 6b7ace0..e95a5b7 100644 --- a/test/readFlatbufferTest.cpp +++ b/test/readFlatbufferTest.cpp @@ -22,12 +22,15 @@ /// split -C 200m --numeric-suffixes 2018_02_28_16_39_13_FusionTrack.json 2018_02_28_16_39_13_FusionTrack +/// The command to get the json file from flatbuffer binary file, these two files should be located in the same folder. +/// flatc -I . --json LogKUKAiiwaFusionTrack.fbs -- 2018_03_14_22_06_29_FusionTrack.flik + int main(int argc, char* argv[]) { /// Define the file names std::string foldname = "/home/chunting/src/V-REP_PRO_EDU_V3_4_0_Linux/"; - std::string kukaTimeStamp = "2018_03_05_10_26_21_Kukaiiwa"; - std::string FTTimeStamp = "2018_03_05_10_26_40_FusionTrack"; + std::string kukaTimeStamp = "2018_03_14_22_06_39_Kukaiiwa"; + std::string FTTimeStamp = "2018_03_14_22_06_29_FusionTrack"; std::string kukaBinaryfile = foldname + kukaTimeStamp+ ".iiwa"; std::string fusiontrackBinaryfile = foldname + FTTimeStamp +".flik"; std::string foldtimestamp = current_date_and_time_string(); @@ -44,6 +47,7 @@ int main(int argc, char* argv[]) std::string FT_TimeEvent_CSV = foldname + foldtimestamp + "/FT_TimeEvent.csv"; std::string FT_Marker22_CSV = foldname + foldtimestamp + "/FT_Pose_Marker22.csv"; std::string FT_Marker55_CSV = foldname + foldtimestamp + "/FT_Pose_Marker55.csv"; + std::string FT_Marker50000_CSV = foldname + foldtimestamp + "/FT_Pose_Marker50000.csv"; std::string FTKUKA_TimeEvent_CSV = foldname + foldtimestamp + "/FTKUKA_TimeEvent.csv"; @@ -58,6 +62,7 @@ int main(int argc, char* argv[]) uint32_t markerID_22 = 22; uint32_t markerID_55 = 55; + uint32_t markerID_50000 = 50000; timeEventM_FT = grl::MatrixXd::Zero(FT_size, timeEventM_FT.cols()); Eigen::MatrixXd markerPose(FT_size, grl::col_Pose); // Get the pose of the marker 22. @@ -83,7 +88,17 @@ int main(int argc, char* argv[]) grl::regularizeTimeEvent(timeEventM_FT); grl::writeMatrixToCSV(FT_Marker55_CSV, FT_Labels_Pose, timeEventM_FT, markerPose); } + // Change it back to the original size + timeEventM_FT.resize(FT_size, grl::col_timeEvent); + markerPose.resize(FT_size, grl::col_Pose); + // Get the pose of the bone marker + int validsize_50000 = grl::getMarkerPose(logKUKAiiwaFusionTrackP, markerID_50000, timeEventM_FT, markerPose); + std::cout<<"validsize_50000: " << validsize_50000 << std::endl; + if(validsize_50000>2) { + grl::regularizeTimeEvent(timeEventM_FT); + grl::writeMatrixToCSV(FT_Marker50000_CSV, FT_Labels_Pose, timeEventM_FT, markerPose); + } ///////////////////////////////////////////////////////////////////////////////////////////////////// /// Write KUKA data to CSV //////////////////////////////////////////////////////////////////////////////////////////////////// From 288dc23c82827d1b21a747217b718a1fd4eb1f17 Mon Sep 17 00:00:00 2001 From: Chunting Date: Wed, 21 Mar 2018 02:11:23 -0400 Subject: [PATCH 059/102] Replay --- include/grl/flatbuffer/FlatbufferToEigen.hpp | 37 +-- .../grl/flatbuffer/ParseflatbuffertoCSV.hpp | 179 ++++++++++++- .../grl/vrep/HandEyeCalibrationVrepPlugin.hpp | 44 ++- .../grl/vrep/InverseKinematicsVrepPlugin.hpp | 250 ++++++++++++++++-- src/lua/robone.lua | 18 +- .../CMakeLists.txt | 25 +- .../CMakeLists.txt | 29 +- .../camodocal/HandEyeCalibration.cpp | 9 +- test/KukaLBRiiwaVrepPluginTest.cpp | 52 ++-- test/readFlatbufferTest.cpp | 20 +- 10 files changed, 552 insertions(+), 111 deletions(-) diff --git a/include/grl/flatbuffer/FlatbufferToEigen.hpp b/include/grl/flatbuffer/FlatbufferToEigen.hpp index 203a50a..9dee7bb 100644 --- a/include/grl/flatbuffer/FlatbufferToEigen.hpp +++ b/include/grl/flatbuffer/FlatbufferToEigen.hpp @@ -4,23 +4,11 @@ #define FLATBUFFERS_DEBUG_VERIFICATION_FAILURE // // grl -/// Can't include the vrep header files. -// #include "grl/vrep/Vrep.hpp" -// #include "grl/vrep/VrepRobotArmDriver.hpp" -// #include "grl/vrep/VrepRobotArmJacobian.hpp" - -// #include "grl/vrep/Eigen.hpp" -// #include "grl/vrep/Vrep.hpp" -// #include "camodocal/calib/HandEyeCalibration.h" - -// #include "v_repLib.h" -// FusionTrack Libraries - #include #include #include "camodocal/EigenUtils.h" - - +#include +#include "grl/vrep/SpaceVecAlg.hpp" namespace grl { @@ -65,7 +53,26 @@ namespace grl { pose << r.transpose(), eulerAngleEigen.transpose(); PKPose.row(i) = pose; } - return PKPose; + return std::move(PKPose); + } + + std::vector getPTransform(Eigen::MatrixXd& PKPose ) { + std::size_t pose_size = PKPose.size(); + std::vector PTPose; + for(int i=0; i +// #include #include "grl/vector_ostream.hpp" #include "flatbuffers/flatbuffers.h" @@ -74,7 +74,10 @@ #include #include "kukaiiwaURDF.h" - +#include +#include +#include +#include namespace grl { @@ -99,6 +102,8 @@ namespace grl { enum LabelsType { FT_Pose = 0, Joint = 1, Kuka_Pose= 2}; + + /// Get CSV labels /// @param label indicate the type of the label. std::vector getLabels( LabelsType label){ @@ -134,6 +139,12 @@ namespace grl { return true; } + int getStateSize(const fbs_tk::Root &logKUKAiiwaFusionTrackP) { + auto states = logKUKAiiwaFusionTrackP->states(); + return states->size(); + + } + /// Return the original timeEvent of fusiontracker /// @param TlogKUKAiiwaFusionTrackP pointer of the root object for fusiontracker. /// @param fusiontracker_tag tag dispatch @@ -239,8 +250,9 @@ namespace grl { // The first columne is counter int row = 0; // Eigen::MatrixXd markerPose(state_size, grl::col_Pose); + // std::cout <<"State size: " << state_size << " markerPose rows: " << markerPose.rows() << std::endl; int BadCount = 0; - for(int i = 0; iGet(i); auto FT_Message = kukaiiwaFusionTrackMessage->deviceState_as_FusionTrackMessage(); auto FT_Frame = FT_Message->frame(); @@ -253,13 +265,13 @@ namespace grl { auto marker = Makers->Get(markerIndex); auto marker_ID = marker->geometryID(); if(marker_ID == makerID){ - auto timeEvent = kukaiiwaFusionTrackMessage->timeEvent(); timeEventM(row,TimeType::local_receive_time) = timeEvent->local_receive_time(); timeEventM(row,TimeType::local_request_time) = timeEvent->local_request_time(); timeEventM(row,TimeType::device_time) = timeEvent->device_time(); timeEventM(row,TimeType::time_Y) = timeEventM(row,TimeType::device_time) - timeEventM(row,TimeType::local_receive_time); timeEventM(row,TimeType::counterIdx) = counter; + auto Pose = marker->transform(); auto FB_r = Pose->position(); auto FB_q = Pose->orientation(); @@ -267,30 +279,42 @@ namespace grl { Eigen::Vector3d r(FB_r.x(), FB_r.y(), FB_r.z()); Eigen::Quaterniond q(FB_q.w(), FB_q.x(), FB_q.y(), FB_q.z()); Eigen::Matrix3d E = q.normalized().toRotationMatrix(); - Eigen::Vector3d eulerAngleEigen = RadtoDegree*E.eulerAngles(0,1,2); + Eigen::Vector3d eulerAngleEigen = RadtoDegree*E.eulerAngles(0,1,2); // X Y Z Eigen::RowVectorXd pose(grl::col_Pose); pose << r.transpose(), eulerAngleEigen.transpose(); - if(markerFrame == true) { - Eigen::VectorXd onePose(7); - onePose << r, q.w(), q.x(), q.y(), q.z(); - auto markerToCameraTransform = grl::MarkerPoseToAffine3f(onePose); - auto cameraTomarkerTransform = markerToCameraTransform.inverse(); - auto _markerPose = grl::Affine3fToMarkerPose(cameraTomarkerTransform); - pose = grl::getPluckerPose(_markerPose); + // if(markerFrame == true) { + // Eigen::VectorXd onePose(7); + // onePose << r, q.w(), q.x(), q.y(), q.z(); + // auto markerToCameraTransform = grl::MarkerPoseToAffine3f(onePose); + // auto cameraTomarkerTransform = markerToCameraTransform.inverse(); + // auto _markerPose = grl::Affine3fToMarkerPose(cameraTomarkerTransform); + // pose = grl::getPluckerPose(_markerPose); + + // } + + - } markerPose.row(row++) = pose; + + // Once read the specified marker information, skip out of the marker loop. + // It can keep from reading duplicate information. + // Sometimes in the same frame, there exists two markers with the same geometryID + break; } } + } else { + BadCount++; } } if(row < state_size) { markerPose.conservativeResize(row, Eigen::NoChange_t{}); timeEventM.conservativeResize(row, Eigen::NoChange_t{}); } - std::cout <<"State size: " << state_size <<" markerPose size: " << markerPose.rows() << " timeEvent: " << timeEventM.rows() << " makerID: " << makerID <(state_size-markerPose.rows()); + std::cout <<"State size: " << state_size <<" markerPose size: " << markerPose.rows() + << " lossing rate " << diff/state_size << " makerID: " << makerID < m_data; +}; + +std::istream& operator>>(std::istream& str, CSVRow& data) +{ + data.readNextRow(str); + return str; +} + +class CSVIterator +{ + public: + typedef std::input_iterator_tag iterator_category; + typedef CSVRow value_type; + typedef std::size_t difference_type; + typedef CSVRow* pointer; + typedef CSVRow& reference; + + CSVIterator(std::istream& str) :m_str(str.good()?&str:NULL) { ++(*this); } + CSVIterator() :m_str(NULL) {} + + // Pre Increment + CSVIterator& operator++() {if (m_str) { if (!((*m_str) >> m_row)){m_str = NULL;}}return *this;} + // Post increment + CSVIterator operator++(int) {CSVIterator tmp(*this);++(*this);return tmp;} + CSVRow const& operator*() const {return m_row;} + CSVRow const* operator->() const {return &m_row;} + + bool operator==(CSVIterator const& rhs) {return ((this == &rhs) || ((this->m_str == NULL) && (rhs.m_str == NULL)));} + bool operator!=(CSVIterator const& rhs) {return !((*this) == rhs);} + private: + std::istream* m_str; + CSVRow m_row; +}; + + /// Get the joint angles at specific time point (index) + + /// @return jointPosition, Eigen vector which contains joint position of the seven joints. + Eigen::VectorXf getJointAnglesFromCSV(std::string filename, int rowIdx){ + + // std::ifstream file("/home/chunting/src/V-REP_PRO_EDU_V3_4_0_Linux/2018_03_20_20_35_20/KUKA_Joint.csv"); + std::ifstream file(filename); + grl::CSVIterator loop(file); + std::size_t joint_size = 7; + Eigen::VectorXf jointPosition(joint_size); + int row = 0; + + for(grl::CSVIterator loop(file); loop != grl::CSVIterator(); ++loop) + { + + if(row == rowIdx){ + for(int joint_index = 0; joint_indexinfo( "\nrobotTipInRobotBase: \n{}", poseString(robotTipInRobotBase)); - logger_->info( "\nfiducialInOpticalTrackerBase: \n{}", poseString(fiducialInOpticalTrackerBase)); + std::cout << "rvec1: " << std::endl << rvec1 << std::endl; + std::cout << "tvec1: " << std::endl << tvec1 << std::endl; + std::cout << "rvec2: " << std::endl << rvec2 << std::endl; + std::cout << "tvec2: " << std::endl << tvec2 << std::endl; - logger_->info( "\nrobotTipInFirstTipBase: \n{}", poseString(robotTipInFirstTipBase)); - logger_->info( "\nfiducialInFirstFiducialBase: \n{}", poseString(fiducialInFirstFiducialBase)); + logger_->info( "\nrobotTipInRobotBase: \n{}", poseString(robotTipInRobotBase)); + logger_->info( "\nfiducialInOpticalTrackerBase: \n{}", poseString(fiducialInOpticalTrackerBase)); - // print simulation transfrom from tip to fiducial - Eigen::Affine3d RobotTipToFiducial = getObjectTransform(opticalTrackerDetectedObjectName,robotTip); - logger_->info( poseString(RobotTipToFiducial,"expected RobotTipToFiducial (simulation only): ")); + logger_->info( "\nrobotTipInFirstTipBase: \n{}", poseString(robotTipInFirstTipBase)); + logger_->info( "\nfiducialInFirstFiducialBase: \n{}", poseString(fiducialInFirstFiducialBase)); + + // print simulation transfrom from tip to fiducial + Eigen::Affine3d RobotTipToFiducial = getObjectTransform(opticalTrackerDetectedObjectName,robotTip); + logger_->info( poseString(RobotTipToFiducial,"expected RobotTipToFiducial (simulation only): ")); - BOOST_VERIFY(robotTipInFirstTipBase.translation().norm() - fiducialInFirstFiducialBase.translation().norm() < 0.1); + //BOOST_VERIFY(robotTipInFirstTipBase.translation().norm() - fiducialInFirstFiducialBase.translation().norm() < 0.1); } } diff --git a/include/grl/vrep/InverseKinematicsVrepPlugin.hpp b/include/grl/vrep/InverseKinematicsVrepPlugin.hpp index 5e89735..20f5ee3 100644 --- a/include/grl/vrep/InverseKinematicsVrepPlugin.hpp +++ b/include/grl/vrep/InverseKinematicsVrepPlugin.hpp @@ -5,6 +5,30 @@ /// @todo remove IGNORE_ROTATION or make it a runtime configurable parameter // #define IGNORE_ROTATION +#include +#include +#include +#include +#include + +#include "flatbuffers/util.h" +#include "grl/flatbuffer/ParseflatbuffertoCSV.hpp" +#include +#include "grl/flatbuffer/FusionTrack_generated.h" +#include "grl/flatbuffer/Time_generated.h" +#include "grl/flatbuffer/LogKUKAiiwaFusionTrack_generated.h" +#include "grl/time.hpp" + + +#include +#include +#include +#include + +#include +#include +/// Boost to create an empty folder +#include #include @@ -52,6 +76,19 @@ #include + +// FusionTrack Libraries +// #include +// For replay the recorded data +// #include "grl/flatbuffer/ParseflatbuffertoCSV.hpp" +#include + #include + + // mc_rbdyn_urdf +// https://github.com/jrl-umi3218/mc_rbdyn_urdf +#include +#include "grl/flatbuffer/kukaiiwaURDF.h" + namespace grl { namespace vrep { @@ -213,9 +250,9 @@ class InverseKinematicsVrepPlugin ikGroupTargetName_ = (std::get(armDriverSimulatedParams)); robotFlangeTipName_ = (std::get(armDriverSimulatedParams)); - ikGroupBaseHandle_ = grl::vrep::getHandle(ikGroupBaseName_); - ikGroupTipHandle_ = grl::vrep::getHandle(ikGroupTipName_); - ikGroupTargetHandle_ = grl::vrep::getHandle(ikGroupTargetName_); + ikGroupBaseHandle_ = grl::vrep::getHandle(ikGroupBaseName_); // "Robotiiwa" + ikGroupTipHandle_ = grl::vrep::getHandle(ikGroupTipName_); // "RobotMillTip" + ikGroupTargetHandle_ = grl::vrep::getHandle(ikGroupTargetName_); // "RobotMillTipTarget" /// for why this is named as it is see: https://github.com/jrl-umi3218/Tasks/blob/master/tests/arms.h#L34 sva::PTransform X_base = getObjectPTransform(ikGroupBaseHandle_); //X_base = sva::PTransformd(X_base.rotation().inverse(), X_base.translation()); @@ -249,12 +286,15 @@ class InverseKinematicsVrepPlugin boost::copy(linkNames_, std::back_inserter(rbd_bodyNames_)); boost::copy(jointNames_, std::back_inserter(rbd_jointNames_)); /// @todo TODO(ahundt) replace hardcoded joint strings with a vrep tree traversal object that generates an RBDyn MultiBodyGraph - jointNames_.push_back("LBR_iiwa_14_R820_link8"); + + // In this part, it added both joints and links into the jointNames_, why? + // Should we add them seperately? + jointNames_.push_back("LBR_iiwa_14_R820_link8"); // Is this a joint? In vrep it's a link. jointNames_.push_back("cutter_joint"); rbd_jointNames_.push_back("LBR_iiwa_14_R820_link8"); rbd_jointNames_.push_back(robotFlangeTipName_); rbd_jointNames_.push_back("cutter_joint"); - rbd_jointNames_.push_back(ikGroupTipName_); + rbd_jointNames_.push_back(ikGroupTipName_); // "RobotMillTip" rbd_bodyNames_.push_back("cutter_joint"); rbd_bodyNames_.push_back(ikGroupTipName_); @@ -272,7 +312,7 @@ class InverseKinematicsVrepPlugin std::string thisJointName = rbd_jointNames_[i]; rbd::Joint::Type jointType = rbd::Joint::Fixed; /// @todo TODO(ahundt) fix hard coded Revolute vs fixed joint https://github.com/ahundt/grl/issues/114 - if(simGetObjectType(rbd_jointHandles_[i])==sim_object_joint_type) + if(simGetObjectType(rbd_jointHandles_[i]) == sim_object_joint_type) { jointType = rbd::Joint::Rev; } @@ -307,20 +347,24 @@ class InverseKinematicsVrepPlugin rbd_mbg_.addBody(b_i); } - + // Dummy10 in VRep, by default it's at the origin of the world frame. std::string dummyName0(("Dummy"+ boost::lexical_cast(0+10))); int currentDummy0 = simGetObjectHandle(dummyName0.c_str()); Eigen::Affine3d eto0 = getObjectTransform(rbd_jointHandles_[0],-1); - logger_->info("{} \n{}", dummyName0 , eto0.matrix()); + logger_->info("InverseKinematics: \n {} \n{}", dummyName0 , eto0.matrix()); setObjectTransform(currentDummy0,-1,eto0); - std::vector frameHandles; + std::vector frameHandles; // Joint frames frameHandles.push_back(simGetObjectHandle(ikGroupBaseName_.c_str())); boost::copy(rbd_jointHandles_,std::back_inserter(frameHandles)); for(std::size_t i = 0; i < rbd_jointHandles_.size()-1; i++) { // note: Tasks takes transforms in the successor (child link) frame, so it is the inverse of v-rep // thus we are getting the current joint in the frame of the next joint + + // It seems strange that we get the current joint in the frame of the previous one, not the next one? + // to means the transform matrix of joint frameHandles[i+1] relative to frameHandles[i]? + sva::PTransformd to(getObjectPTransform(frameHandles[i+1],frameHandles[i])); // this should be the identity matrix because we set the joints to 0! //sva::PTransformd from(getJointPTransform(rbd_jointHandles_[i+1])); @@ -332,13 +376,24 @@ class InverseKinematicsVrepPlugin std::string prevBody = rbd_bodyNames_[i]; std::string curJoint = rbd_jointNames_[i]; std::string nextBody = rbd_bodyNames_[i+1]; - + /* prevBody: Body 1 name; + to: Transformation from prevBody to curJoint in prevBody coordinate. + nextBody: Body 2 name. + from: Transformation from body 2 to joint in body 2 coordinate. + curJoint: Joint between the two body name. + isB1toB2 = true: If true use the joint forward value as body 1 to body 2 forward value of the joint and the inverse value for body 2 to body 1 joint. If false the behavior is inversed. + // The body coordinate is fixed at one end of the link, whose origin is coincident with the joint coordinate frame. + // Therefore, the to matrix is the transform matrix between two joints and from matrix is Identity(). + + */ rbd_mbg_.linkBodies(prevBody, to, nextBody, from, curJoint); - } + } + // Can't understand this comment, since didn't see the inverse of v-rep? // note: Tasks takes transforms in the successor (child link) frame, so it is the inverse of v-rep + rbd_mbs_.push_back(rbd_mbg_.makeMultiBody(ikGroupBaseName_,isFixed,X_base)); rbd_mbcs_.push_back(rbd::MultiBodyConfig(rbd_mbs_[0])); rbd_mbcs_[0].zero(rbd_mbs_[0]); @@ -352,6 +407,8 @@ class InverseKinematicsVrepPlugin for (std::size_t i = 0; i < jointHandles_.size(); ++i) { simSetJointPosition(jointHandles_[i],initialJointAngles[i]); } + + // Set RBDyn joint angles from vrep joint angles. SetRBDynArmFromVrep(jointNames_,jointHandles_,simArmMultiBody,simArmConfig); rbd::forwardKinematics(simArmMultiBody, simArmConfig); @@ -389,6 +446,7 @@ class InverseKinematicsVrepPlugin auto& simArmConfig = rbd_mbcs_[simulatedRobotIndex]; // Debug output + // Every dummy represents a joint. for (std::size_t i=0 ; i < jointHandles_.size() ; i++) { Eigen::Affine3d eto; @@ -405,9 +463,10 @@ class InverseKinematicsVrepPlugin } bool dummy_world_frame = true; + // The pose of body i is identical with that of joint i? if( dummy_world_frame ) { - // visualize each joint position + // visualize each joint position in world frame sva::PTransform plinkWorld = simArmConfig.bodyPosW[simArmMultiBody.bodyIndexByName(linkNames_[i])]; Eigen::Affine3d linkWorld = PTranformToEigenAffine(plinkWorld); std::string dummyName(("Dummy0"+ boost::lexical_cast(i+1))); @@ -415,6 +474,7 @@ class InverseKinematicsVrepPlugin if(print) logger_->info("{} RBDyn World\n{}",dummyName, linkWorld.matrix()); setObjectTransform(currentDummy,-1,linkWorld); prevDummy=currentDummy; + // Transformation from parent(i) to i in body coordinate (Xj*Xt). sva::PTransform plinkToSon = simArmConfig.parentToSon[simArmMultiBody.bodyIndexByName(linkNames_[i])]; Eigen::Affine3d linkToSon = PTranformToEigenAffine(plinkToSon); if(print) logger_->info("{} RBDyn ParentLinkToSon\n{}",dummyName, linkToSon.matrix()); @@ -422,7 +482,8 @@ class InverseKinematicsVrepPlugin } else { - // visualize each joint position + // visualize each joint position in the previous joint frame + sva::PTransform plinkWorld = simArmConfig.parentToSon[simArmMultiBody.bodyIndexByName(linkNames_[i])]; Eigen::Affine3d linkWorld = PTranformToEigenAffine(plinkWorld); std::string dummyName(("Dummy0"+ boost::lexical_cast(i+1))); @@ -439,6 +500,89 @@ class InverseKinematicsVrepPlugin setObjectTransform(simGetObjectHandle("Dummy"),-1,tipTf); } +/// @brief Will apply the stored estimate to the v-rep simulation value +/// +/// A default transform is saved when construct() was called +/// so if no estimate has been found that will be used. +void applyEstimate(){ + // Dummy10 in VRep, by default it's at the origin of the world frame. + std::string opticalTrackerDetectedObjectName("Fiducial#22"); + std::string robotTipName("RobotFlangeTip"); + int opticalTrackerDetectedObjectHandle = simGetObjectHandle(opticalTrackerDetectedObjectName.c_str()); + int robotTipHandle = simGetObjectHandle(robotTipName.c_str()); + Eigen::Affine3d transformEstimate = getObjectTransform(opticalTrackerDetectedObjectHandle, robotTipHandle); + Eigen::Quaterniond eigenQuat(0.105045, 0.0502289 , 0.654843 , 0.746742); //wxyz + Eigen::Vector3d eigenPos(0.0754499, 0.0975167, 0.0882869); + + + transformEstimate = eigenQuat; + transformEstimate.translation() = eigenPos; + + // set transform between end effector and fiducial + auto transform = getObjectTransform(opticalTrackerDetectedObjectHandle, robotTipHandle); + std::cout<< "Before resetting the hand eye calibration: " << std::endl << transform.matrix() << std::endl; + setObjectTransform(opticalTrackerDetectedObjectHandle, robotTipHandle, transformEstimate); + transform = getObjectTransform(opticalTrackerDetectedObjectHandle, robotTipHandle); + std::cout<< "After resetting the hand eye calibration: " << std::endl << transform.matrix() << std::endl; +// logger_->info( "Hand Eye Screw Estimation has been set quat wxyz\n: {} , {} , {} , {} \n translation xyz: {} {} {}", +// eigenQuat.w(), eigenQuat.x(), eigenQuat.y(),eigenQuat.z(), transformEstimate.translation().x(), transformEstimate.translation().y(), transformEstimate.translation().z()); +} + void getPoseFromCSV(std::string filename){ + // we only have one robot for the moment so the index of it is 0 + // loadFromBinary(); + if(time_index == 0){ + applyEstimate(); + } + const std::size_t simulatedRobotIndex = 0; + auto& simArmMultiBody = rbd_mbs_[simulatedRobotIndex]; + auto& simArmConfig = rbd_mbcs_[simulatedRobotIndex]; + auto joint_angles = grl::getJointAnglesFromCSV(filename, time_index++); + + std::vector vrepJointAngles; + double angle = 0; + int jointIdx=0; + for(auto i : jointHandles_) + { + angle = joint_angles[jointIdx++]; + simSetJointPosition(i,angle); + vrepJointAngles.push_back(angle); + std::cout << angle << std::endl; + } + + // Set RBDyn joint angles from vrep joint angles. + SetRBDynArmFromVrep(jointNames_,jointHandles_,simArmMultiBody,simArmConfig); + + rbd::forwardKinematics(simArmMultiBody, simArmConfig); + rbd::forwardVelocity(simArmMultiBody, simArmConfig); + + debugFrames(); + + } + + void replayMarker(std::string filename, int time_index){ + auto markerPose = grl::getMarkerPoseFromCSV(filename, time_index); + Eigen::Vector3d eigenPos(markerPose[0], markerPose[1], markerPose[2]); + Eigen::Quaternionf eigenQuat; + eigenQuat = Eigen::AngleAxisf(markerPose[3], Eigen::Vector3f::UnitX()) + * Eigen::AngleAxisf(markerPose[4], Eigen::Vector3f::UnitY()) + * Eigen::AngleAxisf(markerPose[5], Eigen::Vector3f::UnitZ()); + + std::string markerPoseFromTracker("Dummy20"); + int markerPoseHandle = simGetObjectHandle(markerPoseFromTracker.c_str()); + + std::string OpticalTrackerBase("OpticalTrackerBase#0"); + int OpticalTrackerBaseHandle = simGetObjectHandle(OpticalTrackerBase.c_str()); + + Eigen::Affine3d transformEstimate = getObjectTransform(markerPoseHandle, OpticalTrackerBaseHandle); + transformEstimate = eigenQuat.cast(); + transformEstimate.translation() = eigenPos; + + // set transform between end effector and fiducial + setObjectTransform(markerPoseHandle, OpticalTrackerBaseHandle, transformEstimate); + logger_->info( "MarkerPose has been set quat wxyz\n: {} , {} , {} , {} \n translation xyz: {} {} {}", + eigenQuat.w(), eigenQuat.x(), eigenQuat.y(),eigenQuat.z(), transformEstimate.translation().x(), transformEstimate.translation().y(), transformEstimate.translation().z()); + + } void testPose(){ @@ -471,7 +615,67 @@ class InverseKinematicsVrepPlugin debugFrames(); } + void testURDFPose(){ + + /// simulation tick time step in float seconds from vrep API call + float simulationTimeStep = simGetSimulationTimeStep(); + + // we only have one robot for the moment so the index of it is 0 + const std::size_t simulatedRobotIndex = 0; + auto& simArmMultiBody = rbd_mbs_[simulatedRobotIndex]; + auto& simArmConfig = rbd_mbcs_[simulatedRobotIndex]; + + std::vector vrepJointAngles; + double angle = 0.7; + for(auto i : jointHandles_) + { + angle *= -1; + simSetJointPosition(i,angle); + vrepJointAngles.push_back(angle); + } + + /// @todo TODO(ahundt) rethink where/when/how to send command for the joint angles. Return to LUA? Set Directly? Distribute via vrep send message command? + //////////////////////////////////////////////////// + // Set joints to current arm position in simulation + SetRBDynArmFromVrep(jointNames_,jointHandles_,simArmMultiBody,simArmConfig); + rbd::forwardKinematics(simArmMultiBody, simArmConfig); + rbd::forwardVelocity(simArmMultiBody, simArmConfig); + + debugFrames(); + } + + // void replay(){ + // loadFromBinary(); + // /// simulation tick time step in float seconds from vrep API call + // float simulationTimeStep = simGetSimulationTimeStep(); + + // // we only have one robot for the moment so the index of it is 0 + // const std::size_t simulatedRobotIndex = 0; + // auto& simArmMultiBody = rbd_mbs_[simulatedRobotIndex]; + // auto& simArmConfig = rbd_mbcs_[simulatedRobotIndex]; + // auto joint_angles = grl::getJointAnglesFromBinary(kukaStatesP_, time_index++); + + // std::vector vrepJointAngles; + // double angle = 0; + // int jointIdx=0; + // for(auto i : jointHandles_) + // { + // angle = joint_angles[jointIdx++]; + // simSetJointPosition(i,angle); + // vrepJointAngles.push_back(angle); + // } + + // /// @todo TODO(ahundt) rethink where/when/how to send command for the joint angles. Return to LUA? Set Directly? Distribute via vrep send message command? + + // //////////////////////////////////////////////////// + // // Set joints to current arm position in simulation + // SetRBDynArmFromVrep(jointNames_,jointHandles_,simArmMultiBody,simArmConfig); + // rbd::forwardKinematics(simArmMultiBody, simArmConfig); + // rbd::forwardVelocity(simArmMultiBody, simArmConfig); + + // debugFrames(); + // } /// Configures updateKinematics with the goal the kinematics should aim for enum class GoalPosE { realGoalPosition, debugGoalPosition }; /// Configures updateKinematics the algorithm the kinematics should use for solving @@ -537,7 +741,7 @@ class InverseKinematicsVrepPlugin /// @todo TODO(ahundt) also copy acceleration velocity, torque, etc - + // Get the current and desired transform in V-Rep const auto& handleParams = VrepRobotArmDriverSimulatedP_->getVrepHandleParams(); Eigen::Affine3d currentEndEffectorPose = getObjectTransform( @@ -687,6 +891,7 @@ class InverseKinematicsVrepPlugin // use basic inverse kinematics to solve for the position //rbd::InverseKinematics ik(simArmMultiBody,simArmMultiBody.jointIndexByName(jointNames_[6])); rbd::InverseKinematics ik(simArmMultiBody,simArmMultiBody.bodyIndexByName(ikGroupTipName_)); + // Update the simArmMultiBody,simArmConfig based on the inverse kinematics. ik.sInverseKinematics(simArmMultiBody,simArmConfig,targetWorldTransform); // update the simulated arm position } @@ -745,9 +950,17 @@ class InverseKinematicsVrepPlugin /// blocking call, call in separate thread, just allocates memory void run_one(){ // If true, run real inverse kinematics algorith, if false go to a test pose. - const bool ik = true; - if(ik) updateKinematics(); - else testPose(); + const bool ik = false; + if(ik) { + updateKinematics(); + } else { + std::string fileName = "/home/chunting/src/V-REP_PRO_EDU_V3_4_0_Linux/2018_03_20_20_35_20/KUKA_Joint.csv"; + // std::cout << "Call Run One..." << std::endl; + // testPose(); + // applyEstimate(); + // getPoseFromCSV(fileName); + replayMarker(fileName, time_index); + } } @@ -758,6 +971,9 @@ class InverseKinematicsVrepPlugin void solve(){ } + /// for the replay purpose. + int time_index = 0; + fbs_tk::Root kukaStatesP_; std::shared_ptr logger_; diff --git a/src/lua/robone.lua b/src/lua/robone.lua index f6ba601..6cbfca9 100644 --- a/src/lua/robone.lua +++ b/src/lua/robone.lua @@ -265,7 +265,7 @@ robone.cutBoneScript=function() end end - target=simGetObjectHandle('RobotMillTipTarget') + target = simGetObjectHandle('RobotMillTipTarget') targetBase=simGetObjectHandle('Robotiiwa') bone=simGetObjectHandle('FemurBone') table=simGetObjectHandle('highTable') @@ -348,13 +348,13 @@ robone.handEyeCalibScript=function() maxJerk={jerk,jerk,jerk,jerk*DtoR} targetVel={targetV,targetV,targetV,targetV*DtoR} - -- target=simGetObjectHandle('RobotMillTipTarget') - target=simGetObjectHandle('RobotFlangeTipTarget') + target=simGetObjectHandle('RobotMillTipTarget') + -- target=simGetObjectHandle('RobotFlangeTipTarget') targetBase=simGetObjectHandle('Robotiiwa') path=simGetObjectHandle('HandEyeCalibPath') circleCalib = simGetObjectHandle('CircleCalibPath') endeffectorTarget=simGetObjectHandle('RobotMillTipTarget') - numSteps=36 + numSteps=16 startP,startO=grl.getTransformBetweenHandles(target,targetBase) @@ -370,16 +370,18 @@ robone.handEyeCalibScript=function() simDisplayDialog('Error','HandEyeCalibration plugin was not found. (v_repExtHandEyeCalibration.dll)&&nSimulation will not run correctly',sim_dlgstyle_ok,true,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1}) else + simWait(30.0) -- Run the hand eye calibration -- simExtHandEyeCalibStart() -- call handEyeCalibrationPG->construct(); - simExtHandEyeCalibStart('Robotiiwa' , 'RobotFlangeTipTarget', 'OpticalTrackerBase', 'Fiducial#22') + simExtHandEyeCalibStart('Robotiiwa' , 'RobotMillTipTarget', 'OpticalTrackerBase', 'Fiducial#22') -- Fill out the vectors, and then pass these vectors to + for i=0,1,1/numSteps do p,o=grl.getPathPointInWorldFrame(path,i) simRMLMoveToPosition(target,-1,-1,currentVel,currentAccel,maxVel,maxAccel,maxJerk,p,o,nil) - simWait(0.5) + simWait(0.2) --0.5 simExtHandEyeCalibAddFrame() simWait(0.25) end @@ -564,6 +566,7 @@ robone.configureOpticalTracker=function() '50000' -- GeometryID ) + end @@ -575,11 +578,14 @@ robone.configureOpticalTracker=function() simAddStatusbarMessage('robone.configureOpticalTracker() + v_repExtAtracsysFusionTrackVrepPlugin: Moving bone marker position relative to the optical tracker.') simExtAtracsysFusionTrackClearObjects() -- The bone should move (bone is attached to Fiducial #55) + simExtAtracsysFusionTrackAddObject('Fiducial#55', -- ObjectToMove 'OpticalTrackerBase#0', -- FrameInWhichToMoveObject 'Fiducial#55', -- ObjectBeingMeasured '55' -- GeometryID ) + + robothandle = simGetObjectHandle ('LBR_iiwa_14_R820#0') --trankerhandle = simGetObjectHandle ('OpticalTrackerBase#0') robotToWorldM = simGetObjectMatrix(robothandle, -1) diff --git a/src/v_repExtGrlCisstInverseKinematics/CMakeLists.txt b/src/v_repExtGrlCisstInverseKinematics/CMakeLists.txt index c72394b..d58f102 100644 --- a/src/v_repExtGrlCisstInverseKinematics/CMakeLists.txt +++ b/src/v_repExtGrlCisstInverseKinematics/CMakeLists.txt @@ -32,20 +32,22 @@ #TODO: add cisst-saw found flag and make it work correctly for independent grl https://github.com/jhu-cisst/cisst-saw if(cisst_cisstNumerical_FOUND AND CISSTNETLIB_FOUND AND sawConstraintController_FOUND AND Boost_FOUND) - #TODO: #WARNING: INSTALLER WILL BE BROKEN UNLESS THIS IS CHANGED TO USE basis_target_link_libraries, etc. + #TODO: #WARNING: INSTALLER WILL BE BROKEN UNLESS THIS IS CHANGED TO USE basis_target_link_libraries, etc. #@see https://github.com/schuhschuh/cmake-basis/issues/442 basis_include_directories( - ${PROJECT_INCLUDE_DIR}/thirdparty/vrep/include - ${CISSTNETLIB_INCLUDE_DIRS} - ${CISST_INCLUDE_DIR} + ${PROJECT_INCLUDE_DIR}/thirdparty/vrep/include + ${CISSTNETLIB_INCLUDE_DIRS} + ${CISST_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIR} ${sawConstraintController_INCLUDE_DIR} + ${FLATBUFFERS_INCLUDE_DIRS} ## remove it after calibration debug ) # ${CISST_SAW_INCLUDE_DIRS} basis_add_library(v_repExtGrlCisstInverseKinematics SHARED v_repExtGrlInverseKinematics.cpp ) - basis_target_link_libraries(v_repExtGrlCisstInverseKinematics - ${Boost_LIBRARIES} + basis_target_link_libraries(v_repExtGrlCisstInverseKinematics + ${Boost_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} + ${Boost_REGEX_LIBRARY} # ${cisstNetlib_LIBRARY_cisstNetlib} # ${cisstNetlib_LIBRARY_cisstNetlib_gcc} # ${cisstNetlib_LIBRARY_cisstNetlib_blas} @@ -57,6 +59,17 @@ if(cisst_cisstNumerical_FOUND AND CISSTNETLIB_FOUND AND sawConstraintController_ #${CISSTNETLIB_LIBRARIES} ${sawConstraintController_LIBRARIES} sawConstraintController + ${Nanopb_LIBRARIES} + ${CMAKE_THREAD_LIBS_INIT} + ${FLATBUFFERS_STATIC_LIB} + -lboost_iostreams -lboost_system -lboost_filesystem + -ldl + KukaFRIClient + mc_rbdyn_urdf + SpaceVecAlg #TODO: re-enable when exported targets are correct again + sch-core + RBDyn + Tasks ## remove it after calibration work v_repLib ) endif() diff --git a/src/v_repExtGrlInverseKinematics/CMakeLists.txt b/src/v_repExtGrlInverseKinematics/CMakeLists.txt index 7870e9c..719bb75 100644 --- a/src/v_repExtGrlInverseKinematics/CMakeLists.txt +++ b/src/v_repExtGrlInverseKinematics/CMakeLists.txt @@ -40,26 +40,35 @@ if( SpaceVecAlg_FOUND AND sch-core_FOUND AND RBDyn_FOUND AND Tasks_FOUND AND Boo message(STATUS v_repExtGrlInverseKinematics BUILDING!!!) # TODO(ahundt) link_directories is a hack, remove me! link_directories(/usr/local/lib) - #TODO: #WARNING: INSTALLER WILL BE BROKEN UNLESS THIS IS CHANGED TO USE basis_target_link_libraries, etc. + #TODO: #WARNING: INSTALLER WILL BE BROKEN UNLESS THIS IS CHANGED TO USE basis_target_link_libraries, etc. #@see https://github.com/schuhschuh/cmake-basis/issues/442 basis_include_directories( - ${PROJECT_INCLUDE_DIR}/thirdparty/vrep/include + ${PROJECT_INCLUDE_DIR}/thirdparty/vrep/include ${EIGEN3_INCLUDE_DIR} ${SpaceVecAlg_INCLUDE_DIR} + ${FLATBUFFERS_INCLUDE_DIRS} # TODO(ahundt): include directories from Tasks & its deps #$ #$ # /include/mylib ) # ${CISST_SAW_INCLUDE_DIRS} basis_add_library(v_repExtGrlInverseKinematics SHARED v_repExtGrlInverseKinematics.cpp ) - basis_target_link_libraries(v_repExtGrlInverseKinematics - ${Boost_LIBRARIES} - ${CMAKE_THREAD_LIBS_INIT} - SpaceVecAlg #TODO: re-enable when exported targets are correct again - sch-core - RBDyn - Tasks - v_repLib + basis_target_link_libraries(v_repExtGrlInverseKinematics + ${Boost_LIBRARIES} + ${Boost_REGEX_LIBRARY} + ${FRI_Client_SDK_Cpp_LIBRARIES} + ${Nanopb_LIBRARIES} + ${CMAKE_THREAD_LIBS_INIT} + ${FLATBUFFERS_STATIC_LIB} + -lboost_iostreams -lboost_system -lboost_filesystem + -ldl + KukaFRIClient + mc_rbdyn_urdf + SpaceVecAlg #TODO: re-enable when exported targets are correct again + sch-core + RBDyn + Tasks + v_repLib ) endif() diff --git a/src/v_repExtHandEyeCalibration/camodocal/HandEyeCalibration.cpp b/src/v_repExtHandEyeCalibration/camodocal/HandEyeCalibration.cpp index 52701ef..1591b94 100644 --- a/src/v_repExtHandEyeCalibration/camodocal/HandEyeCalibration.cpp +++ b/src/v_repExtHandEyeCalibration/camodocal/HandEyeCalibration.cpp @@ -130,6 +130,9 @@ HandEyeCalibration::estimateHandEyeScrew(const std::vector + #include #include #include #include - +#include //// local includes #include "grl/vrep/KukaLBRiiwaVrepPlugin.hpp" BOOST_AUTO_TEST_SUITE(KukaLBRiiwaVrepPluginTest) BOOST_AUTO_TEST_CASE(initialization){ -auto plugin = std::make_shared(); -//BOOST_CHECK_THROW (std::make_shared(),boost::exception); -// plugin->construct() // shouldn't work, because vrep isn't around to get handles -//// uncomment to test error output -// try { -// BOOST_LOG_TRIVIAL(info) << "Starting KUKA LBR iiwa plugin connection to Kuka iiwa\n"; -// auto kukaVrepPluginPG = std::make_shared(); -// kukaVrepPluginPG->construct(); -// } catch (boost::exception& e){ -// // log the error and print it to the screen, don't release the exception -// BOOST_LOG_TRIVIAL(error) << boost::diagnostic_information(e); -// } + auto plugin = std::make_shared(); + BOOST_CHECK_THROW (std::make_shared(),boost::exception); + plugin->construct(); // shouldn't work, because vrep isn't around to get handles + // uncomment to test error output + try { + // BOOST_LOG_TRIVIAL(info) << "Starting KUKA LBR iiwa plugin connection to Kuka iiwa\n"; + auto kukaVrepPluginPG = std::make_shared(); + kukaVrepPluginPG->construct(); + } catch (boost::exception& e){ + // log the error and print it to the screen, don't release the exception + // BOOST_LOG_TRIVIAL(error) << boost::diagnostic_information(e); + } } BOOST_AUTO_TEST_CASE(connectToFake){ - std::vector jointHandles; + std::vector jointNames; - jointHandles.push_back("LBR_iiwa_14_R820_joint1"); // Joint1Handle, - jointHandles.push_back("LBR_iiwa_14_R820_joint2"); // Joint2Handle, - jointHandles.push_back("LBR_iiwa_14_R820_joint3"); // Joint3Handle, - jointHandles.push_back("LBR_iiwa_14_R820_joint4"); // Joint4Handle, - jointHandles.push_back("LBR_iiwa_14_R820_joint5"); // Joint5Handle, - jointHandles.push_back("LBR_iiwa_14_R820_joint6"); // Joint6Handle, - jointHandles.push_back("LBR_iiwa_14_R820_joint7"); // Joint7Handle, + jointNames.push_back("LBR_iiwa_14_R820_joint1"); // Joint1Handle, + jointNames.push_back("LBR_iiwa_14_R820_joint2"); // Joint2Handle, + jointNames.push_back("LBR_iiwa_14_R820_joint3"); // Joint3Handle, + jointNames.push_back("LBR_iiwa_14_R820_joint4"); // Joint4Handle, + jointNames.push_back("LBR_iiwa_14_R820_joint5"); // Joint5Handle, + jointNames.push_back("LBR_iiwa_14_R820_joint6"); // Joint6Handle, + jointNames.push_back("LBR_iiwa_14_R820_joint7"); // Joint7Handle, auto config = std::make_tuple( - jointHandles , // JointHandles, + jointNames , // JointHandles, "RobotFlangeTip" , // RobotFlangeTipHandle, "RobotMillTip" , // RobotTipHandle, "RobotMillTipTarget" , // RobotTargetHandle, @@ -58,7 +59,12 @@ BOOST_AUTO_TEST_CASE(connectToFake){ "FRI" , // KukaMonitorMode (options are FRI, JAVA) "IK_Group1_iiwa" // IKGroupName ); -auto plugin = std::make_shared(config); + auto plugin = std::make_shared(config); + std::shared_ptr VrepRobotArmDriverSimulatedP_; + // Get the arm that will be used to generate simulated results to command robot + // the "base" of this ik is Robotiiwa + VrepRobotArmDriverSimulatedP_ = std::make_shared(); + VrepRobotArmDriverSimulatedP_->construct(); } BOOST_AUTO_TEST_SUITE_END() diff --git a/test/readFlatbufferTest.cpp b/test/readFlatbufferTest.cpp index e95a5b7..f3558f9 100644 --- a/test/readFlatbufferTest.cpp +++ b/test/readFlatbufferTest.cpp @@ -29,8 +29,8 @@ int main(int argc, char* argv[]) { /// Define the file names std::string foldname = "/home/chunting/src/V-REP_PRO_EDU_V3_4_0_Linux/"; - std::string kukaTimeStamp = "2018_03_14_22_06_39_Kukaiiwa"; - std::string FTTimeStamp = "2018_03_14_22_06_29_FusionTrack"; + std::string kukaTimeStamp = "2018_03_20_19_36_47_Kukaiiwa"; + std::string FTTimeStamp = "2018_03_20_19_37_08_FusionTrack"; std::string kukaBinaryfile = foldname + kukaTimeStamp+ ".iiwa"; std::string fusiontrackBinaryfile = foldname + FTTimeStamp +".flik"; std::string foldtimestamp = current_date_and_time_string(); @@ -51,6 +51,15 @@ int main(int argc, char* argv[]) std::string FTKUKA_TimeEvent_CSV = foldname + foldtimestamp + "/FTKUKA_TimeEvent.csv"; + auto strRobot = grl::getURDFModel(); + rbd::MultiBody mb = strRobot.mb; + rbd::MultiBodyConfig mbc(mb); + rbd::MultiBodyGraph mbg(strRobot.mbg); + std::size_t nrJoints = mb.nrJoints(); + std::size_t nrBodies = strRobot.mb.nrBodies(); + std::vector jointNames; + std::cout<<"Joint Size: "<< nrJoints << std::endl; + // Put all the data into the same coordinate system --- Marker frame bool inMarkerFrame = false; // Indicate the marker pose is in marker frame or tracker frame. /// Write FT data to CSV @@ -58,7 +67,7 @@ int main(int argc, char* argv[]) grl::MatrixXd timeEventM_FT = grl::getTimeStamp(logKUKAiiwaFusionTrackP, grl::fusiontracker_tag()); grl::regularizeTimeEvent(timeEventM_FT); - std::size_t FT_size = timeEventM_FT.rows(); + std::size_t FT_size = grl::getStateSize(logKUKAiiwaFusionTrackP); uint32_t markerID_22 = 22; uint32_t markerID_55 = 55; @@ -137,10 +146,13 @@ int main(int argc, char* argv[]) // PKPose = Eigen::MatrixXd::Zero(PKPose.rows(), PKPose.cols()); // PKPose = grl::getPluckerPose(Fudicial_Robot_Pose); // grl::writeMatrixToCSV(FudicialToRobotPose_CSV, PK_Pose_Labels, timeEventM_Kuka, PKPose); - } + + + + // grl::writeFTKUKATimeEventToCSV(FTKUKA_TimeEvent_CSV, logKUKAiiwaFusionTrackP, kukaStatesP); return 0; } From c24e58c187bc2e58c1d0b0610d12f4fc3bf348d2 Mon Sep 17 00:00:00 2001 From: Andrew Hundt Date: Fri, 23 Mar 2018 15:45:04 -0400 Subject: [PATCH 060/102] cmake and compile build/config fixes --- BasisProject.cmake | 2 +- src/v_repExtGrlCisstInverseKinematics/CMakeLists.txt | 1 - src/v_repExtGrlInverseKinematics/CMakeLists.txt | 1 - test/CMakeLists.txt | 6 ++---- 4 files changed, 3 insertions(+), 7 deletions(-) diff --git a/BasisProject.cmake b/BasisProject.cmake index 5abac38..4ebafbf 100644 --- a/BasisProject.cmake +++ b/BasisProject.cmake @@ -104,7 +104,7 @@ basis_project ( # dependencies DEPENDS FlatBuffers # google flatbuffers https://github.com/google/flatbuffers - Boost{program_options,filesystem,unit_test_framework,system,regex,coroutine,chrono} + Boost{program_options,filesystem,unit_test_framework,system,regex,coroutine,chrono,iostreams} # OPTIONAL_DEPENDS # diff --git a/src/v_repExtGrlCisstInverseKinematics/CMakeLists.txt b/src/v_repExtGrlCisstInverseKinematics/CMakeLists.txt index d58f102..74c8c38 100644 --- a/src/v_repExtGrlCisstInverseKinematics/CMakeLists.txt +++ b/src/v_repExtGrlCisstInverseKinematics/CMakeLists.txt @@ -62,7 +62,6 @@ if(cisst_cisstNumerical_FOUND AND CISSTNETLIB_FOUND AND sawConstraintController_ ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} ${FLATBUFFERS_STATIC_LIB} - -lboost_iostreams -lboost_system -lboost_filesystem -ldl KukaFRIClient mc_rbdyn_urdf diff --git a/src/v_repExtGrlInverseKinematics/CMakeLists.txt b/src/v_repExtGrlInverseKinematics/CMakeLists.txt index 719bb75..c4d9a45 100644 --- a/src/v_repExtGrlInverseKinematics/CMakeLists.txt +++ b/src/v_repExtGrlInverseKinematics/CMakeLists.txt @@ -60,7 +60,6 @@ message(STATUS v_repExtGrlInverseKinematics BUILDING!!!) ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} ${FLATBUFFERS_STATIC_LIB} - -lboost_iostreams -lboost_system -lboost_filesystem -ldl KukaFRIClient mc_rbdyn_urdf diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index a050418..8180e0c 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -109,8 +109,7 @@ if(TARGET KukaFRIClient OR FRI_Client_SDK_Cpp_FOUND) ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} ${FLATBUFFERS_STATIC_LIB} - -lboost_iostreams -lboost_system -lboost_filesystem - -ldl + ${LINUX_ONLY_LIBS} KukaFRIClient mc_rbdyn_urdf SpaceVecAlg #TODO: re-enable when exported targets are correct again @@ -123,8 +122,7 @@ if(TARGET KukaFRIClient OR FRI_Client_SDK_Cpp_FOUND) basis_target_link_libraries(KinematicsCalibration_test ${Boost_LIBRARIES} ${Boost_REGEX_LIBRARY} ${FRI_Client_SDK_Cpp_LIBRARIES} ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} ${FLATBUFFERS_STATIC_LIB} - -lboost_iostreams -lboost_system -lboost_filesystem - -ldl + ${LINUX_ONLY_LIBS} mc_rbdyn_urdf SpaceVecAlg #TODO: re-enable when exported targets are correct again sch-core From 596bafb4600b83b960b3c2065ca0c672e200719e Mon Sep 17 00:00:00 2001 From: Andrew Hundt Date: Fri, 23 Mar 2018 18:38:34 -0400 Subject: [PATCH 061/102] KukaJAVAdriver.hpp namespace fix --- include/grl/kuka/KukaJAVAdriver.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/grl/kuka/KukaJAVAdriver.hpp b/include/grl/kuka/KukaJAVAdriver.hpp index 5682ca2..6472d78 100644 --- a/include/grl/kuka/KukaJAVAdriver.hpp +++ b/include/grl/kuka/KukaJAVAdriver.hpp @@ -188,7 +188,7 @@ class KukaJAVAdriver : public std::enable_shared_from_this { /// @todo TODO(ahundt) Need to switch back to an appropriate exception rather than exiting so VREP isn't taken down. /// @todo TODO(ahundt) switch from linux socket to boost::asio::ip::udp::socket, see Kuka.hpp and KukaFRIdriver.hpp for examples, and make use of KukaUDP class. /// After creation of the socket, bind function binds the socket to the address and port number specified in local_sockaddr(custom data structure). - if (bind(socket_local, (struct sockaddr *)&local_sockaddr, sizeof(local_sockaddr)) < 0) { + if (::bind(socket_local, (struct sockaddr *)&local_sockaddr, sizeof(local_sockaddr)) < 0) { printf("Error binding sr_joint!\n"); BOOST_THROW_EXCEPTION(std::runtime_error("KukaJAVAdriver Error opening socket. Check that the port is available, and that all the cables are connected tightly. If you have no other options try restarting your computer.")); } From dbfc875de19a513651041f544d9181945f11279b Mon Sep 17 00:00:00 2001 From: Andrew Hundt Date: Fri, 23 Mar 2018 18:39:01 -0400 Subject: [PATCH 062/102] cmake build fixes --- .../CMakeLists.txt | 2 +- src/v_repExtGrlInverseKinematics/CMakeLists.txt | 2 +- src/v_repExtKukaLBRiiwaPlugin/CMakeLists.txt | 15 ++++++++------- 3 files changed, 10 insertions(+), 9 deletions(-) diff --git a/src/v_repExtGrlCisstInverseKinematics/CMakeLists.txt b/src/v_repExtGrlCisstInverseKinematics/CMakeLists.txt index 74c8c38..653c33e 100644 --- a/src/v_repExtGrlCisstInverseKinematics/CMakeLists.txt +++ b/src/v_repExtGrlCisstInverseKinematics/CMakeLists.txt @@ -62,7 +62,7 @@ if(cisst_cisstNumerical_FOUND AND CISSTNETLIB_FOUND AND sawConstraintController_ ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} ${FLATBUFFERS_STATIC_LIB} - -ldl + ${LINUX_ONLY_LIBS} KukaFRIClient mc_rbdyn_urdf SpaceVecAlg #TODO: re-enable when exported targets are correct again diff --git a/src/v_repExtGrlInverseKinematics/CMakeLists.txt b/src/v_repExtGrlInverseKinematics/CMakeLists.txt index c4d9a45..c3beffa 100644 --- a/src/v_repExtGrlInverseKinematics/CMakeLists.txt +++ b/src/v_repExtGrlInverseKinematics/CMakeLists.txt @@ -60,7 +60,7 @@ message(STATUS v_repExtGrlInverseKinematics BUILDING!!!) ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} ${FLATBUFFERS_STATIC_LIB} - -ldl + ${LINUX_ONLY_LIBS} KukaFRIClient mc_rbdyn_urdf SpaceVecAlg #TODO: re-enable when exported targets are correct again diff --git a/src/v_repExtKukaLBRiiwaPlugin/CMakeLists.txt b/src/v_repExtKukaLBRiiwaPlugin/CMakeLists.txt index 72295bd..bc4c6e9 100644 --- a/src/v_repExtKukaLBRiiwaPlugin/CMakeLists.txt +++ b/src/v_repExtKukaLBRiiwaPlugin/CMakeLists.txt @@ -35,8 +35,8 @@ if(TARGET KukaFRIClient AND Boost_FOUND AND Boost_CHRONO_FOUND) ${FRI-Client-SDK_Cpp_INCLUDE_DIRS} ${PROJECT_INCLUDE_DIR}/thirdparty/vrep/include ) - basis_add_library(v_repExtKukaLBRiiwa SHARED - v_repExtKukaLBRiiwa.cpp + basis_add_library(v_repExtKukaLBRiiwa SHARED + v_repExtKukaLBRiiwa.cpp ../../include/grl/kuka/KukaDriver.hpp ../../include/grl/kuka/KukaFRIalgorithm.hpp ../../include/grl/kuka/KukaFRIdriver.hpp @@ -45,13 +45,14 @@ if(TARGET KukaFRIClient AND Boost_FOUND AND Boost_CHRONO_FOUND) ../../include/grl/kuka/KukaJAVAdriver.hpp ../../include/grl/kuka/KukaNanopb.hpp ) - basis_target_link_libraries(v_repExtKukaLBRiiwa + basis_target_link_libraries(v_repExtKukaLBRiiwa PUBLIC - ${Boost_LIBRARIES} - ${Boost_REGEX_LIBRARY} - ${Boost_CHRONO_LIBRARY} - ${CMAKE_THREAD_LIBS_INIT} + ${Boost_LIBRARIES} + ${Boost_REGEX_LIBRARY} + ${Boost_CHRONO_LIBRARY} + ${CMAKE_THREAD_LIBS_INIT} ${Nanopb_LIBRARIES} + ${FLATBUFFERS_LIBRARIES} v_repLib KukaFRIClient ) From 0bee7251cd077671e1621b27b42d4b9506e3d122 Mon Sep 17 00:00:00 2001 From: Chunting Date: Fri, 23 Mar 2018 19:35:18 -0400 Subject: [PATCH 063/102] Fix some bugs, detruction, ldl --- config/Settings.cmake | 4 ++- include/grl/flatbuffer/flatbuffer.hpp | 2 +- include/grl/kuka/KukaFRIdriver.hpp | 28 +++++++++++++++---- .../CMakeLists.txt | 2 +- 4 files changed, 27 insertions(+), 9 deletions(-) diff --git a/config/Settings.cmake b/config/Settings.cmake index 804cb12..a013442 100644 --- a/config/Settings.cmake +++ b/config/Settings.cmake @@ -42,7 +42,9 @@ if(UNIX AND CMAKE_SYSTEM_PROCESSOR MATCHES "x86_64") ADD_DEFINITIONS(-fPIC) endif(UNIX AND CMAKE_SYSTEM_PROCESSOR MATCHES "x86_64") - +if(UNIX AND NOT APPLE) + set(LINUX_ONLY_LIBS ${LIBDL_LIBRARIES}) +endif() # Link the boost.log library # @todo consider an alternative to always linking boost log ADD_DEFINITIONS(-DBOOST_LOG_DYN_LINK) diff --git a/include/grl/flatbuffer/flatbuffer.hpp b/include/grl/flatbuffer/flatbuffer.hpp index 7d4eb9d..4b2e066 100644 --- a/include/grl/flatbuffer/flatbuffer.hpp +++ b/include/grl/flatbuffer/flatbuffer.hpp @@ -37,7 +37,7 @@ namespace grl { flatbuffers::LoadFile(jsonPath.c_str(), binary, &jsonfile); if (!ok) { printf("couldn't load files!\n"); - return nullptr; + return false; } // parse schema first, so we can use it to parse the data after diff --git a/include/grl/kuka/KukaFRIdriver.hpp b/include/grl/kuka/KukaFRIdriver.hpp index a2d6cb5..853b00f 100644 --- a/include/grl/kuka/KukaFRIdriver.hpp +++ b/include/grl/kuka/KukaFRIdriver.hpp @@ -103,15 +103,29 @@ class KukaFRIdriver : public std::enable_shared_from_thisjoin(); + if (m_driverThread) + { + m_driverThread->join(); + } + + for(auto saveThreadP : m_saveRecordingThreads) + { + saveThreadP->join(); } } + ~KukaFRIdriver() { + destruct(); + } + /// gets the number of seconds in one message exchange "tick" aka "cycle", /// "time step" of the robot arm's low level controller double getSecondsPerTick() { @@ -612,7 +626,9 @@ class KukaFRIdriver : public std::enable_shared_from_this i){ + jointIpoPostion.push_back(armState.ipoJointPosition[i]); + } externalTorque.push_back(armState.externalTorque[i]); } flatbuffers::Offset jointStatetab = grl::toFlatBuffer(*m_logFileBufferBuilderP, position, velocity, acceleration, torque); diff --git a/src/v_repExtGrlInverseKinematics/CMakeLists.txt b/src/v_repExtGrlInverseKinematics/CMakeLists.txt index c4d9a45..c3beffa 100644 --- a/src/v_repExtGrlInverseKinematics/CMakeLists.txt +++ b/src/v_repExtGrlInverseKinematics/CMakeLists.txt @@ -60,7 +60,7 @@ message(STATUS v_repExtGrlInverseKinematics BUILDING!!!) ${Nanopb_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT} ${FLATBUFFERS_STATIC_LIB} - -ldl + ${LINUX_ONLY_LIBS} KukaFRIClient mc_rbdyn_urdf SpaceVecAlg #TODO: re-enable when exported targets are correct again From ff62f6c05f6e0967c72ebe1cabcf992bfa52e359 Mon Sep 17 00:00:00 2001 From: Chunting Date: Sun, 25 Mar 2018 15:43:34 -0400 Subject: [PATCH 064/102] Set single buffer limit from lua script --- include/grl/kuka/KukaDriver.hpp | 4 +-- include/grl/kuka/KukaFRIdriver.hpp | 16 +++++---- include/grl/sensor/FusionTrackLogAndTrack.hpp | 15 +++++---- include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp | 6 ++-- src/lua/robone.lua | 18 +++++----- .../v_repExtKukaLBRiiwa.cpp | 33 ++++++++++++++----- test/KukaFRITest.cpp | 2 +- 7 files changed, 59 insertions(+), 35 deletions(-) diff --git a/include/grl/kuka/KukaDriver.hpp b/include/grl/kuka/KukaDriver.hpp index f68ed23..c31743c 100644 --- a/include/grl/kuka/KukaDriver.hpp +++ b/include/grl/kuka/KukaDriver.hpp @@ -230,10 +230,10 @@ namespace grl { namespace robot { namespace arm { /// start recording the kuka state data in memory /// return true on success, false on failure - bool start_recording() + bool start_recording(int single_buffer_limit_bytes) { if(FRIdriverP_.get() != nullptr) { - return FRIdriverP_->start_recording(); + return FRIdriverP_->start_recording(single_buffer_limit_bytes); } return false; } diff --git a/include/grl/kuka/KukaFRIdriver.hpp b/include/grl/kuka/KukaFRIdriver.hpp index 853b00f..7c09740 100644 --- a/include/grl/kuka/KukaFRIdriver.hpp +++ b/include/grl/kuka/KukaFRIdriver.hpp @@ -434,11 +434,13 @@ class KukaFRIdriver : public std::enable_shared_from_this> m_saveRecordingThreads; - grl::TimeEvent time_event_stamp; + grl::TimeEvent time_event_stamp; + + const std::size_t MegaByte = 1024*1024; + // If we write too large a flatbuffer + std::size_t single_buffer_limit_bytes = 20*MegaByte; + const std::size_t single_buffer_limit_states = 1350000000; #ifdef HAVE_spdlog std::shared_ptr loggerP; diff --git a/include/grl/sensor/FusionTrackLogAndTrack.hpp b/include/grl/sensor/FusionTrackLogAndTrack.hpp index bfe551c..b70dba1 100644 --- a/include/grl/sensor/FusionTrackLogAndTrack.hpp +++ b/include/grl/sensor/FusionTrackLogAndTrack.hpp @@ -346,9 +346,10 @@ class FusionTrackLogAndTrack : public std::enable_shared_from_this deviceSerialNumbers; try { @@ -582,6 +580,11 @@ class FusionTrackLogAndTrack : public std::enable_shared_from_this loggerP; #endif // HAVE_spdlog diff --git a/include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp b/include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp index e5f6816..84c9267 100644 --- a/include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp +++ b/include/grl/vrep/KukaLBRiiwaVrepPlugin.hpp @@ -177,12 +177,12 @@ class KukaVrepPlugin : public std::enable_shared_from_this { /// start recording the kuka state data in memory /// return true on success, false on failure - bool start_recording() + bool start_recording(int single_buffer_limit_bytes) { if(kukaDriverP_.get() != nullptr) { - return kukaDriverP_->start_recording(); + return kukaDriverP_->start_recording(single_buffer_limit_bytes); } - std::cout << "kukaDriverP_ is nullptr..." << std::endl; + std::cout << "kukaDriverP_ is nullptr..." << single_buffer_limit_bytes << std::endl; return false; } /// stop recording the kuka state data in memory diff --git a/src/lua/robone.lua b/src/lua/robone.lua index 6cbfca9..281594d 100644 --- a/src/lua/robone.lua +++ b/src/lua/robone.lua @@ -11,6 +11,9 @@ robone = {} require "grl" + +KUKA_single_buffer_limit_bytes = 16 -- MB +FB_single_buffer_limit_bytes = 180 -- MB ------------------------------------------ -- Move the arm along the cut file path -- ------------------------------------------ @@ -229,9 +232,7 @@ robone.cutBoneScript=function() end function fcv(ikGroupHandle, CreatedPathHandle, maxVelocity, maxForce, measuredForce, pathLength) - print("run in fcv............... ") - - -- selection parameter for simRMLPos for default behavior + -- selection parameter for simRMLPos for default behavior selection = {1,1,1,1,1,1,1} -- Get position (in meters) on the path (0 = start, pathLength = end) @@ -354,7 +355,7 @@ robone.handEyeCalibScript=function() path=simGetObjectHandle('HandEyeCalibPath') circleCalib = simGetObjectHandle('CircleCalibPath') endeffectorTarget=simGetObjectHandle('RobotMillTipTarget') - numSteps=16 + numSteps=18 startP,startO=grl.getTransformBetweenHandles(target,targetBase) @@ -445,8 +446,8 @@ robone.startRealArmDriverScript=function() 'FRI' , -- KukaMonitorMode (options are FRI, JAVA) "IK_Group1_iiwa" -- IKGroupName ) - - simExtKukaLBRiiwaRecordWhileSimulationIsRunning(true) + + simExtKukaLBRiiwaRecordWhileSimulationIsRunning(true, KUKA_single_buffer_limit_bytes) else simDisplayDialog('Error','KukaLBRiiwa plugin was not found. (v_repExtKukaLBRiiwa.dll)&&nSimulation will run without hardware',sim_dlgstyle_ok,true,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1}) end @@ -594,12 +595,13 @@ robone.configureOpticalTracker=function() print(robotToWorldM[i].." ") end end - + + -- Start collecting data from the optical tracker simExtAtracsysFusionTrackStart() -- Test the below one -- Enable the recordDataScript() to call the method below. - simExtAtracsysFusionTrackRecordWhileSimulationIsRunning(true) + simExtAtracsysFusionTrackRecordWhileSimulationIsRunning(true, FB_single_buffer_limit_bytes) -------------------------------------------------- -- Get the relative transformation matrix between Fiducial#22 and LBR_iiwa_14_R820_link8 --[[ diff --git a/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp b/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp index 6798395..f7ddeb0 100755 --- a/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp +++ b/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp @@ -47,6 +47,9 @@ std::shared_ptr kukaVrepPluginPG; std::shared_ptr loggerPG; /// Recording will begin when the simulation starts running, and log files will be saved every time it stops running. bool recordWhileSimulationIsRunningG = false; +const std::size_t MegaByte = 1024*1024; +// If we write too large a flatbuffer +std::size_t single_buffer_limit_bytes = 20*MegaByte; const int inArgs_KUKA_LBR_IIWA_START[]={ 16, // Example Value // Parameter name @@ -65,7 +68,7 @@ const int inArgs_KUKA_LBR_IIWA_START[]={ sim_lua_arg_string,0, // "30200" , // RemoteHostKukaKoniUDPPort sim_lua_arg_string,0, // "JAVA" , // KukaCommandMode (options are "JAVA", "FRI") sim_lua_arg_string,0, // "FRI" , // KukaMonitorMode (options are "JAVA", "FRI") - sim_lua_arg_string,0, // "IK_Group1_iiwa" // IKGroupName (VREP built in inverse kinematics group) + sim_lua_arg_string,0 // "IK_Group1_iiwa" // IKGroupName (VREP built in inverse kinematics group) }; std::string LUA_KUKA_LBR_IIWA_START_CALL_TIP("number result=simExtKukaLBRiiwaStart(string_table JointHandles , string RobotTipHandle, string RobotFlangeTipHandle, string RobotTargetHandle, string RobotTargetBaseHandle, string RobotModel, string LocalUDPAddress, string LocalUDPPort, string RemoteUDPAddress, string LocalHostKukaKoniUDPAddress, string LocalHostKukaKoniUDPPort, string RemoteHostKukaKoniUDPAddress, string RemoteHostKukaKoniUDPPort, string KukaCommandMode, string KukaMonitorMode, string IKGroupName) -- KukaCommandMode options are JAVA and FRI"); @@ -161,7 +164,10 @@ void LUA_SIM_EXT_KUKA_LBR_IIWA_START(SLuaCallBack* p) ///////////////////////////////////////////////////////////////////////////////////////// /// LUA function to actually start recording the fusiontrack frame data in memory. ///////////////////////////////////////////////////////////////////////////////////////// - +const int inArgs_LUA_SIM_EXT_KUKA_LBR_IIWA_START_RECORDING[] = { + 1, + sim_lua_arg_float,0 // Signle buffer limit +}; // simExtKUKAiiwaStartRecording void LUA_SIM_EXT_KUKA_LBR_IIWA_START_RECORDING(SLuaCallBack *p) { @@ -170,9 +176,11 @@ void LUA_SIM_EXT_KUKA_LBR_IIWA_START_RECORDING(SLuaCallBack *p) if (kukaVrepPluginPG) { std::string log_message("Starting the recording of KUKAiiwa state data in memory.\n"); + std::vector *inData = D.getInDataPtr(); simAddStatusbarMessage(log_message.c_str()); loggerPG->info(log_message); - success = kukaVrepPluginPG->start_recording(); + single_buffer_limit_bytes = inData->at(0).floatData[0]; + success = kukaVrepPluginPG->start_recording(single_buffer_limit_bytes); } D.pushOutData(CLuaFunctionDataItem(success)); D.writeDataToLua(p); @@ -252,11 +260,12 @@ void LUA_SIM_EXT_KUKA_LBR_IIWA_CLEAR_RECORDING(SLuaCallBack *p) #define LUA_SIM_EXT_KUKA_LBR_IIWA_RECORD_WHILE_SIMULATION_IS_RUNNING_COMMAND "simExtKukaLBRiiwaRecordWhileSimulationIsRunning" const int inArgs_LUA_SIM_EXT_KUKA_LBR_IIWA_RECORD_WHILE_SIMULATION_IS_RUNNING[] = { - 1, - sim_lua_arg_bool, 1 // string file name + 2, + sim_lua_arg_bool, 1, // string file name + sim_lua_arg_float,0 // Signle buffer limit }; -std::string LUA_SIM_EXT_KUKA_LBR_IIWA_RECORD_WHILE_SIMULATION_IS_RUNNING_CALL_TIP("number result=simExtKukaLBRiiwaRecordWhileSimulationIsRunning(bool recording)"); +std::string LUA_SIM_EXT_KUKA_LBR_IIWA_RECORD_WHILE_SIMULATION_IS_RUNNING_CALL_TIP("number result=simExtKukaLBRiiwaRecordWhileSimulationIsRunning(bool recording, float single_buffer_limit_bytes)"); void LUA_SIM_EXT_KUKA_LBR_IIWA_RECORD_WHILE_SIMULATION_IS_RUNNING(SLuaCallBack *p) { @@ -270,13 +279,14 @@ void LUA_SIM_EXT_KUKA_LBR_IIWA_RECORD_WHILE_SIMULATION_IS_RUNNING(SLuaCallBack * { std::vector *inData = D.getInDataPtr(); recordWhileSimulationIsRunningG = inData->at(0).boolData[0]; + single_buffer_limit_bytes = inData->at(1).floatData[0]; std::cout << "Start recording while simulation is running..." << recordWhileSimulationIsRunningG << std::endl; if (kukaVrepPluginPG && recordWhileSimulationIsRunningG) { std::string log_message = std::string("simExtKukaLBRiiwaRecordWhileSimulationIsRunning: ") + boost::lexical_cast(recordWhileSimulationIsRunningG); simAddStatusbarMessage(log_message.c_str()); loggerPG->info(log_message); - kukaVrepPluginPG->start_recording(); + kukaVrepPluginPG->start_recording(single_buffer_limit_bytes); } D.pushOutData(CLuaFunctionDataItem(success)); @@ -352,10 +362,15 @@ VREP_DLLEXPORT unsigned char v_repStart(void* reservedPointer,int reservedInt) int inArgs1[] = {0}; // no input arguments /// Register functions to control the recording procedure of the fusiontrack frame data in memory - simRegisterCustomLuaFunction("simExtKukaLBRiiwaStartRecording", "number result=simExtKukaLBRiiwaStartRecording()", inArgs1, LUA_SIM_EXT_KUKA_LBR_IIWA_START_RECORDING); simRegisterCustomLuaFunction("simExtKukaLBRiiwaStopRecording", "number result=simExtKukaLBRiiwaStopRecording()", inArgs1, LUA_SIM_EXT_KUKA_LBR_IIWA_STOP_RECORDING); simRegisterCustomLuaFunction("simExtKukaLBRiiwaClearRecording", "number result=simExtKukaLBRiiwaClearRecording()", inArgs1, LUA_SIM_EXT_KUKA_LBR_IIWA_CLEAR_RECORDING); + CLuaFunctionData::getInputDataForFunctionRegistration(inArgs_LUA_SIM_EXT_KUKA_LBR_IIWA_START_RECORDING, inArgs); + simRegisterCustomLuaFunction("simExtKukaLBRiiwaStartRecording", + "number result=simExtKukaLBRiiwaStartRecording(simxFloat singlebuffersize)", + &inArgs[0], + LUA_SIM_EXT_KUKA_LBR_IIWA_START_RECORDING); + CLuaFunctionData::getInputDataForFunctionRegistration(inArgs_LUA_SIM_EXT_KUKA_LBR_IIWA_SAVE_RECORDING, inArgs); simRegisterCustomLuaFunction(LUA_SIM_EXT_KUKA_LBR_IIWA_SAVE_RECORDING_COMMAND, LUA_SIM_EXT_KUKA_LBR_IIWA_SAVE_RECORDING_CALL_TIP.c_str(), @@ -502,7 +517,7 @@ VREP_DLLEXPORT void* v_repMessage(int message,int* auxiliaryData,void* customDat // loggerPG->error( initerr ); // } if(kukaVrepPluginPG && recordWhileSimulationIsRunningG) { - kukaVrepPluginPG->start_recording(); + kukaVrepPluginPG->start_recording(single_buffer_limit_bytes); } } diff --git a/test/KukaFRITest.cpp b/test/KukaFRITest.cpp index 4c76f0b..23091b9 100644 --- a/test/KukaFRITest.cpp +++ b/test/KukaFRITest.cpp @@ -248,7 +248,7 @@ int main(int argc, char* argv[]) { kukaDriverP->set( jointStateToCommand, grl::revolute_joint_angle_open_chain_command_tag()); - kukaDriverP->start_recording(); + kukaDriverP->start_recording(2); kukaDriverP->run_one(); } From 0a9163de02260ad6d952114185f4e5e21021ceca Mon Sep 17 00:00:00 2001 From: Chunting Date: Sun, 25 Mar 2018 17:25:22 -0400 Subject: [PATCH 065/102] Write the hand eye result into a file --- .../grl/vrep/HandEyeCalibrationVrepPlugin.hpp | 20 +++++++++++ .../v_repExtAtracsysFusionTrack.cpp | 33 ++++++++++++++----- 2 files changed, 44 insertions(+), 9 deletions(-) diff --git a/include/grl/vrep/HandEyeCalibrationVrepPlugin.hpp b/include/grl/vrep/HandEyeCalibrationVrepPlugin.hpp index ba9cb29..89d79b6 100644 --- a/include/grl/vrep/HandEyeCalibrationVrepPlugin.hpp +++ b/include/grl/vrep/HandEyeCalibrationVrepPlugin.hpp @@ -9,11 +9,17 @@ #include "grl/vrep/Eigen.hpp" #include "grl/vrep/Vrep.hpp" +#include "grl/time.hpp" #include "camodocal/calib/HandEyeCalibration.h" #include "v_repLib.h" #include "grl/vector_ostream.hpp" +// boost::filesystem +#include +#include +#include + namespace grl { @@ -173,11 +179,15 @@ void estimateHandEyeScrew(){ logger_->info( "\n{}", poseString(RobotTipToFiducial,"expected RobotTipToFiducial (simulation only): ")); } + + + logger_->info( "\n{}", poseString(transformEstimate,"estimated RobotTipToFiducial:")); applyEstimate(); // print results + Eigen::Quaterniond eigenQuat(transformEstimate.rotation()); logger_->info( "Hand Eye Screw Estimate quat wxyz\n: {} , {} , {} , {} \n translation xyz: {} {} {}", eigenQuat.w(), eigenQuat.x(), eigenQuat.y(),eigenQuat.z(), transformEstimate.translation().x(), transformEstimate.translation().y(), transformEstimate.translation().z()); @@ -186,6 +196,16 @@ void estimateHandEyeScrew(){ detectedObjectQuaternion[0], detectedObjectQuaternion[1], detectedObjectQuaternion[2], detectedObjectQuaternion[3], detectedObjectPosition[0], detectedObjectPosition[1], detectedObjectPosition[2]); + + // Write the hand eye calibration result into a file + auto myFile = boost::filesystem::current_path() /"HandEyeCalibration_Result.txt"; + boost::filesystem::ofstream ofs(myFile/*.native()*/); + // boost::archive::text_oarchive ta(ofs); + ofs <<"\n\n=========== " + current_date_and_time_string() + " =======================================\n"; + ofs << "Hand Eye Screw Estimate quat wxyz: " << eigenQuat.w() << " "<< eigenQuat.x() << " " << eigenQuat.y() << " " << eigenQuat.z() + << "\ntranslation xyz: " << transformEstimate.translation().x() << " " + << transformEstimate.translation().y() << " " << transformEstimate.translation().z(); + } /// @brief Will apply the stored estimate to the v-rep simulation value diff --git a/src/v_repExtAtracsysFusionTrack/v_repExtAtracsysFusionTrack.cpp b/src/v_repExtAtracsysFusionTrack/v_repExtAtracsysFusionTrack.cpp index b01ba30..5f413b5 100755 --- a/src/v_repExtAtracsysFusionTrack/v_repExtAtracsysFusionTrack.cpp +++ b/src/v_repExtAtracsysFusionTrack/v_repExtAtracsysFusionTrack.cpp @@ -50,6 +50,10 @@ std::shared_ptr loggerPG; bool recordWhileSimulationIsRunningG = false; bool shouldPrintConnectionSuccess = true; +const std::size_t MegaByte = 1024*1024; +// If we write too large a flatbuffer +std::size_t single_buffer_limit_bytes = 20*MegaByte; + void removeGeometryID(std::string geometryID_lua_param, grl::FusionTrackLogAndTrack::Params ¶ms) { @@ -319,7 +323,10 @@ void LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_IS_ACTIVE(SLuaCallBack *p) ///////////////////////////////////////////////////////////////////////////////////////// /// LUA function to actually start recording the fusiontrack frame data in memory. ///////////////////////////////////////////////////////////////////////////////////////// - +const int inArgs_LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_START_RECORDING[] = { + 1, + sim_lua_arg_float,0 // Signle buffer limit +}; // simExtAtracsysFusionTrackStartRecording void LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_START_RECORDING(SLuaCallBack *p) { @@ -328,9 +335,11 @@ void LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_START_RECORDING(SLuaCallBack *p) if (fusionTrackPG) { std::string log_message("Starting the recording of fusiontrack frame data in memory.\n"); + std::vector *inData = D.getInDataPtr(); simAddStatusbarMessage(log_message.c_str()); loggerPG->info(log_message); - success = fusionTrackPG->start_recording(); + single_buffer_limit_bytes = inData->at(0).floatData[0]; + success = fusionTrackPG->start_recording(single_buffer_limit_bytes); } D.pushOutData(CLuaFunctionDataItem(success)); D.writeDataToLua(p); @@ -415,11 +424,12 @@ void LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_SAVE_RECORDING(SLuaCallBack *p) #define LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_RECORD_WHILE_SIMULATION_IS_RUNNING_COMMAND "simExtAtracsysFusionTrackRecordWhileSimulationIsRunning" const int inArgs_LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_RECORD_WHILE_SIMULATION_IS_RUNNING[] = { - 1, - sim_lua_arg_bool, 1 // string file name + 2, + sim_lua_arg_bool, 1, // string file name + sim_lua_arg_float,0 // Signle buffer limit }; -std::string LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_RECORD_WHILE_SIMULATION_IS_RUNNING_CALL_TIP("number result=simExtAtracsysFusionTrackRecordWhileSimulationIsRunning(bool recording)"); +std::string LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_RECORD_WHILE_SIMULATION_IS_RUNNING_CALL_TIP("number result=simExtAtracsysFusionTrackRecordWhileSimulationIsRunning(bool recording, float single_buffer_limit_bytes)"); void LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_RECORD_WHILE_SIMULATION_IS_RUNNING(SLuaCallBack *p) { @@ -433,13 +443,14 @@ void LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_RECORD_WHILE_SIMULATION_IS_RUNNING(SLuaCa { std::vector *inData = D.getInDataPtr(); recordWhileSimulationIsRunningG = inData->at(0).boolData[0]; + single_buffer_limit_bytes = inData->at(1).floatData[0]; // std::cout << "Start recording while simulation is running for Tracker..." << recordWhileSimulationIsRunningG << std::endl; if (fusionTrackPG && recordWhileSimulationIsRunningG) { std::string log_message = std::string("simExtAtracsysFusionTrackRecordWhileSimulationIsRunning: ") + boost::lexical_cast(recordWhileSimulationIsRunningG); simAddStatusbarMessage(log_message.c_str()); loggerPG->info(log_message); - success = fusionTrackPG->start_recording(); + success = fusionTrackPG->start_recording(single_buffer_limit_bytes); } D.pushOutData(CLuaFunctionDataItem(success)); @@ -549,10 +560,14 @@ VREP_DLLEXPORT unsigned char v_repStart(void *reservedPointer, int reservedInt) simRegisterCustomLuaFunction("simExtAtracsysFusionTrackClearObjects", "number result=simExtAtracsysFusionTrackClearObjects()", inArgs1, LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_CLEAR_OBJECTS); /// Register functions to control the recording procedure of the fusiontrack frame data in memory - simRegisterCustomLuaFunction("simExtAtracsysFusionTrackStartRecording", "number result=simExtAtracsysFusionTrackStartRecording()", inArgs1, LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_START_RECORDING); simRegisterCustomLuaFunction("simExtAtracsysFusionTrackStopRecording", "number result=simExtAtracsysFusionTrackStopRecording()", inArgs1, LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_STOP_RECORDING); simRegisterCustomLuaFunction("simExtAtracsysFusionTrackClearRecording", "number result=simExtAtracsysFusionTrackClearRecording()", inArgs1, LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_CLEAR_RECORDING); - + + CLuaFunctionData::getInputDataForFunctionRegistration(inArgs_LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_START_RECORDING, inArgs); + simRegisterCustomLuaFunction("simExtAtracsysFusionTrackStartRecording", + "number result=simExtAtracsysFusionTrackStartRecording()", + &inArgs[0], + LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_START_RECORDING); CLuaFunctionData::getInputDataForFunctionRegistration(inArgs_LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_SAVE_RECORDING, inArgs); simRegisterCustomLuaFunction(LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_SAVE_RECORDING_COMMAND, LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_SAVE_RECORDING_CALL_TIP.c_str(), @@ -721,7 +736,7 @@ VREP_DLLEXPORT void *v_repMessage(int message, int *auxiliaryData, void *customD // } if(fusionTrackPG && recordWhileSimulationIsRunningG) { - fusionTrackPG->start_recording(); + fusionTrackPG->start_recording(single_buffer_limit_bytes); } } // simulation has ended From ee3ba717b532b8198f5803b937195a86aae72907 Mon Sep 17 00:00:00 2001 From: Chunting Date: Thu, 29 Mar 2018 14:33:20 -0400 Subject: [PATCH 066/102] To easily analysis data, creat a new branch and redesign the csv files --- example/fusionTrackExample.cpp | 2 +- include/grl/flatbuffer/CSVIterator.hpp | 76 ++++ .../grl/flatbuffer/ParseflatbuffertoCSV.hpp | 429 ++++++++++++------ .../grl/vrep/HandEyeCalibrationVrepPlugin.hpp | 17 +- .../grl/vrep/InverseKinematicsVrepPlugin.hpp | 153 ++++--- src/lua/robone.lua | 6 +- test/KukaPoseEstimationTest.cpp | 2 +- test/readFlatbufferTest.cpp | 17 +- 8 files changed, 486 insertions(+), 216 deletions(-) create mode 100644 include/grl/flatbuffer/CSVIterator.hpp diff --git a/example/fusionTrackExample.cpp b/example/fusionTrackExample.cpp index 17f2422..459c700 100644 --- a/example/fusionTrackExample.cpp +++ b/example/fusionTrackExample.cpp @@ -88,7 +88,7 @@ int main(int argc, char **argv) std::string json_file_suffix = ".json"; std::string fbs_filename("LogKUKAiiwaFusionTrack.fbs"); // std::string includePath = grl::getpathtofbsfile(fbs_filename); - std::string includePath = "/home/chunting/src/robonetracker/build/"; + std::string includePath = "/home/cjiao1/src/robonetracker/build/"; std::size_t builder_size_bytes = 0; diff --git a/include/grl/flatbuffer/CSVIterator.hpp b/include/grl/flatbuffer/CSVIterator.hpp new file mode 100644 index 0000000..e421b2f --- /dev/null +++ b/include/grl/flatbuffer/CSVIterator.hpp @@ -0,0 +1,76 @@ +#ifndef GRL_CSV_ITERATOR +#define GRL_CSV_ITERATOR + +#include +#include +#include +#include +namespace grl { +class CSVRow +{ + public: + std::string const& operator[](std::size_t index) const + { + return m_data[index]; + } + std::size_t size() const + { + return m_data.size(); + } + void readNextRow(std::istream& str) + { + std::string line; + std::getline(str, line); + + std::stringstream lineStream(line); + std::string cell; + + m_data.clear(); + while(std::getline(lineStream, cell, ',')) + { + m_data.push_back(cell); + } + // This checks for a trailing comma with no data after it. + if (!lineStream && cell.empty()) + { + // If there was a trailing comma then add an empty element. + m_data.push_back(""); + } + } + private: + std::vector m_data; +}; + +std::istream& operator>>(std::istream& str, CSVRow& data) +{ + data.readNextRow(str); + return str; +} + +class CSVIterator +{ + public: + typedef std::input_iterator_tag iterator_category; + typedef CSVRow value_type; + typedef std::size_t difference_type; + typedef CSVRow* pointer; + typedef CSVRow& reference; + + CSVIterator(std::istream& str) :m_str(str.good()?&str:NULL) { ++(*this); } + CSVIterator() :m_str(NULL) {} + + // Pre Increment + CSVIterator& operator++() {if (m_str) { if (!((*m_str) >> m_row)){m_str = NULL;}}return *this;} + // Post increment + CSVIterator operator++(int) {CSVIterator tmp(*this);++(*this);return tmp;} + CSVRow const& operator*() const {return m_row;} + CSVRow const* operator->() const {return &m_row;} + + bool operator==(CSVIterator const& rhs) {return ((this == &rhs) || ((this->m_str == NULL) && (rhs.m_str == NULL)));} + bool operator!=(CSVIterator const& rhs) {return !((*this) == rhs);} + private: + std::istream* m_str; + CSVRow m_row; +}; +} +#endif \ No newline at end of file diff --git a/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp b/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp index fa1f225..336a78f 100644 --- a/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp +++ b/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp @@ -12,6 +12,7 @@ // #include "grl/flatbuffer/FlatbufferToEigen.hpp" +#include "grl/flatbuffer/CSVIterator.hpp" // FusionTrack Libraries @@ -74,10 +75,7 @@ #include #include "kukaiiwaURDF.h" -#include -#include -#include -#include + namespace grl { @@ -148,7 +146,7 @@ namespace grl { /// Return the original timeEvent of fusiontracker /// @param TlogKUKAiiwaFusionTrackP pointer of the root object for fusiontracker. /// @param fusiontracker_tag tag dispatch - grl::MatrixXd getTimeStamp(const fbs_tk::Root &logKUKAiiwaFusionTrackP, fusiontracker_tag){ + grl::MatrixXd getTimeStamp(const fbs_tk::Root &logKUKAiiwaFusionTrackP, fusiontracker_tag, int markerID = 0){ auto states = logKUKAiiwaFusionTrackP->states(); std::size_t state_size = states->size(); @@ -166,16 +164,30 @@ namespace grl { for(int markerIndex=0; markerIndexGet(markerIndex); auto marker_ID = marker->geometryID(); - if(marker_ID == 22 || marker_ID == 55){ - auto timeEvent = kukaiiwaFusionTrackMessage->timeEvent(); - timeEventM(row,TimeType::local_receive_time) = timeEvent->local_receive_time(); - timeEventM(row,TimeType::local_request_time) = timeEvent->local_request_time(); - timeEventM(row,TimeType::device_time) = timeEvent->device_time(); - timeEventM(row,TimeType::time_Y) = timeEventM(row,TimeType::device_time) - timeEventM(row,TimeType::local_receive_time); - timeEventM(row,TimeType::counterIdx) = counter; - row++; - break; - } + // if(markerID == 0){ + // if(marker_ID == 22 || marker_ID == 55){ + // auto timeEvent = kukaiiwaFusionTrackMessage->timeEvent(); + // timeEventM(row,TimeType::local_receive_time) = timeEvent->local_receive_time(); + // timeEventM(row,TimeType::local_request_time) = timeEvent->local_request_time(); + // timeEventM(row,TimeType::device_time) = timeEvent->device_time(); + // timeEventM(row,TimeType::time_Y) = timeEventM(row,TimeType::device_time) - timeEventM(row,TimeType::local_receive_time); + // timeEventM(row,TimeType::counterIdx) = counter; + // row++; + // break; + // } + // } else { + if(marker_ID == markerID){ + auto timeEvent = kukaiiwaFusionTrackMessage->timeEvent(); + timeEventM(row,TimeType::local_receive_time) = timeEvent->local_receive_time(); + timeEventM(row,TimeType::local_request_time) = timeEvent->local_request_time(); + timeEventM(row,TimeType::device_time) = timeEvent->device_time(); + timeEventM(row,TimeType::time_Y) = timeEventM(row,TimeType::device_time) - timeEventM(row,TimeType::local_receive_time); + timeEventM(row,TimeType::counterIdx) = counter; + row++; + break; + } + // } + } } } @@ -235,12 +247,12 @@ namespace grl { /// Get the maker pose based on the markerID. The bad data, which means the frame doesn't have the indicated marker information, has been filtered out. /// As for the bad data, both the marker pose and the corresponding timeEvent is skipped. /// @param logKUKAiiwaFusionTrackP, pointer of the root object for fusiontracker. - /// @param makerID, the indicated marker. + /// @param markerID, the indicated marker. /// @param timeEventM, timeEvent without bad data, which is filled out. /// @param markerPose, the pose matrix, which is filled out. /// @return row, the number of valid rows. int getMarkerPose(const fbs_tk::Root &logKUKAiiwaFusionTrackP, - uint32_t &makerID, + uint32_t &markerID, grl::MatrixXd& timeEventM, Eigen::MatrixXd& markerPose, bool markerFrame = false){ @@ -264,7 +276,7 @@ namespace grl { for(int markerIndex=0; markerIndexGet(markerIndex); auto marker_ID = marker->geometryID(); - if(marker_ID == makerID){ + if(marker_ID == markerID){ auto timeEvent = kukaiiwaFusionTrackMessage->timeEvent(); timeEventM(row,TimeType::local_receive_time) = timeEvent->local_receive_time(); timeEventM(row,TimeType::local_request_time) = timeEvent->local_request_time(); @@ -314,7 +326,7 @@ namespace grl { } double diff = static_cast(state_size-markerPose.rows()); std::cout <<"State size: " << state_size <<" markerPose size: " << markerPose.rows() - << " lossing rate " << diff/state_size << " makerID: " << makerID < &logKUKAiiwaFusionTrackP, - const fbs_tk::Root &kukaStatesP) { + const fbs_tk::Root &kukaStatesP, + uint32_t &markerID) { - grl::MatrixXd timeEventM_FT = grl::getTimeStamp(logKUKAiiwaFusionTrackP, grl::fusiontracker_tag()); + grl::MatrixXd timeEventM_FT = grl::getTimeStamp(logKUKAiiwaFusionTrackP, grl::fusiontracker_tag(), markerID); grl::MatrixXd timeEventM_Kuka = grl::getTimeStamp(kukaStatesP, grl::kuka_tag()); std::size_t kuka_state_size = timeEventM_Kuka.rows(); std::cout<< "------Kuka_state_size: "<< timeEventM_Kuka.rows() << std::endl; std::size_t FT_state_size = timeEventM_FT.rows(); std::cout<< "------FT_state_size: "<< timeEventM_FT.rows() << std::endl; + + /////////////////////////////////////////////////////////////////////////////////// + + + std::size_t FT_size = grl::getStateSize(logKUKAiiwaFusionTrackP); + + + grl::MatrixXd timeEventM = grl::MatrixXd::Zero(FT_size, timeEventM_FT.cols()); + Eigen::MatrixXd markerPose(FT_size, grl::col_Pose); + // Get the pose of the marker 22. + // Since the bad data is skipped, the timeEventM_FT also needs to be filled out with the orinial time stamp. + + int validsize_22 = grl::getMarkerPose(logKUKAiiwaFusionTrackP, markerID, timeEventM, markerPose); + assert(timeEventM.rows() == timeEventM_FT.rows()); + ////////////////////////////////////////////////////////////////////////////////// + + + int kuka_index = 0; int FT_index = 0; @@ -474,7 +505,8 @@ namespace grl { grl::VectorXd kuka_local_request_time = timeEventM_Kuka.col(local_request_time) - initial_local_time * grl::VectorXd::Ones(kuka_state_size); grl::VectorXd kuka_local_receive_time = timeEventM_Kuka.col(local_receive_time) - initial_local_time * grl::VectorXd::Ones(kuka_state_size); grl::VectorXd kuka_device_time = timeEventM_Kuka.col(device_time) - initial_device_time_kuka * grl::VectorXd::Ones(kuka_state_size); - + + Eigen::MatrixXd jointAngles = grl::getAllJointAngles(kukaStatesP); std::ofstream fs; // create a name for the file output @@ -486,9 +518,22 @@ namespace grl { << "FT_device_time_offset," << "device_time_offset_kuka," << "Y_FT," - << "Y_kuka" + << "Y_kuka," + << "FT_X," + << "FT_Y," + << "FT_Z," + << "FT_A," + << "FT_B," + << "FT_C," + << "K_Joint1," + << "K_Joint2," + << "K_Joint3," + << "K_Joint4," + << "K_Joint5," + << "K_Joint6," + << "K_Joint7" << std::endl; - + std::cout << "Start to write ... "<< std::endl <<"FT_index: " << FT_index << " kuka_index: " << kuka_index << std::endl; int64_t kuka_diff = kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index); int64_t FT_diff = FT_device_time(FT_index) - FT_local_receive_time(FT_index); while ( kuka_index < kuka_state_size && FT_index < FT_state_size ) @@ -496,16 +541,26 @@ namespace grl { // If the row value is the kuka time, then the FT cells should be left blank. if ( kuka_local_receive_time(kuka_index) < FT_local_receive_time(FT_index) ){ // write the data to the output file + auto jointrow = jointAngles.row(kuka_index); fs << kuka_local_receive_time(kuka_index) <<"," <<"," << kuka_local_request_time(kuka_index) << "," << "," << kuka_device_time(kuka_index) <<"," << "," - << kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index) - kuka_diff + << kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index) - kuka_diff << "," + << ",,,,,," + << jointrow[0] << "," + << jointrow[1] << "," + << jointrow[2] << "," + << jointrow[3] << "," + << jointrow[4] << "," + << jointrow[5] << "," + << jointrow[6] << std::endl; kuka_index++; } else if( kuka_local_receive_time(kuka_index) > FT_local_receive_time(FT_index)) { + auto matrixrow = markerPose.row(FT_index); // If the row value is the FT time, then the kuka cells should be left blank. fs << FT_local_receive_time(FT_index) <<"," << FT_local_request_time(FT_index) << "," @@ -513,7 +568,13 @@ namespace grl { << FT_device_time(FT_index) << "," << "," << FT_device_time(FT_index) - FT_local_receive_time(FT_index) - FT_diff << "," - << std::endl; + << "," + << matrixrow[0] << "," + << matrixrow[1] << "," + << matrixrow[2] << "," + << matrixrow[3] << "," + << matrixrow[4] << "," + << matrixrow[5] << std::endl; FT_index++; } else { // In case the time is extactly equivent with each other. @@ -529,25 +590,155 @@ namespace grl { kuka_index++; } } + std::cout << "FT_index: " << FT_index << " kuka_index: " << kuka_index << " Sum: " << FT_index+kuka_index << std::endl; fs.close(); } - /// Get the transformation matrix from LBR_iiwa_14_R820#0 to OpticalTrackerBase#0. - /// See the physical relationship in vrep. - std::vector getRobotToTrackerMatrix(std::vector& FudicialPose){ - Eigen::Vector3d trans; - trans << 0.20652678608894 , -0.70480275154114 ,1.4510889053345 ; - Eigen::Matrix3d rot; - rot << -0.19507896900177, -0.8963907957077, -0.39802974462509 , - 0.9409122467041, -0.28804504871368, 0.17808490991592, - -0.28378140926361, -0.33688074350357, 0.89776360988617 ; - sva::PTransformd relativeTransform(rot, trans); - std::vector to(FudicialPose.size()); - for(int i=0; i timeEventM_FT(FT_index,local_receive_time) && kuka_index == 0){ +// FT_index++; +// } + +// auto initial_local_time = std::min(timeEventM_FT(FT_index,local_receive_time), timeEventM_Kuka(kuka_index,local_receive_time)); +// auto initial_device_time_kuka = timeEventM_Kuka(kuka_index,device_time); +// auto initial_device_time_FT = timeEventM_FT(FT_index,device_time); +// grl::VectorXd FT_local_request_time = timeEventM_FT.col(local_request_time) - initial_local_time * grl::VectorXd::Ones(FT_state_size); +// grl::VectorXd FT_local_receive_time = timeEventM_FT.col(local_receive_time) - initial_local_time * grl::VectorXd::Ones(FT_state_size); +// grl::VectorXd FT_device_time = timeEventM_FT.col(device_time) - initial_device_time_FT * grl::VectorXd::Ones(FT_state_size); + +// grl::VectorXd kuka_local_request_time = timeEventM_Kuka.col(local_request_time) - initial_local_time * grl::VectorXd::Ones(kuka_state_size); +// grl::VectorXd kuka_local_receive_time = timeEventM_Kuka.col(local_receive_time) - initial_local_time * grl::VectorXd::Ones(kuka_state_size); +// grl::VectorXd kuka_device_time = timeEventM_Kuka.col(device_time) - initial_device_time_kuka * grl::VectorXd::Ones(kuka_state_size); + + +// std::ofstream fs; +// // create a name for the file output +// fs.open( FTKUKA_TimeEvent_CSVfilename, std::ofstream::out | std::ofstream::app); +// // write the file headers +// fs << "local_receive_time_offset_X," +// << "FT_local_request_time," +// << "KUKA_local_request_time," +// << "FT_device_time_offset," +// << "device_time_offset_kuka," +// << "Y_FT," +// << "Y_kuka" +// << std::endl; + +// int64_t kuka_diff = kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index); +// int64_t FT_diff = FT_device_time(FT_index) - FT_local_receive_time(FT_index); +// while ( kuka_index < kuka_state_size && FT_index < FT_state_size ) +// { +// // If the row value is the kuka time, then the FT cells should be left blank. +// if ( kuka_local_receive_time(kuka_index) < FT_local_receive_time(FT_index) ){ +// // write the data to the output file +// fs << kuka_local_receive_time(kuka_index) <<"," +// <<"," +// << kuka_local_request_time(kuka_index) << "," +// << "," +// << kuka_device_time(kuka_index) <<"," +// << "," +// << kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index) - kuka_diff +// << std::endl; +// kuka_index++; +// } else if( kuka_local_receive_time(kuka_index) > FT_local_receive_time(FT_index)) { +// // If the row value is the FT time, then the kuka cells should be left blank. +// fs << FT_local_receive_time(FT_index) <<"," +// << FT_local_request_time(FT_index) << "," +// <<"," +// << FT_device_time(FT_index) << "," +// << "," +// << FT_device_time(FT_index) - FT_local_receive_time(FT_index) - FT_diff << "," +// << std::endl; +// FT_index++; +// } else { +// // In case the time is extactly equivent with each other. +// fs << FT_local_receive_time(FT_index) <<"," +// << FT_local_request_time(FT_index) << "," +// << kuka_local_request_time(kuka_index) << "," +// << FT_device_time(FT_index) << "," +// << kuka_device_time(kuka_index) <<"," +// << FT_device_time(FT_index) - FT_local_receive_time(FT_index) - FT_diff << "," +// << kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index) - kuka_diff +// << std::endl; +// FT_index++; +// kuka_index++; +// } +// } +// fs.close(); +// } + /// Based on the RBDy and URDF model, get the cartesian pose of the end effector. /// BE CAREFUL THAT THE URDF MODEL HAS BEEN MODIFIED, THE MARKER LINK HAS BEEN ADDED. /// @@ -614,98 +805,59 @@ namespace grl { return std::move(strRobot); } -class CSVRow -{ - public: - std::string const& operator[](std::size_t index) const - { - return m_data[index]; - } - std::size_t size() const - { - return m_data.size(); - } - void readNextRow(std::istream& str) - { - std::string line; - std::getline(str, line); - std::stringstream lineStream(line); - std::string cell; +int getRowsNumber(std::string filename){ + // std::ifstream file("/home/cjiao1/src/V-REP_PRO_EDU_V3_4_0_Linux/2018_03_20_20_35_20/KUKA_Joint.csv"); + std::ifstream file(filename); + std::size_t row_size = 0; + + if (!file.is_open()) { + std::cout << "failed to open " << filename << '\n'; + } else { + grl::CSVIterator loop(file); + + for(grl::CSVIterator loop(file); loop != grl::CSVIterator(); ++loop) + { + row_size++; + } + } + return row_size; - m_data.clear(); - while(std::getline(lineStream, cell, ',')) - { - m_data.push_back(cell); - } - // This checks for a trailing comma with no data after it. - if (!lineStream && cell.empty()) - { - // If there was a trailing comma then add an empty element. - m_data.push_back(""); - } - } - private: - std::vector m_data; -}; - -std::istream& operator>>(std::istream& str, CSVRow& data) -{ - data.readNextRow(str); - return str; } -class CSVIterator -{ - public: - typedef std::input_iterator_tag iterator_category; - typedef CSVRow value_type; - typedef std::size_t difference_type; - typedef CSVRow* pointer; - typedef CSVRow& reference; - - CSVIterator(std::istream& str) :m_str(str.good()?&str:NULL) { ++(*this); } - CSVIterator() :m_str(NULL) {} - - // Pre Increment - CSVIterator& operator++() {if (m_str) { if (!((*m_str) >> m_row)){m_str = NULL;}}return *this;} - // Post increment - CSVIterator operator++(int) {CSVIterator tmp(*this);++(*this);return tmp;} - CSVRow const& operator*() const {return m_row;} - CSVRow const* operator->() const {return &m_row;} - - bool operator==(CSVIterator const& rhs) {return ((this == &rhs) || ((this->m_str == NULL) && (rhs.m_str == NULL)));} - bool operator!=(CSVIterator const& rhs) {return !((*this) == rhs);} - private: - std::istream* m_str; - CSVRow m_row; -}; - /// Get the joint angles at specific time point (index) - /// @return jointPosition, Eigen vector which contains joint position of the seven joints. - Eigen::VectorXf getJointAnglesFromCSV(std::string filename, int rowIdx){ + int getJointAnglesFromCSV(std::string filename, Eigen::VectorXf &jointPosition, int rowIdx){ - // std::ifstream file("/home/chunting/src/V-REP_PRO_EDU_V3_4_0_Linux/2018_03_20_20_35_20/KUKA_Joint.csv"); + // std::ifstream file("/home/cjiao1/src/V-REP_PRO_EDU_V3_4_0_Linux/2018_03_20_20_35_20/KUKA_Joint.csv"); + // int rowNum = getRowsNumber(filename); std::ifstream file(filename); - grl::CSVIterator loop(file); std::size_t joint_size = 7; - Eigen::VectorXf jointPosition(joint_size); - int row = 0; - - for(grl::CSVIterator loop(file); loop != grl::CSVIterator(); ++loop) - { - - if(row == rowIdx){ - for(int joint_index = 0; joint_index((*loop)[0]); + } + row++; } - } - row++; } - return jointPosition; + return -1; + } /// Get the joint angles at specific time point (index) @@ -714,22 +866,29 @@ class CSVIterator Eigen::VectorXf getMarkerPoseFromCSV(std::string filename, int rowIdx){ std::ifstream file(filename); - grl::CSVIterator loop(file); std::size_t size = 6; Eigen::VectorXf markerpose(size); - int row = 0; - - for(grl::CSVIterator loop(file); loop != grl::CSVIterator(); ++loop) - { - - if(row == rowIdx){ - for(int index = 0; index #include #include #include @@ -197,15 +198,19 @@ void estimateHandEyeScrew(){ detectedObjectPosition[0], detectedObjectPosition[1], detectedObjectPosition[2]); - // Write the hand eye calibration result into a file + // Write the hand eye calibration results into a file auto myFile = boost::filesystem::current_path() /"HandEyeCalibration_Result.txt"; - boost::filesystem::ofstream ofs(myFile/*.native()*/); + boost::filesystem::ofstream ofs(myFile, std::ios_base::app); // boost::archive::text_oarchive ta(ofs); ofs <<"\n\n=========== " + current_date_and_time_string() + " =======================================\n"; - ofs << "Hand Eye Screw Estimate quat wxyz: " << eigenQuat.w() << " "<< eigenQuat.x() << " " << eigenQuat.y() << " " << eigenQuat.z() - << "\ntranslation xyz: " << transformEstimate.translation().x() << " " - << transformEstimate.translation().y() << " " << transformEstimate.translation().z(); - + ofs << "Hand Eye Screw Estimate quat wxyz: " << eigenQuat.w() << ", "<< eigenQuat.x() << ", " << eigenQuat.y() << ", " << eigenQuat.z() + << "\nTranslation xyz: " << transformEstimate.translation().x() << ", " << transformEstimate.translation().y() << ", " << transformEstimate.translation().z() + << "\n\nestimated RobotTipToFiducial: \n" << poseString(transformEstimate); + ofs.close(); + if ( boost::filesystem::exists( myFile )) { + std::cout << "Hand eye calibration result has been writen into " << myFile.string() << std::endl; + } + } /// @brief Will apply the stored estimate to the v-rep simulation value diff --git a/include/grl/vrep/InverseKinematicsVrepPlugin.hpp b/include/grl/vrep/InverseKinematicsVrepPlugin.hpp index 20f5ee3..058a3d4 100644 --- a/include/grl/vrep/InverseKinematicsVrepPlugin.hpp +++ b/include/grl/vrep/InverseKinematicsVrepPlugin.hpp @@ -76,6 +76,12 @@ #include +// boost::filesystem +#include +#include +#include +#include + // FusionTrack Libraries // #include @@ -88,6 +94,7 @@ // https://github.com/jrl-umi3218/mc_rbdyn_urdf #include #include "grl/flatbuffer/kukaiiwaURDF.h" +#include "grl/flatbuffer/CSVIterator.hpp" namespace grl { namespace vrep { @@ -505,14 +512,20 @@ class InverseKinematicsVrepPlugin /// A default transform is saved when construct() was called /// so if no estimate has been found that will be used. void applyEstimate(){ - // Dummy10 in VRep, by default it's at the origin of the world frame. + std::string opticalTrackerDetectedObjectName("Fiducial#22"); std::string robotTipName("RobotFlangeTip"); int opticalTrackerDetectedObjectHandle = simGetObjectHandle(opticalTrackerDetectedObjectName.c_str()); int robotTipHandle = simGetObjectHandle(robotTipName.c_str()); Eigen::Affine3d transformEstimate = getObjectTransform(opticalTrackerDetectedObjectHandle, robotTipHandle); - Eigen::Quaterniond eigenQuat(0.105045, 0.0502289 , 0.654843 , 0.746742); //wxyz - Eigen::Vector3d eigenPos(0.0754499, 0.0975167, 0.0882869); + Eigen::Quaterniond eigenQuat(0.105064, 0.0503505, 0.65479, 0.746777); //wxyz + // 0.0251104, 0.00752018, 0.996255, 0.0823903 + // 0.105045, 0.0502289 , 0.654843 , 0.746742 + + Eigen::Vector3d eigenPos(0.0753178, 0.0973988, 0.088042); + // 0.0754499, 0.0975167, 0.0882869 + // 0.0388255, 0.102084, 0.0902271 + transformEstimate = eigenQuat; @@ -522,32 +535,40 @@ void applyEstimate(){ auto transform = getObjectTransform(opticalTrackerDetectedObjectHandle, robotTipHandle); std::cout<< "Before resetting the hand eye calibration: " << std::endl << transform.matrix() << std::endl; setObjectTransform(opticalTrackerDetectedObjectHandle, robotTipHandle, transformEstimate); + // auto myFile = boost::filesystem::current_path() /"RoboneSimulation_private_calibration_2.ttt"; + std::string sceneName = "/home/cjiao1/src/robonetracker/modules/roboneprivate/data/RoboneSimulation_private_calibration_2.ttt"; + simSaveScene(sceneName.c_str()); transform = getObjectTransform(opticalTrackerDetectedObjectHandle, robotTipHandle); - std::cout<< "After resetting the hand eye calibration: " << std::endl << transform.matrix() << std::endl; + std::cout<< "After resetting the hand eye calibration: " << std::endl << transform.matrix() << std::endl; // logger_->info( "Hand Eye Screw Estimation has been set quat wxyz\n: {} , {} , {} , {} \n translation xyz: {} {} {}", // eigenQuat.w(), eigenQuat.x(), eigenQuat.y(),eigenQuat.z(), transformEstimate.translation().x(), transformEstimate.translation().y(), transformEstimate.translation().z()); } - void getPoseFromCSV(std::string filename){ +void getPoseFromCSV(std::string filename, int time_index){ // we only have one robot for the moment so the index of it is 0 // loadFromBinary(); - if(time_index == 0){ - applyEstimate(); - } + // if(time_index == 0){ + // applyEstimate(); + // } const std::size_t simulatedRobotIndex = 0; auto& simArmMultiBody = rbd_mbs_[simulatedRobotIndex]; auto& simArmConfig = rbd_mbcs_[simulatedRobotIndex]; - auto joint_angles = grl::getJointAnglesFromCSV(filename, time_index++); + Eigen::VectorXf joint_angles = Eigen::VectorXf::Zero(7); + // std::cout << "\nBefore: " << joint_angles << std::endl; + int timeStamp = grl::getJointAnglesFromCSV(filename, joint_angles, time_index); + assert(timeStamp != -1 ); std::vector vrepJointAngles; double angle = 0; - int jointIdx=0; + int jointIdx = 0; + // std::cout << "Time: " << timeStamp << std::endl; for(auto i : jointHandles_) { angle = joint_angles[jointIdx++]; simSetJointPosition(i,angle); vrepJointAngles.push_back(angle); - std::cout << angle << std::endl; + // std::cout << angle << " "; } + // std::cout << std::endl; // Set RBDyn joint angles from vrep joint angles. SetRBDynArmFromVrep(jointNames_,jointHandles_,simArmMultiBody,simArmConfig); @@ -557,30 +578,61 @@ void applyEstimate(){ debugFrames(); - } - - void replayMarker(std::string filename, int time_index){ - auto markerPose = grl::getMarkerPoseFromCSV(filename, time_index); - Eigen::Vector3d eigenPos(markerPose[0], markerPose[1], markerPose[2]); - Eigen::Quaternionf eigenQuat; - eigenQuat = Eigen::AngleAxisf(markerPose[3], Eigen::Vector3f::UnitX()) - * Eigen::AngleAxisf(markerPose[4], Eigen::Vector3f::UnitY()) - * Eigen::AngleAxisf(markerPose[5], Eigen::Vector3f::UnitZ()); - - std::string markerPoseFromTracker("Dummy20"); - int markerPoseHandle = simGetObjectHandle(markerPoseFromTracker.c_str()); - + std::string opticalTrackerDetectedObjectName("Fiducial#22"); + int opticalTrackerDetectedObjectHandle = simGetObjectHandle(opticalTrackerDetectedObjectName.c_str()); std::string OpticalTrackerBase("OpticalTrackerBase#0"); int OpticalTrackerBaseHandle = simGetObjectHandle(OpticalTrackerBase.c_str()); + // get fiducial in optical tracker base frame + Eigen::Affine3d transformEstimate = getObjectTransform(opticalTrackerDetectedObjectHandle, OpticalTrackerBaseHandle); + Eigen::VectorXd markerPose = grl::Affine3fToMarkerPose(transformEstimate.cast()); + Eigen::RowVectorXd pose = grl::getPluckerPose(markerPose); + + + // // Write the hand eye calibration results into a file + std::string suffix = "ForwardKinematics_Pose.csv"; + auto myFile = boost::filesystem::current_path() /suffix; + // if(boost::filesystem::exists(myFile)){ + // boost::filesystem::remove(myFile); + // } + boost::filesystem::ofstream ofs(myFile, std::ios_base::app); + if(time_index == 0) { + ofs << "local_receive_time_offset_X, K_X,K_Y,K_Z,K_A,K_B,K_C\n"; + } + ofs << timeStamp << ", " << pose[0] << ", " << pose[1] << ", "<< pose[2] << ", "<< pose[3] << ", "<< pose[4] << ", "<< pose[5] << "\n"; + ofs.close(); + + + + - Eigen::Affine3d transformEstimate = getObjectTransform(markerPoseHandle, OpticalTrackerBaseHandle); - transformEstimate = eigenQuat.cast(); - transformEstimate.translation() = eigenPos; + } - // set transform between end effector and fiducial - setObjectTransform(markerPoseHandle, OpticalTrackerBaseHandle, transformEstimate); - logger_->info( "MarkerPose has been set quat wxyz\n: {} , {} , {} , {} \n translation xyz: {} {} {}", - eigenQuat.w(), eigenQuat.x(), eigenQuat.y(),eigenQuat.z(), transformEstimate.translation().x(), transformEstimate.translation().y(), transformEstimate.translation().z()); + void replayMarker(std::string filename, int time_index){ + int rowIdx = time_index*10; + //for(int i=rowIdx; i(); + transformEstimate.translation() = eigenPos; + + // set transform between end effector and fiducial + setObjectTransform(markerPoseHandle, OpticalTrackerBaseHandle, transformEstimate); + // logger_->info( "MarkerPose has been set quat wxyz\n: {} , {} , {} , {} \n translation xyz: {} {} {}", + // eigenQuat.w(), eigenQuat.x(), eigenQuat.y(),eigenQuat.z(), transformEstimate.translation().x(), transformEstimate.translation().y(), transformEstimate.translation().z()); + + //} } @@ -615,35 +667,7 @@ void applyEstimate(){ debugFrames(); } - void testURDFPose(){ - /// simulation tick time step in float seconds from vrep API call - float simulationTimeStep = simGetSimulationTimeStep(); - - // we only have one robot for the moment so the index of it is 0 - const std::size_t simulatedRobotIndex = 0; - auto& simArmMultiBody = rbd_mbs_[simulatedRobotIndex]; - auto& simArmConfig = rbd_mbcs_[simulatedRobotIndex]; - - std::vector vrepJointAngles; - double angle = 0.7; - for(auto i : jointHandles_) - { - angle *= -1; - simSetJointPosition(i,angle); - vrepJointAngles.push_back(angle); - } - - /// @todo TODO(ahundt) rethink where/when/how to send command for the joint angles. Return to LUA? Set Directly? Distribute via vrep send message command? - - //////////////////////////////////////////////////// - // Set joints to current arm position in simulation - SetRBDynArmFromVrep(jointNames_,jointHandles_,simArmMultiBody,simArmConfig); - rbd::forwardKinematics(simArmMultiBody, simArmConfig); - rbd::forwardVelocity(simArmMultiBody, simArmConfig); - - debugFrames(); - } // void replay(){ // loadFromBinary(); @@ -954,12 +978,17 @@ void applyEstimate(){ if(ik) { updateKinematics(); } else { - std::string fileName = "/home/chunting/src/V-REP_PRO_EDU_V3_4_0_Linux/2018_03_20_20_35_20/KUKA_Joint.csv"; + auto myFile = boost::filesystem::current_path() /"2018_03_26_19_23_38"; + std::string pathName = myFile.string(); + std::string kukaJoint = pathName+"/FTKUKA_TimeEvent.csv"; // KUKA_Joint.csv FTKUKA_TimeEvent.csv + std::string markerPose = pathName+"/FT_Pose_Marker22.csv"; + // std::cout << "Call Run One..." << std::endl; // testPose(); // applyEstimate(); - // getPoseFromCSV(fileName); - replayMarker(fileName, time_index); + getPoseFromCSV(kukaJoint, time_index); + // replayMarker(markerPose, time_index); + time_index++; } } diff --git a/src/lua/robone.lua b/src/lua/robone.lua index 281594d..c39a7a2 100644 --- a/src/lua/robone.lua +++ b/src/lua/robone.lua @@ -12,8 +12,8 @@ robone = {} require "grl" -KUKA_single_buffer_limit_bytes = 16 -- MB -FB_single_buffer_limit_bytes = 180 -- MB +KUKA_single_buffer_limit_bytes = 15 -- MB +FB_single_buffer_limit_bytes = 160 -- MB ------------------------------------------ -- Move the arm along the cut file path -- ------------------------------------------ @@ -355,7 +355,7 @@ robone.handEyeCalibScript=function() path=simGetObjectHandle('HandEyeCalibPath') circleCalib = simGetObjectHandle('CircleCalibPath') endeffectorTarget=simGetObjectHandle('RobotMillTipTarget') - numSteps=18 + numSteps=24 startP,startO=grl.getTransformBetweenHandles(target,targetBase) diff --git a/test/KukaPoseEstimationTest.cpp b/test/KukaPoseEstimationTest.cpp index 1400b92..5e68ed7 100644 --- a/test/KukaPoseEstimationTest.cpp +++ b/test/KukaPoseEstimationTest.cpp @@ -59,7 +59,7 @@ const double RadtoDegree = 180/3.14159265359; const double MeterToMM = 1000; -std::string foldname = "/home/chunting/src/V-REP_PRO_EDU_V3_4_0_Linux/"; +std::string foldname = "/home/cjiao1/src/V-REP_PRO_EDU_V3_4_0_Linux/"; std::string foldtimestamp = current_date_and_time_string(); diff --git a/test/readFlatbufferTest.cpp b/test/readFlatbufferTest.cpp index f3558f9..bad5758 100644 --- a/test/readFlatbufferTest.cpp +++ b/test/readFlatbufferTest.cpp @@ -28,9 +28,9 @@ int main(int argc, char* argv[]) { /// Define the file names - std::string foldname = "/home/chunting/src/V-REP_PRO_EDU_V3_4_0_Linux/"; - std::string kukaTimeStamp = "2018_03_20_19_36_47_Kukaiiwa"; - std::string FTTimeStamp = "2018_03_20_19_37_08_FusionTrack"; + std::string foldname = "/home/cjiao1/src/V-REP_PRO_EDU_V3_4_0_Linux/"; + std::string kukaTimeStamp = "2018_03_26_19_06_21_Kukaiiwa"; + std::string FTTimeStamp = "2018_03_26_19_06_21_FusionTrack"; std::string kukaBinaryfile = foldname + kukaTimeStamp+ ".iiwa"; std::string fusiontrackBinaryfile = foldname + FTTimeStamp +".flik"; std::string foldtimestamp = current_date_and_time_string(); @@ -62,16 +62,17 @@ int main(int argc, char* argv[]) // Put all the data into the same coordinate system --- Marker frame bool inMarkerFrame = false; // Indicate the marker pose is in marker frame or tracker frame. + uint32_t markerID_22 = 22; + uint32_t markerID_55 = 55; + uint32_t markerID_50000 = 50000; /// Write FT data to CSV fbs_tk::Root logKUKAiiwaFusionTrackP = fbs_tk::open_root(fusiontrackBinaryfile); - grl::MatrixXd timeEventM_FT = grl::getTimeStamp(logKUKAiiwaFusionTrackP, grl::fusiontracker_tag()); + grl::MatrixXd timeEventM_FT = grl::getTimeStamp(logKUKAiiwaFusionTrackP, grl::fusiontracker_tag(), markerID_22); grl::regularizeTimeEvent(timeEventM_FT); std::size_t FT_size = grl::getStateSize(logKUKAiiwaFusionTrackP); - uint32_t markerID_22 = 22; - uint32_t markerID_55 = 55; - uint32_t markerID_50000 = 50000; + timeEventM_FT = grl::MatrixXd::Zero(FT_size, timeEventM_FT.cols()); Eigen::MatrixXd markerPose(FT_size, grl::col_Pose); // Get the pose of the marker 22. @@ -153,7 +154,7 @@ int main(int argc, char* argv[]) - // grl::writeFTKUKATimeEventToCSV(FTKUKA_TimeEvent_CSV, logKUKAiiwaFusionTrackP, kukaStatesP); + grl::writeFTKUKATimeEventToCSV(FTKUKA_TimeEvent_CSV, logKUKAiiwaFusionTrackP, kukaStatesP, markerID_22); return 0; } From 8ce0cc64db485490f4924f5c1ee456abedd3ef25 Mon Sep 17 00:00:00 2001 From: Chunting Date: Thu, 29 Mar 2018 23:13:13 -0400 Subject: [PATCH 067/102] Fix bugs --- .../grl/flatbuffer/ParseflatbuffertoCSV.hpp | 310 ++++++++---------- test/readFlatbufferTest.cpp | 73 ++++- 2 files changed, 199 insertions(+), 184 deletions(-) diff --git a/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp b/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp index 336a78f..7acde19 100644 --- a/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp +++ b/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp @@ -129,16 +129,16 @@ namespace grl { bool checkmonotonic( T &time){ for(int i=1; i &logKUKAiiwaFusionTrackP) { - auto states = logKUKAiiwaFusionTrackP->states(); + template + int getStateSize(const T &binaryObjectP) { + auto states = binaryObjectP->states(); return states->size(); } @@ -383,7 +383,7 @@ namespace grl { std::size_t data_rows_size = dataM.rows(); assert(labels_size == cols_size && time_rows_size>0 && cols_size>0 && time_rows_size==data_rows_size); auto time = timeM.col(TimeType::local_receive_time); - assert(checkmonotonic(time)); + // assert(checkmonotonic(time)); // create an ofstream for the file output (see the link on streams for more info) std::ofstream fs; // create a name for the file output @@ -450,6 +450,116 @@ namespace grl { } fs.close(); } + + + /// With the same method to process the time data from both Kuka and fusiontracker, combine it and write to a csv file. + /// @param FTKUKA_TimeEvent_CSVfilename, the csv file name + void writeFTKUKATimeEventToCSV(std::string& FTKUKA_TimeEvent_CSVfilename, + grl::MatrixXd& timeEventM_FT, + grl::MatrixXd& timeEventM_Kuka, + Eigen::MatrixXd& jointAngles, + Eigen::MatrixXd& markerPose, + int FT_index=0, + int kuka_index=0) { + int FT_time_size = timeEventM_FT.rows(); + int kuka_time_size = timeEventM_Kuka.rows(); + grl::VectorXd FT_local_request_time = timeEventM_FT.col(local_request_time); + grl::VectorXd FT_local_receive_time = timeEventM_FT.col(local_receive_time); + grl::VectorXd FT_device_time = timeEventM_FT.col(device_time); + // assert(checkmonotonic(FT_local_receive_time)); // Hit the asserion, should check the time data from FT_index, not from 0 + + grl::VectorXd kuka_local_request_time = timeEventM_Kuka.col(local_request_time); + grl::VectorXd kuka_local_receive_time = timeEventM_Kuka.col(local_receive_time); + grl::VectorXd kuka_device_time = timeEventM_Kuka.col(device_time); + // assert(checkmonotonic(kuka_local_receive_time)); + int64_t kuka_diff = kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index); + int64_t FT_diff = FT_device_time(FT_index) - FT_local_receive_time(FT_index); + + + std::ofstream fs; + // create a name for the file output + fs.open( FTKUKA_TimeEvent_CSVfilename, std::ofstream::out | std::ofstream::app); + // write the file headers + fs << "local_receive_time_offset_X," + << "FT_local_request_time," + << "KUKA_local_request_time," + << "FT_device_time_offset," + << "device_time_offset_kuka," + << "Y_FT," + << "Y_kuka," + << "FT_X," + << "FT_Y," + << "FT_Z," + << "FT_A," + << "FT_B," + << "FT_C," + << "K_Joint1," + << "K_Joint2," + << "K_Joint3," + << "K_Joint4," + << "K_Joint5," + << "K_Joint6," + << "K_Joint7" + << std::endl; + std::cout << "Start to write ... "<< std::endl <<"FT_index: " << FT_index << " kuka_index: " << kuka_index << std::endl; + + while ( kuka_index < kuka_time_size && FT_index < FT_time_size ) + { + // If the row value is the kuka time, then the FT cells should be left blank. + if ( kuka_local_receive_time(kuka_index) < FT_local_receive_time(FT_index) ){ + // write the data to the output file + auto jointrow = jointAngles.row(kuka_index); + fs << kuka_local_receive_time(kuka_index) <<"," + <<"," + << kuka_local_request_time(kuka_index) << "," + << "," + << kuka_device_time(kuka_index) <<"," + << "," + << kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index) - kuka_diff << "," + << ",,,,,," + << jointrow[0] << "," + << jointrow[1] << "," + << jointrow[2] << "," + << jointrow[3] << "," + << jointrow[4] << "," + << jointrow[5] << "," + << jointrow[6] + << std::endl; + kuka_index++; + } else if( kuka_local_receive_time(kuka_index) > FT_local_receive_time(FT_index)) { + auto matrixrow = markerPose.row(FT_index); + // If the row value is the FT time, then the kuka cells should be left blank. + fs << FT_local_receive_time(FT_index) <<"," + << FT_local_request_time(FT_index) << "," + <<"," + << FT_device_time(FT_index) << "," + << "," + << FT_device_time(FT_index) - FT_local_receive_time(FT_index) - FT_diff << "," + << "," + << matrixrow[0] << "," + << matrixrow[1] << "," + << matrixrow[2] << "," + << matrixrow[3] << "," + << matrixrow[4] << "," + << matrixrow[5] << std::endl; + FT_index++; + } else { + // In case the time is extactly equivent with each other. + fs << FT_local_receive_time(FT_index) <<"," + << FT_local_request_time(FT_index) << "," + << kuka_local_request_time(kuka_index) << "," + << FT_device_time(FT_index) << "," + << kuka_device_time(kuka_index) <<"," + << FT_device_time(FT_index) - FT_local_receive_time(FT_index) - FT_diff << "," + << kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index) - kuka_diff + << std::endl; + FT_index++; + kuka_index++; + } + } + std::cout << "FT_index: " << FT_index << " kuka_index: " << kuka_index << " Sum: " << FT_index+kuka_index << std::endl; + fs.close(); +} /// With the same method to process the time data from both Kuka and fusiontracker, combine it and write to a csv file. /// @param FTKUKA_TimeEvent_CSVfilename, the csv file name void writeFTKUKATimeEventToCSV(std::string& FTKUKA_TimeEvent_CSVfilename, @@ -459,18 +569,15 @@ namespace grl { grl::MatrixXd timeEventM_FT = grl::getTimeStamp(logKUKAiiwaFusionTrackP, grl::fusiontracker_tag(), markerID); grl::MatrixXd timeEventM_Kuka = grl::getTimeStamp(kukaStatesP, grl::kuka_tag()); - std::size_t kuka_state_size = timeEventM_Kuka.rows(); - std::cout<< "------Kuka_state_size: "<< timeEventM_Kuka.rows() << std::endl; - std::size_t FT_state_size = timeEventM_FT.rows(); - std::cout<< "------FT_state_size: "<< timeEventM_FT.rows() << std::endl; - - /////////////////////////////////////////////////////////////////////////////////// - - std::size_t FT_size = grl::getStateSize(logKUKAiiwaFusionTrackP); - - + std::size_t kuka_size = grl::getStateSize(kukaStatesP); + + std::cout<< "------Kuka_size: "<< kuka_size << std::endl; + std::cout<< "------FT_size: "<< FT_size << std::endl; + + std::size_t FT_time_size = timeEventM_FT.rows(); + std::size_t kuka_time_size = timeEventM_Kuka.rows(); grl::MatrixXd timeEventM = grl::MatrixXd::Zero(FT_size, timeEventM_FT.cols()); Eigen::MatrixXd markerPose(FT_size, grl::col_Pose); // Get the pose of the marker 22. @@ -485,29 +592,33 @@ namespace grl { int kuka_index = 0; int FT_index = 0; - // Filter out the very beginning data,since the tracker starts to work once the scene is loaded in vrep. + // Skip the very beginning data,since the tracker starts to work once the scene is loaded in vrep. // But kuka starts to work only after clicking on the start button. - // To combine the time from two devices, they should share the same starting time point. - while(kuka_index timeEventM_FT(FT_index,local_receive_time) && kuka_index == 0){ + while(FT_index timeEventM_FT(FT_index,local_receive_time) && kuka_index == 0){ FT_index++; } auto initial_local_time = std::min(timeEventM_FT(FT_index,local_receive_time), timeEventM_Kuka(kuka_index,local_receive_time)); auto initial_device_time_kuka = timeEventM_Kuka(kuka_index,device_time); auto initial_device_time_FT = timeEventM_FT(FT_index,device_time); - grl::VectorXd FT_local_request_time = timeEventM_FT.col(local_request_time) - initial_local_time * grl::VectorXd::Ones(FT_state_size); - grl::VectorXd FT_local_receive_time = timeEventM_FT.col(local_receive_time) - initial_local_time * grl::VectorXd::Ones(FT_state_size); - grl::VectorXd FT_device_time = timeEventM_FT.col(device_time) - initial_device_time_FT * grl::VectorXd::Ones(FT_state_size); + grl::VectorXd FT_local_request_time = timeEventM_FT.col(local_request_time) - initial_local_time * grl::VectorXd::Ones(FT_time_size); + grl::VectorXd FT_local_receive_time = timeEventM_FT.col(local_receive_time) - initial_local_time * grl::VectorXd::Ones(FT_time_size); + grl::VectorXd FT_device_time = timeEventM_FT.col(device_time) - initial_device_time_FT * grl::VectorXd::Ones(FT_time_size); - grl::VectorXd kuka_local_request_time = timeEventM_Kuka.col(local_request_time) - initial_local_time * grl::VectorXd::Ones(kuka_state_size); - grl::VectorXd kuka_local_receive_time = timeEventM_Kuka.col(local_receive_time) - initial_local_time * grl::VectorXd::Ones(kuka_state_size); - grl::VectorXd kuka_device_time = timeEventM_Kuka.col(device_time) - initial_device_time_kuka * grl::VectorXd::Ones(kuka_state_size); + grl::VectorXd kuka_local_request_time = timeEventM_Kuka.col(local_request_time) - initial_local_time * grl::VectorXd::Ones(kuka_time_size); + grl::VectorXd kuka_local_receive_time = timeEventM_Kuka.col(local_receive_time) - initial_local_time * grl::VectorXd::Ones(kuka_time_size); + grl::VectorXd kuka_device_time = timeEventM_Kuka.col(device_time) - initial_device_time_kuka * grl::VectorXd::Ones(kuka_time_size); Eigen::MatrixXd jointAngles = grl::getAllJointAngles(kukaStatesP); + + int64_t kuka_diff = kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index); + int64_t FT_diff = FT_device_time(FT_index) - FT_local_receive_time(FT_index); + std::ofstream fs; // create a name for the file output fs.open( FTKUKA_TimeEvent_CSVfilename, std::ofstream::out | std::ofstream::app); @@ -534,9 +645,8 @@ namespace grl { << "K_Joint7" << std::endl; std::cout << "Start to write ... "<< std::endl <<"FT_index: " << FT_index << " kuka_index: " << kuka_index << std::endl; - int64_t kuka_diff = kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index); - int64_t FT_diff = FT_device_time(FT_index) - FT_local_receive_time(FT_index); - while ( kuka_index < kuka_state_size && FT_index < FT_state_size ) + + while ( kuka_index < kuka_time_size && FT_index < FT_time_size ) { // If the row value is the kuka time, then the FT cells should be left blank. if ( kuka_local_receive_time(kuka_index) < FT_local_receive_time(FT_index) ){ @@ -594,150 +704,6 @@ namespace grl { fs.close(); } - - /// With the same method to process the time data from both Kuka and fusiontracker, combine it and write to a csv file. - /// @param FTKUKA_TimeEvent_CSVfilename, the csv file name -// int CombineToCSV(std::string& FTKUKA_TimeEvent_CSVfilename, -// std::string& FT_Marker22_CSVfilename, -// std::string& Kuka_Pose_CSVfilename -// ) { - -// // std::ifstream file("/home/cjiao1/src/V-REP_PRO_EDU_V3_4_0_Linux/2018_03_20_20_35_20/KUKA_Joint.csv"); -// std::ifstream Time_file(FTKUKA_TimeEvent_CSVfilename); -// std::ifstream FT_file(FT_Marker22_CSVfilename); -// std::ifstream KUKA_file(Kuka_Pose_CSVfilename); -// std::size_t joint_size = 7; -// Eigen::VectorXf jointPosition(joint_size); -// if (!Time_file.is_open()) { -// std::cout << "failed to open " << FTKUKA_TimeEvent_CSVfilename << '\n'; -// return -1; -// } -// if (!FT_file.is_open()) { -// std::cout << "failed to open " << FT_Marker22_CSVfilename << '\n'; -// return -1; -// } -// if (!KUKA_file.is_open()) { -// std::cout << "failed to open " << Kuka_Pose_CSVfilename << '\n'; -// return -1; -// } - -// grl::CSVIterator Time_loop(Time_file); -// grl::CSVIterator FT_loop(FT_file); -// grl::CSVIterator Kuka_loop(_file); - -// int row = 0; - -// for(grl::CSVIterator loop(file); loop != grl::CSVIterator(); ++loop) -// { - -// if(row == rowIdx){ -// for(int joint_index = 0; joint_index timeEventM_FT(FT_index,local_receive_time) && kuka_index == 0){ -// FT_index++; -// } - -// auto initial_local_time = std::min(timeEventM_FT(FT_index,local_receive_time), timeEventM_Kuka(kuka_index,local_receive_time)); -// auto initial_device_time_kuka = timeEventM_Kuka(kuka_index,device_time); -// auto initial_device_time_FT = timeEventM_FT(FT_index,device_time); -// grl::VectorXd FT_local_request_time = timeEventM_FT.col(local_request_time) - initial_local_time * grl::VectorXd::Ones(FT_state_size); -// grl::VectorXd FT_local_receive_time = timeEventM_FT.col(local_receive_time) - initial_local_time * grl::VectorXd::Ones(FT_state_size); -// grl::VectorXd FT_device_time = timeEventM_FT.col(device_time) - initial_device_time_FT * grl::VectorXd::Ones(FT_state_size); - -// grl::VectorXd kuka_local_request_time = timeEventM_Kuka.col(local_request_time) - initial_local_time * grl::VectorXd::Ones(kuka_state_size); -// grl::VectorXd kuka_local_receive_time = timeEventM_Kuka.col(local_receive_time) - initial_local_time * grl::VectorXd::Ones(kuka_state_size); -// grl::VectorXd kuka_device_time = timeEventM_Kuka.col(device_time) - initial_device_time_kuka * grl::VectorXd::Ones(kuka_state_size); - - -// std::ofstream fs; -// // create a name for the file output -// fs.open( FTKUKA_TimeEvent_CSVfilename, std::ofstream::out | std::ofstream::app); -// // write the file headers -// fs << "local_receive_time_offset_X," -// << "FT_local_request_time," -// << "KUKA_local_request_time," -// << "FT_device_time_offset," -// << "device_time_offset_kuka," -// << "Y_FT," -// << "Y_kuka" -// << std::endl; - -// int64_t kuka_diff = kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index); -// int64_t FT_diff = FT_device_time(FT_index) - FT_local_receive_time(FT_index); -// while ( kuka_index < kuka_state_size && FT_index < FT_state_size ) -// { -// // If the row value is the kuka time, then the FT cells should be left blank. -// if ( kuka_local_receive_time(kuka_index) < FT_local_receive_time(FT_index) ){ -// // write the data to the output file -// fs << kuka_local_receive_time(kuka_index) <<"," -// <<"," -// << kuka_local_request_time(kuka_index) << "," -// << "," -// << kuka_device_time(kuka_index) <<"," -// << "," -// << kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index) - kuka_diff -// << std::endl; -// kuka_index++; -// } else if( kuka_local_receive_time(kuka_index) > FT_local_receive_time(FT_index)) { -// // If the row value is the FT time, then the kuka cells should be left blank. -// fs << FT_local_receive_time(FT_index) <<"," -// << FT_local_request_time(FT_index) << "," -// <<"," -// << FT_device_time(FT_index) << "," -// << "," -// << FT_device_time(FT_index) - FT_local_receive_time(FT_index) - FT_diff << "," -// << std::endl; -// FT_index++; -// } else { -// // In case the time is extactly equivent with each other. -// fs << FT_local_receive_time(FT_index) <<"," -// << FT_local_request_time(FT_index) << "," -// << kuka_local_request_time(kuka_index) << "," -// << FT_device_time(FT_index) << "," -// << kuka_device_time(kuka_index) <<"," -// << FT_device_time(FT_index) - FT_local_receive_time(FT_index) - FT_diff << "," -// << kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index) - kuka_diff -// << std::endl; -// FT_index++; -// kuka_index++; -// } -// } -// fs.close(); -// } /// Based on the RBDy and URDF model, get the cartesian pose of the end effector. /// BE CAREFUL THAT THE URDF MODEL HAS BEEN MODIFIED, THE MARKER LINK HAS BEEN ADDED. diff --git a/test/readFlatbufferTest.cpp b/test/readFlatbufferTest.cpp index bad5758..d62298d 100644 --- a/test/readFlatbufferTest.cpp +++ b/test/readFlatbufferTest.cpp @@ -67,22 +67,71 @@ int main(int argc, char* argv[]) uint32_t markerID_50000 = 50000; /// Write FT data to CSV fbs_tk::Root logKUKAiiwaFusionTrackP = fbs_tk::open_root(fusiontrackBinaryfile); - grl::MatrixXd timeEventM_FT = grl::getTimeStamp(logKUKAiiwaFusionTrackP, grl::fusiontracker_tag(), markerID_22); - grl::regularizeTimeEvent(timeEventM_FT); - + fbs_tk::Root kukaStatesP = fbs_tk::open_root(kukaBinaryfile); std::size_t FT_size = grl::getStateSize(logKUKAiiwaFusionTrackP); + std::size_t kuka_size = grl::getStateSize(kukaStatesP); + Eigen::MatrixXd markerPose(FT_size, grl::col_Pose); + // grl::MatrixXd timeEventM_FT = grl::getTimeStamp(logKUKAiiwaFusionTrackP, grl::fusiontracker_tag(), markerID_22); + + grl::MatrixXd timeEventM_FT = grl::MatrixXd::Zero(FT_size, grl::Time_Labels.size()); + grl::MatrixXd timeEventM_Kuka = grl::MatrixXd::Zero(kuka_size, grl::Time_Labels.size()); + timeEventM_Kuka = grl::getTimeStamp(kukaStatesP,grl::kuka_tag()); + int validsize_22 = grl::getMarkerPose(logKUKAiiwaFusionTrackP, markerID_22, timeEventM_FT, markerPose); + std::cout<<"validsize_22: " << validsize_22 << std::endl; + + + + Eigen::MatrixXd jointAngles = grl::getAllJointAngles(kukaStatesP); + std::cout<< "------Kuka_size: "<< kuka_size << std::endl; + std::cout<< "------FT_size: "<< FT_size << std::endl; + std::size_t FT_time_size = timeEventM_FT.rows(); + std::size_t kuka_time_size = timeEventM_Kuka.rows(); + grl::MatrixXd timeEventM = grl::MatrixXd::Zero(FT_size, timeEventM_FT.cols()); + + + + int kuka_index = 0; + int FT_index = 0; + + // Skip the very beginning data,since the tracker starts to work once the scene is loaded in vrep. + // But kuka starts to work only after clicking on the start button. + // To combine the time from two devices, they should have the same starting time point. + while(kuka_index timeEventM_FT(FT_index,grl::TimeType::local_receive_time) && kuka_index == 0){ + FT_index++; + } + + auto initial_local_time = std::min(timeEventM_FT(FT_index,grl::TimeType::local_receive_time), timeEventM_Kuka(kuka_index, grl::TimeType::local_receive_time)); + auto initial_device_time_kuka = timeEventM_Kuka(kuka_index,grl::TimeType::device_time); + auto initial_device_time_FT = timeEventM_FT(FT_index,grl::TimeType::device_time); + timeEventM_FT.col(grl::TimeType::local_request_time) = timeEventM_FT.col(grl::TimeType::local_request_time) - initial_local_time * grl::VectorXd::Ones(FT_time_size); + timeEventM_FT.col(grl::TimeType::local_receive_time) = timeEventM_FT.col(grl::TimeType::local_receive_time) - initial_local_time * grl::VectorXd::Ones(FT_time_size); + timeEventM_FT.col(grl::TimeType::device_time) = timeEventM_FT.col(grl::TimeType::device_time) - initial_device_time_FT * grl::VectorXd::Ones(FT_time_size); + + timeEventM_Kuka.col(grl::TimeType::local_request_time) = timeEventM_Kuka.col(grl::TimeType::local_request_time) - initial_local_time * grl::VectorXd::Ones(kuka_time_size); + timeEventM_Kuka.col(grl::TimeType::local_receive_time) = timeEventM_Kuka.col(grl::TimeType::local_receive_time) - initial_local_time * grl::VectorXd::Ones(kuka_time_size); + timeEventM_Kuka.col(grl::TimeType::device_time) = timeEventM_Kuka.col(grl::TimeType::device_time) - initial_device_time_kuka * grl::VectorXd::Ones(kuka_time_size); + + + + // grl::regularizeTimeEvent(timeEventM_FT); + + + // timeEventM_FT = grl::MatrixXd::Zero(FT_size, timeEventM_FT.cols()); - timeEventM_FT = grl::MatrixXd::Zero(FT_size, timeEventM_FT.cols()); - Eigen::MatrixXd markerPose(FT_size, grl::col_Pose); // Get the pose of the marker 22. // Since the bad data is skipped, the timeEventM_FT also needs to be filled out with the orinial time stamp. - int validsize_22 = grl::getMarkerPose(logKUKAiiwaFusionTrackP, markerID_22, timeEventM_FT, markerPose, inMarkerFrame); - std::cout<<"validsize_22: " << validsize_22 << std::endl; + std::vector FT_Labels_Pose = grl::getLabels(grl::LabelsType::FT_Pose); + + grl::writeFTKUKATimeEventToCSV(FTKUKA_TimeEvent_CSV, + timeEventM_FT, timeEventM_Kuka, jointAngles, markerPose, FT_index, kuka_index); if(validsize_22>2){ - grl::regularizeTimeEvent(timeEventM_FT); + // grl::regularizeTimeEvent(timeEventM_FT); grl::writeTimeEventToCSV(FT_TimeEvent_CSV, timeEventM_FT); grl::writeMatrixToCSV(FT_Marker22_CSV, FT_Labels_Pose, timeEventM_FT, markerPose); @@ -112,11 +161,11 @@ int main(int argc, char* argv[]) ///////////////////////////////////////////////////////////////////////////////////////////////////// /// Write KUKA data to CSV //////////////////////////////////////////////////////////////////////////////////////////////////// - fbs_tk::Root kukaStatesP = fbs_tk::open_root(kukaBinaryfile); - grl::MatrixXd timeEventM_Kuka = grl::getTimeStamp(kukaStatesP,grl::kuka_tag()); + + grl::regularizeTimeEvent(timeEventM_Kuka); grl::writeTimeEventToCSV(KUKA_TimeEvent_CSV, timeEventM_Kuka); - Eigen::MatrixXd jointAngles = grl::getAllJointAngles(kukaStatesP); + std::vector Kuka_Joint_Labels = grl::getLabels(grl::LabelsType::Joint); if(jointAngles.rows() == timeEventM_Kuka.rows()){ @@ -154,7 +203,7 @@ int main(int argc, char* argv[]) - grl::writeFTKUKATimeEventToCSV(FTKUKA_TimeEvent_CSV, logKUKAiiwaFusionTrackP, kukaStatesP, markerID_22); + // grl::writeFTKUKATimeEventToCSV(FTKUKA_TimeEvent_CSV, logKUKAiiwaFusionTrackP, kukaStatesP, markerID_22); return 0; } From 88925ee9cb7c5268f741392fd01f1716cd72c75a Mon Sep 17 00:00:00 2001 From: Chunting Date: Fri, 30 Mar 2018 17:10:48 -0400 Subject: [PATCH 068/102] Reorganize the CSV files, make the time baseline consistent --- .../grl/flatbuffer/ParseflatbuffertoCSV.hpp | 334 ++++++++++++++++-- test/readFlatbufferTest.cpp | 163 +++++---- 2 files changed, 394 insertions(+), 103 deletions(-) diff --git a/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp b/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp index 7acde19..347e8fa 100644 --- a/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp +++ b/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp @@ -89,15 +89,27 @@ namespace grl { std::vector Time_Labels = {"local_receive_time_X", "local_request_time_offset", "device_time_offset", "time_Y", "counter"}; // std::vector PK_Pose_Labels = {"P_X", "P_Y", "P_Z", "Q_X", "Q_Y", "Q_Z", "Q_W"}; + std::vector M_Pos_Joint_Labels = {"M_Pos_J1", "M_Pos_J2", "M_Pos_J3", "M_Pos_J4", "M_Pos_J5", "M_Pos_J6", "M_Pos_J7"}; + std::vector M_Tor_Joint_Labels = {"M_Tor_J1", "M_Tor_J2", "M_Tor_J3", "M_Tor_J4", "M_Tor_J5", "M_Tor_J6", "M_Tor_J7"}; + std::vector C_Pos_Joint_Labels = {"C_Pos_J1", "C_Pos_J2", "C_Pos_J3", "C_Pos_J4", "C_Pos_J5", "C_Pos_J6", "C_Pos_J7"}; + std::vector C_Tor_Joint_Labels = {"C_Tor_J1", "C_Tor_J2", "C_Tor_J3", "C_Tor_J4", "C_Tor_J5", "C_Tor_J6", "C_Tor_J7"}; + std::vector E_Tor_Joint_Labels = {"E_Tor_J1", "E_Tor_J2", "E_Tor_J3", "E_Tor_J4", "E_Tor_J5", "E_Tor_J6", "E_Tor_J7"}; + std::vector IPO_Joint_Labels = {"IPO_J1", "IPO_J2", "IPO_J3", "IPO_J4", "IPO_J5", "IPO_J6", "IPO_J7"}; std::vector Joint_Labels = {"Joint_1", "Joint_2", "Joint_3", "Joint_4", "Joint_5", "Joint_6", "Joint_7"}; + std::vector Transform_Labels = {"X", "Y", "Z", "Q_W", "Q_X", "Q_Y", "Q_Z"}; std::vector PK_Pose_Labels = {"X", "Y", "Z", "A", "B", "C"}; + enum TimeType { local_receive_time = 0, local_request_time = 1, device_time = 2, time_Y = 3, counterIdx=4}; + enum Joint_Index { joint_1, joint_2, joint_3, joint_4, joint_5, joint_6, joint_7}; + enum LabelsType { FT_Pose = 0, Joint = 1, Kuka_Pose= 2}; + struct kuka_tag {}; struct fusiontracker_tag {}; int col_timeEvent=Time_Labels.size(); int col_Kuka_Joint = Joint_Labels.size(); + const static int jointNum = 7; int col_Pose = PK_Pose_Labels.size(); - enum TimeType { local_receive_time = 0, local_request_time = 1, device_time = 2, time_Y = 3, counterIdx=4}; - enum LabelsType { FT_Pose = 0, Joint = 1, Kuka_Pose= 2}; + + @@ -229,18 +241,17 @@ namespace grl { /// Process the time data to get the time offset based on the starting time point. /// See https://github.com/facontidavide/PlotJuggler/issues/68 /// @param timeEventM timeEvent matrix which should have four columns - void regularizeTimeEvent(grl::MatrixXd& timeEventM){ + void regularizeTimeEvent(grl::MatrixXd& timeEventM, + int64_t initial_local_time, + int64_t initial_device_time, + int rowIndex){ + std::cout<< "initial_local_time: " << initial_local_time << std::endl; std::size_t time_size = timeEventM.rows(); assert(time_size>0); - auto initial_local_time = timeEventM(0,TimeType::local_receive_time); - auto initial_device_time = timeEventM(0,TimeType::device_time); - timeEventM.col(TimeType::local_receive_time) = timeEventM.col(TimeType::local_receive_time) - initial_local_time * grl::VectorXd::Ones(time_size); - timeEventM.col(TimeType::local_request_time) = timeEventM.col(TimeType::local_request_time) - initial_local_time * grl::VectorXd::Ones(time_size); - - timeEventM.col(TimeType::device_time) = timeEventM.col(TimeType::device_time) - initial_device_time * grl::VectorXd::Ones(time_size); - timeEventM.col(TimeType::time_Y) = timeEventM.col(TimeType::device_time) - timeEventM.col(TimeType::local_receive_time); - grl::VectorXd timeCol = timeEventM.col(TimeType::local_receive_time); - assert(checkmonotonic(timeCol)); + timeEventM.col(grl::TimeType::local_request_time) = timeEventM.col(grl::TimeType::local_request_time) - initial_local_time * grl::VectorXd::Ones(time_size); + timeEventM.col(grl::TimeType::local_receive_time) = timeEventM.col(grl::TimeType::local_receive_time) - initial_local_time * grl::VectorXd::Ones(time_size); + timeEventM.col(grl::TimeType::device_time) = timeEventM.col(grl::TimeType::device_time) - initial_device_time * grl::VectorXd::Ones(time_size); + timeEventM.col(grl::TimeType::time_Y) = timeEventM.col(grl::TimeType::time_Y) - timeEventM(rowIndex, grl::TimeType::time_Y) * grl::VectorXd::Ones(time_size); } @@ -282,6 +293,7 @@ namespace grl { timeEventM(row,TimeType::local_request_time) = timeEvent->local_request_time(); timeEventM(row,TimeType::device_time) = timeEvent->device_time(); timeEventM(row,TimeType::time_Y) = timeEventM(row,TimeType::device_time) - timeEventM(row,TimeType::local_receive_time); + timeEventM(row,TimeType::counterIdx) = counter; auto Pose = marker->transform(); @@ -321,6 +333,8 @@ namespace grl { } } if(row < state_size) { + int64_t FT_diff = timeEventM(0,TimeType::device_time) - timeEventM(0,TimeType::local_receive_time); + timeEventM.col(TimeType::time_Y) = timeEventM.col(TimeType::time_Y) - FT_diff * grl::VectorXd::Ones(timeEventM.rows()); markerPose.conservativeResize(row, Eigen::NoChange_t{}); timeEventM.conservativeResize(row, Eigen::NoChange_t{}); } @@ -329,9 +343,267 @@ namespace grl { << " lossing rate " << diff/state_size << " markerID: " << markerID < &logKUKAiiwaFusionTrackP, + uint32_t &markerID, + grl::MatrixXd& timeEventM_FT, + Eigen::MatrixXd& transform, + int startIndex = 0){ + auto states = logKUKAiiwaFusionTrackP->states(); + std::size_t state_size = states->size(); + assert(state_size>0); + // The first columne is counter + int row = 0; + // Eigen::MatrixXd markerPose(state_size, grl::col_Pose); + // std::cout <<"State size: " << state_size << " markerPose rows: " << markerPose.rows() << std::endl; + int BadCount = 0; + for(int stateIdx = startIndex; startIndexGet(stateIdx); + auto FT_Message = kukaiiwaFusionTrackMessage->deviceState_as_FusionTrackMessage(); + auto FT_Frame = FT_Message->frame(); + auto counter = FT_Frame->counter(); + auto Makers = FT_Frame->markers(); + /// In some frames, there are no markers, where we skip that line. Also we need to remove the corresponding line in TimeEvent. + if(Makers!=nullptr) { + auto makerSize = Makers->size(); + for(int markerIndex=0; markerIndexGet(markerIndex); + auto marker_ID = marker->geometryID(); + if(marker_ID == markerID){ + auto timeEvent = kukaiiwaFusionTrackMessage->timeEvent(); + timeEventM_FT(row,TimeType::local_receive_time) = timeEvent->local_receive_time(); + timeEventM_FT(row,TimeType::local_request_time) = timeEvent->local_request_time(); + timeEventM_FT(row,TimeType::device_time) = timeEvent->device_time(); + timeEventM_FT(row,TimeType::time_Y) = timeEventM_FT(row,TimeType::device_time) - timeEventM_FT(row,TimeType::local_receive_time); + timeEventM_FT(row,TimeType::counterIdx) = counter; + auto Pose = marker->transform(); + auto FB_r = Pose->position(); + auto FB_q = Pose->orientation(); + // Convert the flatbuffer type to Eigen type + // Eigen::Vector3d r(FB_r.x(), FB_r.y(), FB_r.z()); + // Eigen::Quaterniond q(FB_q.w(), FB_q.x(), FB_q.y(), FB_q.z()); + transform.row(row++) << FB_r.x(), FB_r.y(), FB_r.z(), FB_q.w(), FB_q.x(), FB_q.y(), FB_q.z(); + break; + } + } + } else { + BadCount++; + } + } + if(row < state_size) { + int64_t FT_diff = timeEventM_FT(0,TimeType::device_time) - timeEventM_FT(0,TimeType::local_receive_time); + timeEventM_FT.col(TimeType::time_Y) = timeEventM_FT.col(TimeType::time_Y) - FT_diff * grl::VectorXd::Ones(timeEventM_FT.rows()); + transform.conservativeResize(row, Eigen::NoChange_t{}); + timeEventM_FT.conservativeResize(row, Eigen::NoChange_t{}); + } + double diff = static_cast(state_size-transform.rows()); + std::cout <<"State size: " << state_size <<" markerPose size: " << transform.rows() + << " lossing rate " << diff/state_size << " markerID: " << markerID < &kukaStatesP, + grl::MatrixXd& timeEventM_Kuka, + Eigen::VectorXd& sequenceCounterVec, + Eigen::VectorXd& reflectedSequenceCounterVec, + Eigen::MatrixXd& measuredJointPosition, + Eigen::MatrixXd& measuredTorque, + Eigen::MatrixXd& commandedJointPosition, + Eigen::MatrixXd& commandedTorque, + Eigen::MatrixXd& externalTorque, + Eigen::MatrixXd& jointStateInterpolated, + int startIndex = 0){ + + auto states = kukaStatesP->states(); + std::size_t state_size = states->size(); + Eigen::VectorXf measurdjointPosVec(state_size); + if(startIndex>state_size){ + return -1; + } + for(int stateIdx = startIndex; stateIdxGet(stateIdx); + auto FRIMessage = KUKAiiwaState->FRIMessage(); + auto timeEvent = FRIMessage->timeStamp(); + timeEventM_Kuka(stateIdx,TimeType::local_receive_time) = timeEvent->local_receive_time(); + timeEventM_Kuka(stateIdx,TimeType::local_request_time) = timeEvent->local_request_time(); + timeEventM_Kuka(stateIdx,TimeType::device_time) = timeEvent->device_time(); + timeEventM_Kuka(stateIdx,TimeType::time_Y) = timeEventM_Kuka(stateIdx,TimeType::device_time) - timeEventM_Kuka(stateIdx,TimeType::local_receive_time); + // timeEventM_Kuka(stateIdx,TimeType::counterIdx) = identifier; + sequenceCounterVec(stateIdx) = FRIMessage->sequenceCounter(); + reflectedSequenceCounterVec(stateIdx) = FRIMessage->reflectedSequenceCounter(); + for(int jointIdx=0; jointIdx<7; jointIdx++){ + measuredJointPosition(stateIdx, jointIdx) = FRIMessage->measuredJointPosition()->size()>0 ? FRIMessage->measuredJointPosition()->Get(jointIdx):0; // flatbuffers::Vector * + measuredTorque(stateIdx, jointIdx) = FRIMessage->measuredTorque()->size()>0 ? FRIMessage->measuredTorque()->Get(jointIdx):0; + commandedJointPosition(stateIdx, jointIdx) = FRIMessage->commandedJointPosition()->size()>0 ? FRIMessage->commandedJointPosition()->Get(jointIdx):0; + commandedTorque(stateIdx, jointIdx) = FRIMessage->commandedTorque()->size()>0 ? FRIMessage->commandedTorque()->Get(jointIdx):0; + externalTorque(stateIdx, jointIdx) = FRIMessage->externalTorque()->size()>0 ? FRIMessage->externalTorque()->Get(jointIdx):0; + jointStateInterpolated(stateIdx, jointIdx) = FRIMessage->jointStateInterpolated()->size()>0 ? FRIMessage->jointStateInterpolated()->Get(jointIdx):0; + } + } + return 1; + } + /// With the same method to process the time data from both Kuka and fusiontracker, combine it and write to a csv file. + /// @param FTKUKA_TimeEvent_CSVfilename, the csv file name + void writeFRIMessageToCSV(std::string& KUKA_FRI_CSVfilename, + grl::MatrixXd& timeEventM_Kuka, + Eigen::VectorXd& sequenceCounterVec, + Eigen::VectorXd& reflectedSequenceCounterVec, + Eigen::MatrixXd& measuredJointPosition, + Eigen::MatrixXd& measuredTorque, + Eigen::MatrixXd& commandedJointPosition, + Eigen::MatrixXd& commandedTorque, + Eigen::MatrixXd& externalTorque, + Eigen::MatrixXd& jointStateInterpolated, + int startIndex = 0) { + + int kuka_time_size = timeEventM_Kuka.rows(); + + assert(kuka_time_size == sequenceCounterVec.size()); + assert(kuka_time_size == reflectedSequenceCounterVec.size()); + assert(kuka_time_size == measuredJointPosition.rows()); + assert(kuka_time_size == measuredTorque.rows()); + assert(kuka_time_size == commandedJointPosition.rows()); + assert(kuka_time_size == commandedTorque.rows()); + assert(kuka_time_size == externalTorque.rows()); + assert(kuka_time_size == jointStateInterpolated.rows()); + auto receive_time = timeEventM_Kuka.col(grl::TimeType::local_receive_time); + assert(checkmonotonic(receive_time)); + + std::ofstream fs; + // create a name for the file output + fs.open(KUKA_FRI_CSVfilename, std::ofstream::out | std::ofstream::app); + // write the file headers + fs << grl::Time_Labels[grl::TimeType::local_receive_time] << "," + << grl::Time_Labels[grl::TimeType::local_request_time] << "," + << grl::Time_Labels[grl::TimeType::device_time] << "," + << grl::Time_Labels[grl::TimeType::time_Y] << "," + << "SequenceCounter," + << "reflectedSequenceCounter," + << grl::M_Pos_Joint_Labels[grl::Joint_Index::joint_1] << "," + << grl::M_Pos_Joint_Labels[grl::Joint_Index::joint_2] << "," + << grl::M_Pos_Joint_Labels[grl::Joint_Index::joint_3] << "," + << grl::M_Pos_Joint_Labels[grl::Joint_Index::joint_4] << "," + << grl::M_Pos_Joint_Labels[grl::Joint_Index::joint_5] << "," + << grl::M_Pos_Joint_Labels[grl::Joint_Index::joint_6] << "," + << grl::M_Pos_Joint_Labels[grl::Joint_Index::joint_7] << "," + + << grl::M_Tor_Joint_Labels[grl::Joint_Index::joint_1] << "," + << grl::M_Tor_Joint_Labels[grl::Joint_Index::joint_2] << "," + << grl::M_Tor_Joint_Labels[grl::Joint_Index::joint_3] << "," + << grl::M_Tor_Joint_Labels[grl::Joint_Index::joint_4] << "," + << grl::M_Tor_Joint_Labels[grl::Joint_Index::joint_5] << "," + << grl::M_Tor_Joint_Labels[grl::Joint_Index::joint_6] << "," + << grl::M_Tor_Joint_Labels[grl::Joint_Index::joint_7] << "," + + << grl::C_Pos_Joint_Labels[grl::Joint_Index::joint_1] << "," + << grl::C_Pos_Joint_Labels[grl::Joint_Index::joint_2] << "," + << grl::C_Pos_Joint_Labels[grl::Joint_Index::joint_3] << "," + << grl::C_Pos_Joint_Labels[grl::Joint_Index::joint_4] << "," + << grl::C_Pos_Joint_Labels[grl::Joint_Index::joint_5] << "," + << grl::C_Pos_Joint_Labels[grl::Joint_Index::joint_6] << "," + << grl::C_Pos_Joint_Labels[grl::Joint_Index::joint_7] << "," + + << grl::C_Tor_Joint_Labels[grl::Joint_Index::joint_1] << "," + << grl::C_Tor_Joint_Labels[grl::Joint_Index::joint_2] << "," + << grl::C_Tor_Joint_Labels[grl::Joint_Index::joint_3] << "," + << grl::C_Tor_Joint_Labels[grl::Joint_Index::joint_4] << "," + << grl::C_Tor_Joint_Labels[grl::Joint_Index::joint_5] << "," + << grl::C_Tor_Joint_Labels[grl::Joint_Index::joint_6] << "," + << grl::C_Tor_Joint_Labels[grl::Joint_Index::joint_7] << "," + + << grl::E_Tor_Joint_Labels[grl::Joint_Index::joint_1] << "," + << grl::E_Tor_Joint_Labels[grl::Joint_Index::joint_2] << "," + << grl::E_Tor_Joint_Labels[grl::Joint_Index::joint_3] << "," + << grl::E_Tor_Joint_Labels[grl::Joint_Index::joint_4] << "," + << grl::E_Tor_Joint_Labels[grl::Joint_Index::joint_5] << "," + << grl::E_Tor_Joint_Labels[grl::Joint_Index::joint_6] << "," + << grl::E_Tor_Joint_Labels[grl::Joint_Index::joint_7] << "," + + << grl::IPO_Joint_Labels[grl::Joint_Index::joint_1] << "," + << grl::IPO_Joint_Labels[grl::Joint_Index::joint_2] << "," + << grl::IPO_Joint_Labels[grl::Joint_Index::joint_3] << "," + << grl::IPO_Joint_Labels[grl::Joint_Index::joint_4] << "," + << grl::IPO_Joint_Labels[grl::Joint_Index::joint_5] << "," + << grl::IPO_Joint_Labels[grl::Joint_Index::joint_6] << "," + << grl::IPO_Joint_Labels[grl::Joint_Index::joint_7] << std::endl; + + int kuka_index = startIndex; + // int64_t diff = timeEventM_Kuka(startIndex, grl::TimeType::time_Y); + while ( kuka_index < kuka_time_size) + { + + fs << timeEventM_Kuka(kuka_index, grl::TimeType::local_receive_time) <<"," + << timeEventM_Kuka(kuka_index, grl::TimeType::local_request_time) <<"," + << timeEventM_Kuka(kuka_index, grl::TimeType::device_time) <<"," + << timeEventM_Kuka(kuka_index, grl::TimeType::time_Y)<<"," + << sequenceCounterVec(kuka_index) <<"," + << reflectedSequenceCounterVec(kuka_index) <<"," + << measuredJointPosition(kuka_index, grl::Joint_Index::joint_1) <<"," + << measuredJointPosition(kuka_index, grl::Joint_Index::joint_2) <<"," + << measuredJointPosition(kuka_index, grl::Joint_Index::joint_3) <<"," + << measuredJointPosition(kuka_index, grl::Joint_Index::joint_4) <<"," + << measuredJointPosition(kuka_index, grl::Joint_Index::joint_5) <<"," + << measuredJointPosition(kuka_index, grl::Joint_Index::joint_6) <<"," + << measuredJointPosition(kuka_index, grl::Joint_Index::joint_7) <<"," + + << measuredTorque(kuka_index, grl::Joint_Index::joint_1) <<"," + << measuredTorque(kuka_index, grl::Joint_Index::joint_2) <<"," + << measuredTorque(kuka_index, grl::Joint_Index::joint_3) <<"," + << measuredTorque(kuka_index, grl::Joint_Index::joint_4) <<"," + << measuredTorque(kuka_index, grl::Joint_Index::joint_5) <<"," + << measuredTorque(kuka_index, grl::Joint_Index::joint_6) <<"," + << measuredTorque(kuka_index, grl::Joint_Index::joint_7) <<"," + + << commandedJointPosition(kuka_index, grl::Joint_Index::joint_1) <<"," + << commandedJointPosition(kuka_index, grl::Joint_Index::joint_2) <<"," + << commandedJointPosition(kuka_index, grl::Joint_Index::joint_3) <<"," + << commandedJointPosition(kuka_index, grl::Joint_Index::joint_4) <<"," + << commandedJointPosition(kuka_index, grl::Joint_Index::joint_5) <<"," + << commandedJointPosition(kuka_index, grl::Joint_Index::joint_6) <<"," + << commandedJointPosition(kuka_index, grl::Joint_Index::joint_7) <<"," + + << commandedTorque(kuka_index, grl::Joint_Index::joint_1) <<"," + << commandedTorque(kuka_index, grl::Joint_Index::joint_2) <<"," + << commandedTorque(kuka_index, grl::Joint_Index::joint_3) <<"," + << commandedTorque(kuka_index, grl::Joint_Index::joint_4) <<"," + << commandedTorque(kuka_index, grl::Joint_Index::joint_5) <<"," + << commandedTorque(kuka_index, grl::Joint_Index::joint_6) <<"," + << commandedTorque(kuka_index, grl::Joint_Index::joint_7) <<"," + + << externalTorque(kuka_index, grl::Joint_Index::joint_1) <<"," + << externalTorque(kuka_index, grl::Joint_Index::joint_2) <<"," + << externalTorque(kuka_index, grl::Joint_Index::joint_3) <<"," + << externalTorque(kuka_index, grl::Joint_Index::joint_4) <<"," + << externalTorque(kuka_index, grl::Joint_Index::joint_5) <<"," + << externalTorque(kuka_index, grl::Joint_Index::joint_6) <<"," + << externalTorque(kuka_index, grl::Joint_Index::joint_7) <<"," + + << jointStateInterpolated(kuka_index, grl::Joint_Index::joint_1) <<"," + << jointStateInterpolated(kuka_index, grl::Joint_Index::joint_2) <<"," + << jointStateInterpolated(kuka_index, grl::Joint_Index::joint_3) <<"," + << jointStateInterpolated(kuka_index, grl::Joint_Index::joint_4) <<"," + << jointStateInterpolated(kuka_index, grl::Joint_Index::joint_5) <<"," + << jointStateInterpolated(kuka_index, grl::Joint_Index::joint_6) <<"," + << jointStateInterpolated(kuka_index, grl::Joint_Index::joint_7) << std::endl; + kuka_index++; + } + + std::cout << "kuka_time_size: " << kuka_time_size << " kuka_index: " << kuka_index << std::endl; + fs.close(); + } /// Get the joint angles of a specific joint (joint_index) /// @param kukaStatesP pointer of the root object for kuka. /// @param joint_index, return the joint angles of the indicated joint. @@ -375,8 +647,9 @@ namespace grl { /// @param labels, the labels for the data to write, which should be the first row of the csv file /// @param timeM, by default we write all the data combined with the timestamp, whose type is grl::MatrixXd (int64_t). /// @param dataM, the data to write, whose type is Eigen::MatrixXd + /// @param rowIndex, In order to make the data from two devices match each other, we just take down the data from the specific row. template - void writeMatrixToCSV(const std::string& CSV_FileName, std::vector &labels, T1& timeM, T2& dataM){ + void writeMatrixToCSV(const std::string& CSV_FileName, std::vector &labels, T1& timeM, T2& dataM, int rowIndex=0){ std::size_t labels_size = labels.size(); std::size_t cols_size = timeM.cols() + dataM.cols(); std::size_t time_rows_size = timeM.rows(); @@ -393,7 +666,7 @@ namespace grl { fs << labels[col] << ","; } fs << labels[cols_size-1] << std::endl; - for(int row_index=0; row_index0) { device_time_step = device_timeV(i) - device_timeV(i-1); receive_time_step = local_receive_timeV(i) - local_receive_timeV(i-1); } // write the data to the output file fs << local_receive_timeV(i) << "," // A - << local_request_timeV(i)<<"," // B - << device_timeV(i) <<"," // C - << device_timeV(i) - local_receive_timeV(i) << "," + << local_request_timeV(i) <<"," // B + << device_timeV(i) <<"," // C + << time_YV(i) <<"," << receive_request(i) << "," << device_time_step << "," << receive_time_step << "," @@ -452,9 +726,9 @@ namespace grl { } - /// With the same method to process the time data from both Kuka and fusiontracker, combine it and write to a csv file. + /// With the same method to process the time data from both Kuka and fusiontracker, combine it and write to a csv file. /// @param FTKUKA_TimeEvent_CSVfilename, the csv file name - void writeFTKUKATimeEventToCSV(std::string& FTKUKA_TimeEvent_CSVfilename, + void writeFTKUKAMessageToCSV(std::string& FTKUKA_TimeEvent_CSVfilename, grl::MatrixXd& timeEventM_FT, grl::MatrixXd& timeEventM_Kuka, Eigen::MatrixXd& jointAngles, @@ -562,7 +836,7 @@ namespace grl { } /// With the same method to process the time data from both Kuka and fusiontracker, combine it and write to a csv file. /// @param FTKUKA_TimeEvent_CSVfilename, the csv file name - void writeFTKUKATimeEventToCSV(std::string& FTKUKA_TimeEvent_CSVfilename, + void writeFTKUKAMessageToCSV(std::string& FTKUKA_TimeEvent_CSVfilename, const fbs_tk::Root &logKUKAiiwaFusionTrackP, const fbs_tk::Root &kukaStatesP, uint32_t &markerID) { diff --git a/test/readFlatbufferTest.cpp b/test/readFlatbufferTest.cpp index d62298d..80d5a0f 100644 --- a/test/readFlatbufferTest.cpp +++ b/test/readFlatbufferTest.cpp @@ -27,7 +27,7 @@ int main(int argc, char* argv[]) { - /// Define the file names + /// Define the csv file names std::string foldname = "/home/cjiao1/src/V-REP_PRO_EDU_V3_4_0_Linux/"; std::string kukaTimeStamp = "2018_03_26_19_06_21_Kukaiiwa"; std::string FTTimeStamp = "2018_03_26_19_06_21_FusionTrack"; @@ -36,7 +36,8 @@ int main(int argc, char* argv[]) std::string foldtimestamp = current_date_and_time_string(); boost::filesystem::path dir{foldname+foldtimestamp}; boost::filesystem::create_directory(dir); - + + std::string KUKA_FRI_CSVfilename = foldname + foldtimestamp + "/KUKA_FRIMessage.csv"; std::string KUKA_TimeEvent_CSV = foldname + foldtimestamp + "/KUKA_TimeEvent.csv"; std::string KUKA_Joint_CSV = foldname + foldtimestamp + "/KUKA_Joint.csv"; std::string KUKA_Pose_CSV = foldname + foldtimestamp + "/KUKA_Pose.csv"; @@ -65,31 +66,43 @@ int main(int argc, char* argv[]) uint32_t markerID_22 = 22; uint32_t markerID_55 = 55; uint32_t markerID_50000 = 50000; - /// Write FT data to CSV + /// Get the object pointer to read data from flatbuffer fbs_tk::Root logKUKAiiwaFusionTrackP = fbs_tk::open_root(fusiontrackBinaryfile); fbs_tk::Root kukaStatesP = fbs_tk::open_root(kukaBinaryfile); std::size_t FT_size = grl::getStateSize(logKUKAiiwaFusionTrackP); std::size_t kuka_size = grl::getStateSize(kukaStatesP); - Eigen::MatrixXd markerPose(FT_size, grl::col_Pose); - // grl::MatrixXd timeEventM_FT = grl::getTimeStamp(logKUKAiiwaFusionTrackP, grl::fusiontracker_tag(), markerID_22); - + + /// Create matrix to contain data + Eigen::MatrixXd markerPose_FT(FT_size, grl::col_Pose); + Eigen::MatrixXd markerTransform_FT = Eigen::MatrixXd::Zero(FT_size, grl::Transform_Labels.size()); grl::MatrixXd timeEventM_FT = grl::MatrixXd::Zero(FT_size, grl::Time_Labels.size()); grl::MatrixXd timeEventM_Kuka = grl::MatrixXd::Zero(kuka_size, grl::Time_Labels.size()); - timeEventM_Kuka = grl::getTimeStamp(kukaStatesP,grl::kuka_tag()); - int validsize_22 = grl::getMarkerPose(logKUKAiiwaFusionTrackP, markerID_22, timeEventM_FT, markerPose); + Eigen::VectorXd sequenceCounterVec = Eigen::VectorXd::Zero(kuka_size); + Eigen::VectorXd reflectedSequenceCounterVec = Eigen::VectorXd::Zero(kuka_size); + Eigen::MatrixXd measuredJointPosition = Eigen::MatrixXd::Zero(kuka_size, grl::jointNum); + Eigen::MatrixXd measuredTorque = Eigen::MatrixXd::Zero(kuka_size, grl::jointNum); + Eigen::MatrixXd commandedJointPosition = Eigen::MatrixXd::Zero(kuka_size, grl::jointNum); + Eigen::MatrixXd commandedTorque = Eigen::MatrixXd::Zero(kuka_size, grl::jointNum); + Eigen::MatrixXd externalTorque = Eigen::MatrixXd::Zero(kuka_size, grl::jointNum); + Eigen::MatrixXd jointStateInterpolated = Eigen::MatrixXd::Zero(kuka_size, grl::jointNum); + int ret = grl::getFRIMessage(kukaStatesP, + timeEventM_Kuka, + sequenceCounterVec, + reflectedSequenceCounterVec, + measuredJointPosition, + measuredTorque, + commandedJointPosition, + commandedTorque, + externalTorque, + jointStateInterpolated); + int validsize_22 = grl::getMarkerPose(logKUKAiiwaFusionTrackP, markerID_22, timeEventM_FT, markerPose_FT); + + std::cout<< "------Kuka_size: "<< kuka_size << " FT_size: "<< FT_size << std::endl; std::cout<<"validsize_22: " << validsize_22 << std::endl; - - - - Eigen::MatrixXd jointAngles = grl::getAllJointAngles(kukaStatesP); - std::cout<< "------Kuka_size: "<< kuka_size << std::endl; - std::cout<< "------FT_size: "<< FT_size << std::endl; std::size_t FT_time_size = timeEventM_FT.rows(); std::size_t kuka_time_size = timeEventM_Kuka.rows(); grl::MatrixXd timeEventM = grl::MatrixXd::Zero(FT_size, timeEventM_FT.cols()); - - int kuka_index = 0; int FT_index = 0; @@ -99,83 +112,51 @@ int main(int argc, char* argv[]) while(kuka_index timeEventM_FT(FT_index,grl::TimeType::local_receive_time) && kuka_index == 0){ + while(kuka_index == 0 && FT_index timeEventM_FT(FT_index,grl::TimeType::local_receive_time) ){ FT_index++; } auto initial_local_time = std::min(timeEventM_FT(FT_index,grl::TimeType::local_receive_time), timeEventM_Kuka(kuka_index, grl::TimeType::local_receive_time)); auto initial_device_time_kuka = timeEventM_Kuka(kuka_index,grl::TimeType::device_time); auto initial_device_time_FT = timeEventM_FT(FT_index,grl::TimeType::device_time); + std::cout<< "Main initial_local_time: " << initial_local_time << std::endl; timeEventM_FT.col(grl::TimeType::local_request_time) = timeEventM_FT.col(grl::TimeType::local_request_time) - initial_local_time * grl::VectorXd::Ones(FT_time_size); timeEventM_FT.col(grl::TimeType::local_receive_time) = timeEventM_FT.col(grl::TimeType::local_receive_time) - initial_local_time * grl::VectorXd::Ones(FT_time_size); timeEventM_FT.col(grl::TimeType::device_time) = timeEventM_FT.col(grl::TimeType::device_time) - initial_device_time_FT * grl::VectorXd::Ones(FT_time_size); + timeEventM_FT.col(grl::TimeType::time_Y) = timeEventM_FT.col(grl::TimeType::time_Y) - timeEventM_FT(FT_index, grl::TimeType::time_Y) * grl::VectorXd::Ones(FT_time_size); timeEventM_Kuka.col(grl::TimeType::local_request_time) = timeEventM_Kuka.col(grl::TimeType::local_request_time) - initial_local_time * grl::VectorXd::Ones(kuka_time_size); timeEventM_Kuka.col(grl::TimeType::local_receive_time) = timeEventM_Kuka.col(grl::TimeType::local_receive_time) - initial_local_time * grl::VectorXd::Ones(kuka_time_size); timeEventM_Kuka.col(grl::TimeType::device_time) = timeEventM_Kuka.col(grl::TimeType::device_time) - initial_device_time_kuka * grl::VectorXd::Ones(kuka_time_size); + timeEventM_Kuka.col(grl::TimeType::time_Y) = timeEventM_Kuka.col(grl::TimeType::time_Y) - timeEventM_Kuka(kuka_index, grl::TimeType::time_Y) * grl::VectorXd::Ones(kuka_time_size); - - - - // grl::regularizeTimeEvent(timeEventM_FT); - - - // timeEventM_FT = grl::MatrixXd::Zero(FT_size, timeEventM_FT.cols()); - - // Get the pose of the marker 22. - // Since the bad data is skipped, the timeEventM_FT also needs to be filled out with the orinial time stamp. - - - std::vector FT_Labels_Pose = grl::getLabels(grl::LabelsType::FT_Pose); - - grl::writeFTKUKATimeEventToCSV(FTKUKA_TimeEvent_CSV, - timeEventM_FT, timeEventM_Kuka, jointAngles, markerPose, FT_index, kuka_index); - if(validsize_22>2){ - // grl::regularizeTimeEvent(timeEventM_FT); - - grl::writeTimeEventToCSV(FT_TimeEvent_CSV, timeEventM_FT); - grl::writeMatrixToCSV(FT_Marker22_CSV, FT_Labels_Pose, timeEventM_FT, markerPose); - } - // Change it back to the original size - timeEventM_FT.resize(FT_size, grl::col_timeEvent); - markerPose.resize(FT_size, grl::col_Pose); - // Get the pose of the bone marker - int validsize_55 = grl::getMarkerPose(logKUKAiiwaFusionTrackP, markerID_55, timeEventM_FT, markerPose); - - std::cout<<"validsize_55: " << validsize_55 << std::endl; - if(validsize_55>2) { - grl::regularizeTimeEvent(timeEventM_FT); - grl::writeMatrixToCSV(FT_Marker55_CSV, FT_Labels_Pose, timeEventM_FT, markerPose); - } - // Change it back to the original size - timeEventM_FT.resize(FT_size, grl::col_timeEvent); - markerPose.resize(FT_size, grl::col_Pose); - // Get the pose of the bone marker - int validsize_50000 = grl::getMarkerPose(logKUKAiiwaFusionTrackP, markerID_50000, timeEventM_FT, markerPose); - - std::cout<<"validsize_50000: " << validsize_50000 << std::endl; - if(validsize_50000>2) { - grl::regularizeTimeEvent(timeEventM_FT); - grl::writeMatrixToCSV(FT_Marker50000_CSV, FT_Labels_Pose, timeEventM_FT, markerPose); - } ///////////////////////////////////////////////////////////////////////////////////////////////////// /// Write KUKA data to CSV //////////////////////////////////////////////////////////////////////////////////////////////////// - - grl::regularizeTimeEvent(timeEventM_Kuka); - grl::writeTimeEventToCSV(KUKA_TimeEvent_CSV, timeEventM_Kuka); - - std::vector Kuka_Joint_Labels = grl::getLabels(grl::LabelsType::Joint); - - if(jointAngles.rows() == timeEventM_Kuka.rows()){ + grl::writeFRIMessageToCSV(KUKA_FRI_CSVfilename, + timeEventM_Kuka, + sequenceCounterVec, + reflectedSequenceCounterVec, + measuredJointPosition, + measuredTorque, + commandedJointPosition, + commandedTorque, + externalTorque, + jointStateInterpolated, + kuka_index); + + timeEventM_Kuka.col(grl::TimeType::counterIdx) = std::move(sequenceCounterVec.cast()); + grl::writeTimeEventToCSV(KUKA_TimeEvent_CSV, timeEventM_Kuka, kuka_index); + std::vector kuka_labels = grl::Time_Labels; + kuka_labels.insert(std::end(kuka_labels), std::begin(grl::M_Pos_Joint_Labels), std::end(grl::M_Pos_Joint_Labels)); // Write the joint angles into csv - grl::writeMatrixToCSV(KUKA_Joint_CSV, Kuka_Joint_Labels, timeEventM_Kuka, jointAngles); + grl::writeMatrixToCSV(KUKA_Joint_CSV, kuka_labels, timeEventM_Kuka, measuredJointPosition); std::vector PK_Pose_Labels = grl::getLabels(grl::LabelsType::Kuka_Pose); // forward kinematic to get the of the end effector - // markerPose = true; // Get the marker pose directly, or get the end effector pose. - bool markerPose = false; - std::vector PoseEE = grl::getPoseEE(jointAngles, markerPose); + // markerPose_FT = true; // Get the marker pose directly, or get the end effector pose. + // bool markerPose_FT = false; + std::vector PoseEE = grl::getPoseEE(measuredJointPosition); // convert the sva::PTransformd to Plucker coordinate. Eigen::MatrixXd PKPose = grl::getPluckerPose(PoseEE); // Write the plucker coordinate into csv. @@ -196,14 +177,50 @@ int main(int argc, char* argv[]) // PKPose = Eigen::MatrixXd::Zero(PKPose.rows(), PKPose.cols()); // PKPose = grl::getPluckerPose(Fudicial_Robot_Pose); // grl::writeMatrixToCSV(FudicialToRobotPose_CSV, PK_Pose_Labels, timeEventM_Kuka, PKPose); + + + + // Get the pose of the marker 22. + // Since the bad data is skipped, the timeEventM_FT also needs to be filled out with the orinial time stamp. + + + std::vector FT_Labels_Pose = grl::getLabels(grl::LabelsType::FT_Pose); + + grl::writeFTKUKAMessageToCSV(FTKUKA_TimeEvent_CSV, timeEventM_FT, timeEventM_Kuka, measuredJointPosition, markerPose_FT, FT_index, kuka_index); + if(validsize_22>2){ + grl::writeTimeEventToCSV(FT_TimeEvent_CSV, timeEventM_FT, FT_index); + grl::writeMatrixToCSV(FT_Marker22_CSV, FT_Labels_Pose, timeEventM_FT, markerPose_FT, FT_index); + } + // Change it back to the original size + timeEventM_FT.resize(FT_size, grl::col_timeEvent); + markerPose_FT.resize(FT_size, grl::col_Pose); + // Get the pose of the bone marker + int validsize_55 = grl::getMarkerPose(logKUKAiiwaFusionTrackP, markerID_55, timeEventM_FT, markerPose_FT); + + std::cout<<"validsize_55: " << validsize_55 << std::endl; + if(validsize_55>2) { + grl::regularizeTimeEvent(timeEventM_FT, initial_local_time, initial_device_time_FT, FT_index); + grl::writeMatrixToCSV(FT_Marker55_CSV, FT_Labels_Pose, timeEventM_FT, markerPose_FT, FT_index); } + // Change it back to the original size + timeEventM_FT.resize(FT_size, grl::col_timeEvent); + markerPose_FT.resize(FT_size, grl::col_Pose); + // Get the pose of the bone marker + int validsize_50000 = grl::getMarkerPose(logKUKAiiwaFusionTrackP, markerID_50000, timeEventM_FT, markerPose_FT); + std::cout<<"validsize_50000: " << validsize_50000 << std::endl; + if(validsize_50000>2) { + grl::regularizeTimeEvent(timeEventM_FT, initial_local_time, initial_device_time_FT, FT_index); + grl::writeMatrixToCSV(FT_Marker50000_CSV, FT_Labels_Pose, timeEventM_FT, markerPose_FT, FT_index); + } + + - // grl::writeFTKUKATimeEventToCSV(FTKUKA_TimeEvent_CSV, logKUKAiiwaFusionTrackP, kukaStatesP, markerID_22); + // grl::writeFTKUKAMessageToCSV(FTKUKA_TimeEvent_CSV, logKUKAiiwaFusionTrackP, kukaStatesP, markerID_22); return 0; } From 49a1044344f147755b1a93210989f1ea54fe4618 Mon Sep 17 00:00:00 2001 From: Chunting Date: Mon, 2 Apr 2018 11:32:36 -0400 Subject: [PATCH 069/102] Fix bugs, make the timestamp consistency. It can plot the data with the same X-axis (timestamp) --- .../grl/flatbuffer/ParseflatbuffertoCSV.hpp | 7 ++-- .../grl/vrep/InverseKinematicsVrepPlugin.hpp | 42 +++---------------- 2 files changed, 8 insertions(+), 41 deletions(-) diff --git a/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp b/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp index 347e8fa..0aef8ba 100644 --- a/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp +++ b/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp @@ -1067,7 +1067,7 @@ int getRowsNumber(std::string filename){ /// Get the joint angles at specific time point (index) /// @return jointPosition, Eigen vector which contains joint position of the seven joints. - int getJointAnglesFromCSV(std::string filename, Eigen::VectorXf &jointPosition, int rowIdx){ + int64_t getJointAnglesFromCSV(std::string filename, Eigen::VectorXf &jointPosition, int rowIdx){ // std::ifstream file("/home/cjiao1/src/V-REP_PRO_EDU_V3_4_0_Linux/2018_03_20_20_35_20/KUKA_Joint.csv"); // int rowNum = getRowsNumber(filename); @@ -1083,17 +1083,16 @@ int getRowsNumber(std::string filename){ for(grl::CSVIterator loop(file); loop != grl::CSVIterator(); ++loop) { - if (!(*loop)[2].empty()) { if(row == rowIdx){ for(int joint_index = 0; joint_index((*loop)[joint_index+5]); } return boost::lexical_cast((*loop)[0]); } row++; - } + } } return -1; diff --git a/include/grl/vrep/InverseKinematicsVrepPlugin.hpp b/include/grl/vrep/InverseKinematicsVrepPlugin.hpp index 058a3d4..ac22c4a 100644 --- a/include/grl/vrep/InverseKinematicsVrepPlugin.hpp +++ b/include/grl/vrep/InverseKinematicsVrepPlugin.hpp @@ -526,8 +526,6 @@ void applyEstimate(){ // 0.0754499, 0.0975167, 0.0882869 // 0.0388255, 0.102084, 0.0902271 - - transformEstimate = eigenQuat; transformEstimate.translation() = eigenPos; @@ -554,13 +552,13 @@ void getPoseFromCSV(std::string filename, int time_index){ auto& simArmConfig = rbd_mbcs_[simulatedRobotIndex]; Eigen::VectorXf joint_angles = Eigen::VectorXf::Zero(7); // std::cout << "\nBefore: " << joint_angles << std::endl; - int timeStamp = grl::getJointAnglesFromCSV(filename, joint_angles, time_index); + int64_t timeStamp = grl::getJointAnglesFromCSV(filename, joint_angles, time_index); assert(timeStamp != -1 ); std::vector vrepJointAngles; double angle = 0; int jointIdx = 0; - // std::cout << "Time: " << timeStamp << std::endl; + std::cout << "Time: " << timeStamp << std::endl; for(auto i : jointHandles_) { angle = joint_angles[jointIdx++]; @@ -669,37 +667,7 @@ void getPoseFromCSV(std::string filename, int time_index){ - // void replay(){ - // loadFromBinary(); - // /// simulation tick time step in float seconds from vrep API call - // float simulationTimeStep = simGetSimulationTimeStep(); - - // // we only have one robot for the moment so the index of it is 0 - // const std::size_t simulatedRobotIndex = 0; - // auto& simArmMultiBody = rbd_mbs_[simulatedRobotIndex]; - // auto& simArmConfig = rbd_mbcs_[simulatedRobotIndex]; - // auto joint_angles = grl::getJointAnglesFromBinary(kukaStatesP_, time_index++); - - // std::vector vrepJointAngles; - // double angle = 0; - // int jointIdx=0; - // for(auto i : jointHandles_) - // { - // angle = joint_angles[jointIdx++]; - // simSetJointPosition(i,angle); - // vrepJointAngles.push_back(angle); - // } - - // /// @todo TODO(ahundt) rethink where/when/how to send command for the joint angles. Return to LUA? Set Directly? Distribute via vrep send message command? - - // //////////////////////////////////////////////////// - // // Set joints to current arm position in simulation - // SetRBDynArmFromVrep(jointNames_,jointHandles_,simArmMultiBody,simArmConfig); - // rbd::forwardKinematics(simArmMultiBody, simArmConfig); - // rbd::forwardVelocity(simArmMultiBody, simArmConfig); - - // debugFrames(); - // } + /// Configures updateKinematics with the goal the kinematics should aim for enum class GoalPosE { realGoalPosition, debugGoalPosition }; /// Configures updateKinematics the algorithm the kinematics should use for solving @@ -978,9 +946,9 @@ void getPoseFromCSV(std::string filename, int time_index){ if(ik) { updateKinematics(); } else { - auto myFile = boost::filesystem::current_path() /"2018_03_26_19_23_38"; + auto myFile = boost::filesystem::current_path() /"2018_03_30_17_07_55"; std::string pathName = myFile.string(); - std::string kukaJoint = pathName+"/FTKUKA_TimeEvent.csv"; // KUKA_Joint.csv FTKUKA_TimeEvent.csv + std::string kukaJoint = pathName+"/KUKA_Joint.csv"; // KUKA_Joint.csv FTKUKA_TimeEvent.csv std::string markerPose = pathName+"/FT_Pose_Marker22.csv"; // std::cout << "Call Run One..." << std::endl; From cb7174be9ddbcdb2103a0ddc95f89492165d1d32 Mon Sep 17 00:00:00 2001 From: Chunting Date: Mon, 2 Apr 2018 19:18:53 -0400 Subject: [PATCH 070/102] Use the request time as the baseline --- .../grl/flatbuffer/ParseflatbuffertoCSV.hpp | 260 +++++++++--------- test/readFlatbufferTest.cpp | 19 +- 2 files changed, 133 insertions(+), 146 deletions(-) diff --git a/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp b/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp index 0aef8ba..8d68f5f 100644 --- a/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp +++ b/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp @@ -87,7 +87,7 @@ namespace grl { typedef Eigen::Matrix VectorXd; typedef Eigen::Matrix MatrixXd; - std::vector Time_Labels = {"local_receive_time_X", "local_request_time_offset", "device_time_offset", "time_Y", "counter"}; + std::vector Time_Labels = { "local_request_time_offset", "local_receive_time_offset", "device_time_offset", "time_Y", "counter"}; // std::vector PK_Pose_Labels = {"P_X", "P_Y", "P_Z", "Q_X", "Q_Y", "Q_Z", "Q_W"}; std::vector M_Pos_Joint_Labels = {"M_Pos_J1", "M_Pos_J2", "M_Pos_J3", "M_Pos_J4", "M_Pos_J5", "M_Pos_J6", "M_Pos_J7"}; std::vector M_Tor_Joint_Labels = {"M_Tor_J1", "M_Tor_J2", "M_Tor_J3", "M_Tor_J4", "M_Tor_J5", "M_Tor_J6", "M_Tor_J7"}; @@ -98,10 +98,10 @@ namespace grl { std::vector Joint_Labels = {"Joint_1", "Joint_2", "Joint_3", "Joint_4", "Joint_5", "Joint_6", "Joint_7"}; std::vector Transform_Labels = {"X", "Y", "Z", "Q_W", "Q_X", "Q_Y", "Q_Z"}; std::vector PK_Pose_Labels = {"X", "Y", "Z", "A", "B", "C"}; - enum TimeType { local_receive_time = 0, local_request_time = 1, device_time = 2, time_Y = 3, counterIdx=4}; + enum TimeType { local_request_time = 0, local_receive_time = 1, device_time = 2, time_Y = 3, counterIdx=4}; enum Joint_Index { joint_1, joint_2, joint_3, joint_4, joint_5, joint_6, joint_7}; enum LabelsType { FT_Pose = 0, Joint = 1, Kuka_Pose= 2}; - + int timeBaseline_index = TimeType::local_request_time; // Another option is TimeType::local_receive_time struct kuka_tag {}; struct fusiontracker_tag {}; int col_timeEvent=Time_Labels.size(); @@ -165,6 +165,7 @@ namespace grl { assert(state_size); int row = 0; grl::MatrixXd timeEventM(state_size,col_timeEvent); + for(int i = 0; iGet(i); auto FT_Message = kukaiiwaFusionTrackMessage->deviceState_as_FusionTrackMessage(); @@ -176,29 +177,16 @@ namespace grl { for(int markerIndex=0; markerIndexGet(markerIndex); auto marker_ID = marker->geometryID(); - // if(markerID == 0){ - // if(marker_ID == 22 || marker_ID == 55){ - // auto timeEvent = kukaiiwaFusionTrackMessage->timeEvent(); - // timeEventM(row,TimeType::local_receive_time) = timeEvent->local_receive_time(); - // timeEventM(row,TimeType::local_request_time) = timeEvent->local_request_time(); - // timeEventM(row,TimeType::device_time) = timeEvent->device_time(); - // timeEventM(row,TimeType::time_Y) = timeEventM(row,TimeType::device_time) - timeEventM(row,TimeType::local_receive_time); - // timeEventM(row,TimeType::counterIdx) = counter; - // row++; - // break; - // } - // } else { - if(marker_ID == markerID){ - auto timeEvent = kukaiiwaFusionTrackMessage->timeEvent(); - timeEventM(row,TimeType::local_receive_time) = timeEvent->local_receive_time(); - timeEventM(row,TimeType::local_request_time) = timeEvent->local_request_time(); - timeEventM(row,TimeType::device_time) = timeEvent->device_time(); - timeEventM(row,TimeType::time_Y) = timeEventM(row,TimeType::device_time) - timeEventM(row,TimeType::local_receive_time); - timeEventM(row,TimeType::counterIdx) = counter; - row++; - break; - } - // } + if(marker_ID == markerID){ + auto timeEvent = kukaiiwaFusionTrackMessage->timeEvent(); + timeEventM(row,TimeType::local_receive_time) = timeEvent->local_receive_time(); + timeEventM(row,TimeType::local_request_time) = timeEvent->local_request_time(); + timeEventM(row,TimeType::device_time) = timeEvent->device_time(); + timeEventM(row,TimeType::time_Y) = timeEventM(row,TimeType::device_time) - timeEventM(row,timeBaseline_index); + timeEventM(row,TimeType::counterIdx) = counter; + row++; + break; + } } } @@ -206,7 +194,7 @@ namespace grl { if(row < state_size) { timeEventM.conservativeResize(row, Eigen::NoChange_t{}); } - grl::VectorXd timeCol = timeEventM.col(TimeType::local_receive_time); + grl::VectorXd timeCol = timeEventM.col(timeBaseline_index); assert(checkmonotonic(timeCol)); return timeEventM; } @@ -223,6 +211,7 @@ namespace grl { std::size_t state_size = states->size(); assert(state_size); grl::MatrixXd timeEventM(state_size,col_timeEvent); + for(int i = 0; iGet(i); auto FRIMessage = KUKAiiwaState->FRIMessage(); @@ -231,10 +220,10 @@ namespace grl { timeEventM(i,TimeType::local_receive_time) = timeEvent->local_receive_time(); timeEventM(i,TimeType::local_request_time) = timeEvent->local_request_time(); timeEventM(i,TimeType::device_time) = timeEvent->device_time(); - timeEventM(i,TimeType::time_Y) = timeEventM(i,TimeType::device_time) - timeEventM(i,TimeType::local_receive_time); + timeEventM(i,TimeType::time_Y) = timeEventM(i,TimeType::device_time) - timeEventM(i,timeBaseline_index); timeEventM(i,TimeType::counterIdx) = identifier; } - grl::VectorXd timeCol = timeEventM.col(TimeType::local_receive_time); + grl::VectorXd timeCol = timeEventM.col(timeBaseline_index); assert(checkmonotonic(timeCol)); return timeEventM; } @@ -275,6 +264,7 @@ namespace grl { // Eigen::MatrixXd markerPose(state_size, grl::col_Pose); // std::cout <<"State size: " << state_size << " markerPose rows: " << markerPose.rows() << std::endl; int BadCount = 0; + for(int i = 0; iGet(i); auto FT_Message = kukaiiwaFusionTrackMessage->deviceState_as_FusionTrackMessage(); @@ -289,10 +279,10 @@ namespace grl { auto marker_ID = marker->geometryID(); if(marker_ID == markerID){ auto timeEvent = kukaiiwaFusionTrackMessage->timeEvent(); - timeEventM(row,TimeType::local_receive_time) = timeEvent->local_receive_time(); timeEventM(row,TimeType::local_request_time) = timeEvent->local_request_time(); + timeEventM(row,TimeType::local_receive_time) = timeEvent->local_receive_time(); timeEventM(row,TimeType::device_time) = timeEvent->device_time(); - timeEventM(row,TimeType::time_Y) = timeEventM(row,TimeType::device_time) - timeEventM(row,TimeType::local_receive_time); + timeEventM(row,TimeType::time_Y) = timeEventM(row,TimeType::device_time) - timeEventM(row,timeBaseline_index); timeEventM(row,TimeType::counterIdx) = counter; @@ -307,19 +297,6 @@ namespace grl { Eigen::RowVectorXd pose(grl::col_Pose); pose << r.transpose(), eulerAngleEigen.transpose(); - - // if(markerFrame == true) { - // Eigen::VectorXd onePose(7); - // onePose << r, q.w(), q.x(), q.y(), q.z(); - // auto markerToCameraTransform = grl::MarkerPoseToAffine3f(onePose); - // auto cameraTomarkerTransform = markerToCameraTransform.inverse(); - // auto _markerPose = grl::Affine3fToMarkerPose(cameraTomarkerTransform); - // pose = grl::getPluckerPose(_markerPose); - - // } - - - markerPose.row(row++) = pose; // Once read the specified marker information, skip out of the marker loop. @@ -333,7 +310,7 @@ namespace grl { } } if(row < state_size) { - int64_t FT_diff = timeEventM(0,TimeType::device_time) - timeEventM(0,TimeType::local_receive_time); + int64_t FT_diff = timeEventM(0,TimeType::device_time) - timeEventM(0,timeBaseline_index); timeEventM.col(TimeType::time_Y) = timeEventM.col(TimeType::time_Y) - FT_diff * grl::VectorXd::Ones(timeEventM.rows()); markerPose.conservativeResize(row, Eigen::NoChange_t{}); timeEventM.conservativeResize(row, Eigen::NoChange_t{}); @@ -363,6 +340,7 @@ namespace grl { // Eigen::MatrixXd markerPose(state_size, grl::col_Pose); // std::cout <<"State size: " << state_size << " markerPose rows: " << markerPose.rows() << std::endl; int BadCount = 0; + for(int stateIdx = startIndex; startIndexGet(stateIdx); auto FT_Message = kukaiiwaFusionTrackMessage->deviceState_as_FusionTrackMessage(); @@ -377,10 +355,10 @@ namespace grl { auto marker_ID = marker->geometryID(); if(marker_ID == markerID){ auto timeEvent = kukaiiwaFusionTrackMessage->timeEvent(); - timeEventM_FT(row,TimeType::local_receive_time) = timeEvent->local_receive_time(); timeEventM_FT(row,TimeType::local_request_time) = timeEvent->local_request_time(); + timeEventM_FT(row,TimeType::local_receive_time) = timeEvent->local_receive_time(); timeEventM_FT(row,TimeType::device_time) = timeEvent->device_time(); - timeEventM_FT(row,TimeType::time_Y) = timeEventM_FT(row,TimeType::device_time) - timeEventM_FT(row,TimeType::local_receive_time); + timeEventM_FT(row,TimeType::time_Y) = timeEventM_FT(row,TimeType::device_time) - timeEventM_FT(row,timeBaseline_index); timeEventM_FT(row,TimeType::counterIdx) = counter; auto Pose = marker->transform(); auto FB_r = Pose->position(); @@ -397,7 +375,7 @@ namespace grl { } } if(row < state_size) { - int64_t FT_diff = timeEventM_FT(0,TimeType::device_time) - timeEventM_FT(0,TimeType::local_receive_time); + int64_t FT_diff = timeEventM_FT(0,TimeType::device_time) - timeEventM_FT(0,timeBaseline_index); timeEventM_FT.col(TimeType::time_Y) = timeEventM_FT.col(TimeType::time_Y) - FT_diff * grl::VectorXd::Ones(timeEventM_FT.rows()); transform.conservativeResize(row, Eigen::NoChange_t{}); timeEventM_FT.conservativeResize(row, Eigen::NoChange_t{}); @@ -430,14 +408,16 @@ namespace grl { if(startIndex>state_size){ return -1; } + for(int stateIdx = startIndex; stateIdxGet(stateIdx); auto FRIMessage = KUKAiiwaState->FRIMessage(); auto timeEvent = FRIMessage->timeStamp(); - timeEventM_Kuka(stateIdx,TimeType::local_receive_time) = timeEvent->local_receive_time(); timeEventM_Kuka(stateIdx,TimeType::local_request_time) = timeEvent->local_request_time(); + timeEventM_Kuka(stateIdx,TimeType::local_receive_time) = timeEvent->local_receive_time(); timeEventM_Kuka(stateIdx,TimeType::device_time) = timeEvent->device_time(); - timeEventM_Kuka(stateIdx,TimeType::time_Y) = timeEventM_Kuka(stateIdx,TimeType::device_time) - timeEventM_Kuka(stateIdx,TimeType::local_receive_time); + // timeEventM_Kuka(stateIdx,TimeType::time_Y) = timeEventM_Kuka(stateIdx,TimeType::device_time) - timeEventM_Kuka(stateIdx,TimeType::local_receive_time); + timeEventM_Kuka(stateIdx,TimeType::time_Y) = timeEventM_Kuka(stateIdx,TimeType::device_time) - timeEventM_Kuka(stateIdx,timeBaseline_index); // timeEventM_Kuka(stateIdx,TimeType::counterIdx) = identifier; sequenceCounterVec(stateIdx) = FRIMessage->sequenceCounter(); reflectedSequenceCounterVec(stateIdx) = FRIMessage->reflectedSequenceCounter(); @@ -486,8 +466,8 @@ namespace grl { // create a name for the file output fs.open(KUKA_FRI_CSVfilename, std::ofstream::out | std::ofstream::app); // write the file headers - fs << grl::Time_Labels[grl::TimeType::local_receive_time] << "," - << grl::Time_Labels[grl::TimeType::local_request_time] << "," + fs << grl::Time_Labels[grl::TimeType::local_request_time] << "," + << grl::Time_Labels[grl::TimeType::local_receive_time] << "," << grl::Time_Labels[grl::TimeType::device_time] << "," << grl::Time_Labels[grl::TimeType::time_Y] << "," << "SequenceCounter," @@ -545,8 +525,8 @@ namespace grl { while ( kuka_index < kuka_time_size) { - fs << timeEventM_Kuka(kuka_index, grl::TimeType::local_receive_time) <<"," - << timeEventM_Kuka(kuka_index, grl::TimeType::local_request_time) <<"," + fs << timeEventM_Kuka(kuka_index, grl::TimeType::local_request_time) <<"," + << timeEventM_Kuka(kuka_index, grl::TimeType::local_receive_time) <<"," << timeEventM_Kuka(kuka_index, grl::TimeType::device_time) <<"," << timeEventM_Kuka(kuka_index, grl::TimeType::time_Y)<<"," << sequenceCounterVec(kuka_index) <<"," @@ -685,8 +665,8 @@ namespace grl { /// @param timeEventM void writeTimeEventToCSV( std::string & CSV_FileName, grl::MatrixXd& timeEventM, int rowIdx) { std::size_t time_size = timeEventM.rows(); - grl::VectorXd local_receive_timeV = timeEventM.col(grl::TimeType::local_receive_time); grl::VectorXd local_request_timeV = timeEventM.col(grl::TimeType::local_request_time); + grl::VectorXd local_receive_timeV = timeEventM.col(grl::TimeType::local_receive_time); grl::VectorXd device_timeV = timeEventM.col(grl::TimeType::device_time); grl::VectorXd time_YV = timeEventM.col(grl::TimeType::time_Y); grl::VectorXd counter = timeEventM.col(grl::TimeType::counterIdx); @@ -696,8 +676,8 @@ namespace grl { // create a name for the file output fs.open( CSV_FileName, std::ofstream::out | std::ofstream::app); // write the file headers - fs << grl::Time_Labels[grl::TimeType::local_receive_time] << "," - << grl::Time_Labels[grl::TimeType::local_request_time] << "," + fs << grl::Time_Labels[grl::TimeType::local_request_time] << "," + << grl::Time_Labels[grl::TimeType::local_receive_time] << "," << grl::Time_Labels[grl::TimeType::device_time] << "," << grl::Time_Labels[grl::TimeType::time_Y] << "," << "Receive-Request," @@ -713,9 +693,9 @@ namespace grl { receive_time_step = local_receive_timeV(i) - local_receive_timeV(i-1); } // write the data to the output file - fs << local_receive_timeV(i) << "," // A - << local_request_timeV(i) <<"," // B - << device_timeV(i) <<"," // C + fs << local_request_timeV(i) <<"," + << local_receive_timeV(i) << "," + << device_timeV(i) <<"," << time_YV(i) <<"," << receive_request(i) << "," << device_time_step << "," @@ -735,6 +715,7 @@ namespace grl { Eigen::MatrixXd& markerPose, int FT_index=0, int kuka_index=0) { + int FT_time_size = timeEventM_FT.rows(); int kuka_time_size = timeEventM_Kuka.rows(); grl::VectorXd FT_local_request_time = timeEventM_FT.col(local_request_time); @@ -746,17 +727,24 @@ namespace grl { grl::VectorXd kuka_local_receive_time = timeEventM_Kuka.col(local_receive_time); grl::VectorXd kuka_device_time = timeEventM_Kuka.col(device_time); // assert(checkmonotonic(kuka_local_receive_time)); - int64_t kuka_diff = kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index); - int64_t FT_diff = FT_device_time(FT_index) - FT_local_receive_time(FT_index); + // Use the receive time as the baseline. + // int64_t kuka_diff = kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index); + // int64_t FT_diff = FT_device_time(FT_index) - FT_local_receive_time(FT_index); + // Use the request time as the baseline + int64_t kuka_diff = kuka_device_time(kuka_index) - kuka_local_request_time(kuka_index); + int64_t FT_diff = FT_device_time(FT_index) - FT_local_request_time(FT_index); + + grl::VectorXd Y_kuka = kuka_device_time - kuka_local_request_time - grl::VectorXd::Ones(kuka_time_size)*kuka_diff; + grl::VectorXd Y_FT = FT_device_time - FT_local_request_time - grl::VectorXd::Ones(FT_time_size)*FT_diff; + std::ofstream fs; // create a name for the file output fs.open( FTKUKA_TimeEvent_CSVfilename, std::ofstream::out | std::ofstream::app); // write the file headers - fs << "local_receive_time_offset_X," - << "FT_local_request_time," - << "KUKA_local_request_time," + fs << "local_request_time," + << "local_receive_time," << "FT_device_time_offset," << "device_time_offset_kuka," << "Y_FT," @@ -779,57 +767,57 @@ namespace grl { while ( kuka_index < kuka_time_size && FT_index < FT_time_size ) { - // If the row value is the kuka time, then the FT cells should be left blank. - if ( kuka_local_receive_time(kuka_index) < FT_local_receive_time(FT_index) ){ - // write the data to the output file - auto jointrow = jointAngles.row(kuka_index); - fs << kuka_local_receive_time(kuka_index) <<"," - <<"," - << kuka_local_request_time(kuka_index) << "," - << "," - << kuka_device_time(kuka_index) <<"," - << "," - << kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index) - kuka_diff << "," - << ",,,,,," - << jointrow[0] << "," - << jointrow[1] << "," - << jointrow[2] << "," - << jointrow[3] << "," - << jointrow[4] << "," - << jointrow[5] << "," - << jointrow[6] - << std::endl; - kuka_index++; - } else if( kuka_local_receive_time(kuka_index) > FT_local_receive_time(FT_index)) { - auto matrixrow = markerPose.row(FT_index); - // If the row value is the FT time, then the kuka cells should be left blank. - fs << FT_local_receive_time(FT_index) <<"," - << FT_local_request_time(FT_index) << "," - <<"," - << FT_device_time(FT_index) << "," - << "," - << FT_device_time(FT_index) - FT_local_receive_time(FT_index) - FT_diff << "," - << "," - << matrixrow[0] << "," - << matrixrow[1] << "," - << matrixrow[2] << "," - << matrixrow[3] << "," - << matrixrow[4] << "," - << matrixrow[5] << std::endl; - FT_index++; - } else { - // In case the time is extactly equivent with each other. - fs << FT_local_receive_time(FT_index) <<"," - << FT_local_request_time(FT_index) << "," - << kuka_local_request_time(kuka_index) << "," - << FT_device_time(FT_index) << "," - << kuka_device_time(kuka_index) <<"," - << FT_device_time(FT_index) - FT_local_receive_time(FT_index) - FT_diff << "," - << kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index) - kuka_diff - << std::endl; - FT_index++; - kuka_index++; - } + // If the row value is the kuka time, then the FT cells should be left empty. + // Use the receive time as the baseline + // if ( kuka_local_receive_time(kuka_index) < FT_local_receive_time(FT_index) ){ + if ( kuka_local_request_time(kuka_index) < FT_local_request_time(FT_index) ){ + // write the data to the output file + auto jointrow = jointAngles.row(kuka_index); + fs << kuka_local_request_time(kuka_index) <<"," + << kuka_local_receive_time(kuka_index) << "," + << "," + << kuka_device_time(kuka_index) <<"," + << "," + << Y_kuka(kuka_index) << "," + << ",,,,,," + << jointrow[0] << "," + << jointrow[1] << "," + << jointrow[2] << "," + << jointrow[3] << "," + << jointrow[4] << "," + << jointrow[5] << "," + << jointrow[6] + << std::endl; + kuka_index++; + } else if( kuka_local_request_time(kuka_index) > FT_local_request_time(FT_index)) { + auto matrixrow = markerPose.row(FT_index); + // If the row value is the FT time, then the kuka cells should be left blank. + fs << FT_local_request_time(FT_index) << "," + << FT_local_receive_time(FT_index) <<"," + << FT_device_time(FT_index) << "," + << "," + << Y_FT(FT_index) << "," + << "," + << matrixrow[0] << "," + << matrixrow[1] << "," + << matrixrow[2] << "," + << matrixrow[3] << "," + << matrixrow[4] << "," + << matrixrow[5] << std::endl; + FT_index++; + } else { + // In case the time is extactly equivent with each other. + fs << FT_local_request_time(FT_index) << "," + << FT_local_receive_time(FT_index) <<"," + // << kuka_local_request_time(kuka_index) << "," + << FT_device_time(FT_index) << "," + << kuka_device_time(kuka_index) <<"," + << Y_FT(FT_index)<< "," + << Y_kuka(kuka_index) + << std::endl; + FT_index++; + kuka_index++; + } } std::cout << "FT_index: " << FT_index << " kuka_index: " << kuka_index << " Sum: " << FT_index+kuka_index << std::endl; fs.close(); @@ -869,14 +857,15 @@ namespace grl { // Skip the very beginning data,since the tracker starts to work once the scene is loaded in vrep. // But kuka starts to work only after clicking on the start button. // To combine the time from two devices, they should have the same starting time point. - while(kuka_index timeEventM_FT(FT_index,local_receive_time) && kuka_index == 0){ + while(FT_index timeEventM_FT(FT_index,timeBaseline_index ) && kuka_index == 0){ FT_index++; } - auto initial_local_time = std::min(timeEventM_FT(FT_index,local_receive_time), timeEventM_Kuka(kuka_index,local_receive_time)); + auto initial_local_time = std::min(timeEventM_FT(FT_index,timeBaseline_index ), timeEventM_Kuka(kuka_index,timeBaseline_index )); auto initial_device_time_kuka = timeEventM_Kuka(kuka_index,device_time); auto initial_device_time_FT = timeEventM_FT(FT_index,device_time); grl::VectorXd FT_local_request_time = timeEventM_FT.col(local_request_time) - initial_local_time * grl::VectorXd::Ones(FT_time_size); @@ -890,8 +879,12 @@ namespace grl { Eigen::MatrixXd jointAngles = grl::getAllJointAngles(kukaStatesP); - int64_t kuka_diff = kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index); - int64_t FT_diff = FT_device_time(FT_index) - FT_local_receive_time(FT_index); + int64_t kuka_diff = kuka_device_time(kuka_index) - kuka_local_request_time(kuka_index); + int64_t FT_diff = FT_device_time(FT_index) - FT_local_request_time(FT_index); + + grl::VectorXd Y_kuka = kuka_device_time - kuka_local_request_time - grl::VectorXd::Ones(kuka_time_size)*kuka_diff; + grl::VectorXd Y_FT = FT_device_time - FT_local_request_time - grl::VectorXd::Ones(FT_time_size)*FT_diff; + std::ofstream fs; // create a name for the file output @@ -926,13 +919,12 @@ namespace grl { if ( kuka_local_receive_time(kuka_index) < FT_local_receive_time(FT_index) ){ // write the data to the output file auto jointrow = jointAngles.row(kuka_index); - fs << kuka_local_receive_time(kuka_index) <<"," - <<"," - << kuka_local_request_time(kuka_index) << "," + fs << kuka_local_request_time(kuka_index) <<"," + << kuka_local_receive_time(kuka_index) << "," << "," << kuka_device_time(kuka_index) <<"," << "," - << kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index) - kuka_diff << "," + << Y_kuka(kuka_index) << "," << ",,,,,," << jointrow[0] << "," << jointrow[1] << "," @@ -943,15 +935,14 @@ namespace grl { << jointrow[6] << std::endl; kuka_index++; - } else if( kuka_local_receive_time(kuka_index) > FT_local_receive_time(FT_index)) { + } else if( kuka_local_request_time(kuka_index) > FT_local_request_time(FT_index)) { auto matrixrow = markerPose.row(FT_index); // If the row value is the FT time, then the kuka cells should be left blank. - fs << FT_local_receive_time(FT_index) <<"," - << FT_local_request_time(FT_index) << "," - <<"," + fs << FT_local_request_time(FT_index) <<"," + << FT_local_receive_time(FT_index) << "," << FT_device_time(FT_index) << "," << "," - << FT_device_time(FT_index) - FT_local_receive_time(FT_index) - FT_diff << "," + << Y_FT(FT_index) << "," << "," << matrixrow[0] << "," << matrixrow[1] << "," @@ -962,13 +953,12 @@ namespace grl { FT_index++; } else { // In case the time is extactly equivent with each other. - fs << FT_local_receive_time(FT_index) <<"," - << FT_local_request_time(FT_index) << "," - << kuka_local_request_time(kuka_index) << "," + fs << kuka_local_request_time(FT_index) <<"," + << kuka_local_receive_time(FT_index) << "," << FT_device_time(FT_index) << "," << kuka_device_time(kuka_index) <<"," - << FT_device_time(FT_index) - FT_local_receive_time(FT_index) - FT_diff << "," - << kuka_device_time(kuka_index) - kuka_local_receive_time(kuka_index) - kuka_diff + << Y_FT(FT_index) << "," + << Y_kuka(kuka_index) << std::endl; FT_index++; kuka_index++; diff --git a/test/readFlatbufferTest.cpp b/test/readFlatbufferTest.cpp index 80d5a0f..3d1c0c1 100644 --- a/test/readFlatbufferTest.cpp +++ b/test/readFlatbufferTest.cpp @@ -39,7 +39,8 @@ int main(int argc, char* argv[]) std::string KUKA_FRI_CSVfilename = foldname + foldtimestamp + "/KUKA_FRIMessage.csv"; std::string KUKA_TimeEvent_CSV = foldname + foldtimestamp + "/KUKA_TimeEvent.csv"; - std::string KUKA_Joint_CSV = foldname + foldtimestamp + "/KUKA_Joint.csv"; + std::string M_Joint_CSV = foldname + foldtimestamp + "/KUKA_Measured_Joint.csv"; + std::string C_Joint_CSV = foldname + foldtimestamp + "/KUKA_Command_Joint.csv"; std::string KUKA_Pose_CSV = foldname + foldtimestamp + "/KUKA_Pose.csv"; std::string KUKA_Inverse_Pose_CSV = foldname + foldtimestamp + "/Inverse_KUKA_Pose.csv"; std::string FudicialToRobotPose_CSV = foldname + foldtimestamp + "/FudicialToRobot_Pose.csv"; @@ -109,14 +110,14 @@ int main(int argc, char* argv[]) // Skip the very beginning data,since the tracker starts to work once the scene is loaded in vrep. // But kuka starts to work only after clicking on the start button. // To combine the time from two devices, they should have the same starting time point. - while(kuka_index timeEventM_FT(FT_index,grl::TimeType::local_receive_time) ){ + while(kuka_index == 0 && FT_index timeEventM_FT(FT_index,grl::timeBaseline_index) ){ FT_index++; } - auto initial_local_time = std::min(timeEventM_FT(FT_index,grl::TimeType::local_receive_time), timeEventM_Kuka(kuka_index, grl::TimeType::local_receive_time)); + auto initial_local_time = std::min(timeEventM_FT(FT_index,grl::timeBaseline_index), timeEventM_Kuka(kuka_index, grl::timeBaseline_index)); auto initial_device_time_kuka = timeEventM_Kuka(kuka_index,grl::TimeType::device_time); auto initial_device_time_FT = timeEventM_FT(FT_index,grl::TimeType::device_time); std::cout<< "Main initial_local_time: " << initial_local_time << std::endl; @@ -151,7 +152,9 @@ int main(int argc, char* argv[]) std::vector kuka_labels = grl::Time_Labels; kuka_labels.insert(std::end(kuka_labels), std::begin(grl::M_Pos_Joint_Labels), std::end(grl::M_Pos_Joint_Labels)); // Write the joint angles into csv - grl::writeMatrixToCSV(KUKA_Joint_CSV, kuka_labels, timeEventM_Kuka, measuredJointPosition); + grl::writeMatrixToCSV(M_Joint_CSV, kuka_labels, timeEventM_Kuka, measuredJointPosition); + std::copy(std::begin(grl::C_Pos_Joint_Labels), std::end(grl::C_Pos_Joint_Labels),std::begin(kuka_labels)+grl::Time_Labels.size()); + grl::writeMatrixToCSV(C_Joint_CSV, kuka_labels, timeEventM_Kuka, commandedJointPosition); std::vector PK_Pose_Labels = grl::getLabels(grl::LabelsType::Kuka_Pose); // forward kinematic to get the of the end effector // markerPose_FT = true; // Get the marker pose directly, or get the end effector pose. @@ -214,12 +217,6 @@ int main(int argc, char* argv[]) grl::writeMatrixToCSV(FT_Marker50000_CSV, FT_Labels_Pose, timeEventM_FT, markerPose_FT, FT_index); } - - - - - - // grl::writeFTKUKAMessageToCSV(FTKUKA_TimeEvent_CSV, logKUKAiiwaFusionTrackP, kukaStatesP, markerID_22); return 0; } From 6984f3f1b3ba6759e6bead832591961480eaa90c Mon Sep 17 00:00:00 2001 From: Chunting Date: Wed, 4 Apr 2018 17:04:29 -0400 Subject: [PATCH 071/102] Fix hard code --- config/FindFlatBuffers.cmake.deleteme | 160 ++++++++++++++++++ example/fusionTrackExample.cpp | 3 +- .../grl/flatbuffer/ParseflatbuffertoCSV.hpp | 33 +++- .../grl/vrep/InverseKinematicsVrepPlugin.hpp | 42 +++-- test/HandEyeCalibration_test.cpp | 11 +- test/KukaPoseEstimationTest.cpp | 11 +- test/readFlatbufferTest.cpp | 73 ++++---- 7 files changed, 279 insertions(+), 54 deletions(-) create mode 100644 config/FindFlatBuffers.cmake.deleteme diff --git a/config/FindFlatBuffers.cmake.deleteme b/config/FindFlatBuffers.cmake.deleteme new file mode 100644 index 0000000..6f3a326 --- /dev/null +++ b/config/FindFlatBuffers.cmake.deleteme @@ -0,0 +1,160 @@ +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# Tries to find Flatbuffers headers and libraries. +# +# Usage of this module as follows: +# +# find_package(Flatbuffers) +# +# Variables used by this module, they can change the default behaviour and need +# to be set before calling find_package: +# +# Flatbuffers_HOME - +# When set, this path is inspected instead of standard library locations as +# the root of the Flatbuffers installation. The environment variable +# FLATBUFFERS_HOME overrides this veriable. +# +# This module defines +# FLATBUFFERS_INCLUDE_DIR, directory containing headers +# FLATBUFFERS_LIBS, directory containing flatbuffers libraries +# FLATBUFFERS_STATIC_LIB, path to libflatbuffers.a +# FLATBUFFERS_FOUND, whether flatbuffers has been found + +if( NOT "$ENV{FLATBUFFERS_HOME}" STREQUAL "") +file( TO_CMAKE_PATH "$ENV{FLATBUFFERS_HOME}" _native_path ) +list( APPEND _flatbuffers_roots ${_native_path} ) +elseif ( Flatbuffers_HOME ) +list( APPEND _flatbuffers_roots ${Flatbuffers_HOME} ) +endif() + +# Try the parameterized roots, if they exist +if ( _flatbuffers_roots ) +find_path( FLATBUFFERS_INCLUDE_DIR NAMES flatbuffers/flatbuffers.h + PATHS ${_flatbuffers_roots} NO_DEFAULT_PATH + PATH_SUFFIXES "include" ) +find_library( FLATBUFFERS_LIBRARIES NAMES flatbuffers + PATHS ${_flatbuffers_roots} NO_DEFAULT_PATH + PATH_SUFFIXES "lib" ) +else () +find_path( FLATBUFFERS_INCLUDE_DIR NAMES flatbuffers/flatbuffers.h ) +find_library( FLATBUFFERS_LIBRARIES NAMES flatbufferFLATBUFFERS_FLATC_EXECUTABLEs ) +endif () + +find_program(FLATBUFFERS_COMPILER flatc +PATH +$ENV{FLATBUFFERS_HOME}/bin +${_flatbuffers_roots}/bin +/usr/local/bin +/usr/bin +NO_DEFAULT_PATH +) + +if (FLATBUFFERS_INCLUDE_DIR AND FLATBUFFERS_LIBRARIES) +set(FLATBUFFERS_FOUND TRUE) +get_filename_component( FLATBUFFERS_LIBS ${FLATBUFFERS_LIBRARIES} PATH ) +set(FLATBUFFERS_LIB_NAME libflatbuffers) +set(FLATBUFFERS_STATIC_LIB ${FLATBUFFERS_LIBS}/${FLATBUFFERS_LIB_NAME}.a) +else () +set(FLATBUFFERS_FOUND FALSE) +endif () + +if (FLATBUFFERS_FOUND) +if (NOT Flatbuffers_FIND_QUIETLY) +message(STATUS "Found the Flatbuffers library: ${FLATBUFFERS_LIBRARIES}") +endif ()FLATBUFFERS_COMPILER +else () +if (NOT Flatbuffers_FIND_QUIETLY) +set(FLATBUFFERS_ERR_MSG "Could not find the Flatbuffers library. Looked in ") +if ( _flatbuffers_roots ) + set(FLATBUFFERS_ERR_MSG "${FLATBUFFERS_ERR_MSG} in ${_flatbuffers_roots}.") +else () + set(FLATBUFFERS_ERR_MSG "${FLATBUFFERS_ERR_MSG} system search paths.") +endif () +if (Flatbuffers_FIND_REQUIRED) + message(FATAL_ERROR "${FLATBUFFERS_ERR_MSG}") +else (Flatbuffers_FIND_REQUIRED) + message(STATUS "${FLATBUFFERS_ERR_MSG}") +endif (Flatbuffers_FIND_REQUIRED) +endif () +endif () + +mark_as_advanced( +FLATBUFFERS_INCLUDE_DIR +FLATBUFFERS_LIBS +FLATBUFFERS_STATIC_LIB +FLATBUFFERS_COMPILER +) + +if(FLATBUFFERS_FOUND) + function(FLATBUFFERS_GENERATE_C_HEADERS Name FLATBUFFERS_DIR OUTPUT_DIR) + # Name is the name of the user defined variable that will be created by this function + # Another variable that will be set is ${NAME}_OUTPUTS which will be set to the names + # of all output files that have been generated.FLATBUFFERS_COMPILER + # FLATBUFFERS_DIR is the directory in which to look for the .fbs files + # OUTPUT_DIR is the directory in which all output files should be placed + set(FLATC_OUTPUTS) + foreach(FILE ${ARGN}) + get_filename_component(FLATC_OUTPUT ${FILE} NAME_WE) + # create a target for the specific flatbuffers file + set(FBS_FILE_COPY_INCLUDE copy_${FLATC_OUTPUT}_to_include) + set(FBS_FILE_COPY_BIN copy_${FLATC_OUTPUT}_to_bin) + # create a target for the generated output cpp file + set(FLATC_OUTPUT + "${OUTPUT_DIR}/${FLATC_OUTPUT}_generated.h") + list(APPEND FLATC_OUTPUTS ${FLATC_OUTPUT} ${FBS_FILE_COPY_INCLUDE} ${FBS_FILE_COPY_BIN}) + + # this is the absolute path to the actual filename.fbs file + set(ABSOLUTE_FBS_FILE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/${FLATBUFFERS_DIR}/${FILE}) + + add_custom_command(OUTPUT ${FLATC_OUTPUT} + COMMAND ${FLATBUFFERS_COMPILER} + # Note: We are setting several custom parameters here to make life easier. + # see flatbuffers documentation for details. + # flatc --gen-name-strings --scoped-enums --gen-object-api -c -j -p -o + # see https://google.github.io/flatbuffers/flatbuffers_guide_using_schema_compiler.html + ARGS --gen-name-strings --scoped-enums --gen-object-api -c -j -p -o "${OUTPUT_DIR}" ${FILE} + MAIN_DEPENDENCY ${ABSOLUTE_FBS_FILE_PATH} + COMMENT "Building C++, Java, and Python header for ${FILE}" + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/${FLATBUFFERS_DIR}) + + # need to copy the flatbuffers schema so config files can be loaded + # http://stackoverflow.com/a/13429998/99379 + # CMAKE_CURRENT_SOURCE_DIR + # this is the directory where the currently processed CMakeLists.txt is located in + # terminal copy commands change between OS versions, so we use CMake's built in file + # copy command which runs with "cmake -E copy file_to_copy file_destination" + # we use some variables here so the path is reproducible. + add_custom_command(OUTPUT ${FBS_FILE_COPY_INCLUDE} + COMMAND ${CMAKE_COMMAND} ARGS -E copy ${ABSOLUTE_FBS_FILE_PATH} ${OUTPUT_DIR} + MAIN_DEPENDENCY ${CMAKE_CURRENT_SOURCE_DIR}/${FLATBUFFERS_DIR}/${FILE} + COMMENT "Copying fbs file ${FILE} to ${OUTPUT_DIR}" + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/${FLATBUFFERS_DIR} + ) + + add_custom_command(OUTPUT ${FBS_FILE_COPY_BIN} + # TODO(ahundt) remove hacky /bin manually set path, this will break for some IDEs + COMMAND ${CMAKE_COMMAND} ARGS -E copy ${ABSOLUTE_FBS_FILE_PATH} ${CMAKE_BINARY_DIR}/bin + MAIN_DEPENDENCY ${CMAKE_CURRENT_SOURCE_DIR}/${FLATBUFFERS_DIR}/${FILE} + COMMENT "Copying fbs file ${FILE} to ${CMAKE_BINARY_DIR}" + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/${FLATBUFFERS_DIR} + ) + endforeach() + set(${Name}_OUTPUTS ${FLATC_OUTPUTS} PARENT_SCOPE) + endfunction() + + set(FLATBUFFERS_INCLUDE_DIRS ${FLATBUFFERS_INCLUDE_DIR}) + include_directories(${CMAKE_BINARY_DIR}) +else() + set(FLATBUFFERS_INCLUDE_DIR) +endif() diff --git a/example/fusionTrackExample.cpp b/example/fusionTrackExample.cpp index 459c700..e254ca1 100644 --- a/example/fusionTrackExample.cpp +++ b/example/fusionTrackExample.cpp @@ -87,8 +87,7 @@ int main(int argc, char **argv) std::string json_file_prefix = "test_text_"; std::string json_file_suffix = ".json"; std::string fbs_filename("LogKUKAiiwaFusionTrack.fbs"); - // std::string includePath = grl::getpathtofbsfile(fbs_filename); - std::string includePath = "/home/cjiao1/src/robonetracker/build/"; + std::size_t builder_size_bytes = 0; diff --git a/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp b/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp index 8d68f5f..a73b901 100644 --- a/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp +++ b/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp @@ -1037,7 +1037,7 @@ namespace grl { int getRowsNumber(std::string filename){ - // std::ifstream file("/home/cjiao1/src/V-REP_PRO_EDU_V3_4_0_Linux/2018_03_20_20_35_20/KUKA_Joint.csv"); + std::ifstream file(filename); std::size_t row_size = 0; @@ -1059,7 +1059,7 @@ int getRowsNumber(std::string filename){ /// @return jointPosition, Eigen vector which contains joint position of the seven joints. int64_t getJointAnglesFromCSV(std::string filename, Eigen::VectorXf &jointPosition, int rowIdx){ - // std::ifstream file("/home/cjiao1/src/V-REP_PRO_EDU_V3_4_0_Linux/2018_03_20_20_35_20/KUKA_Joint.csv"); + // int rowNum = getRowsNumber(filename); std::ifstream file(filename); std::size_t joint_size = 7; @@ -1122,6 +1122,35 @@ int getRowsNumber(std::string filename){ return markerpose; } + void writeRBDytoFile(rbd::MultiBodyGraph& rbd_mbg_, rbd::MultiBody &rbd_mbs_, rbd::MultiBodyConfig& rbd_mbcs_) { + std::size_t nrBodies = rbd_mbs_.nrBodies(); + std::size_t nrJoints = rbd_mbs_.nrJoints(); + for(int bIdx=0; bIdx jointNames; + // std::cout<<"Joint Size: "<< nrJoints << std::endl; + + } + diff --git a/include/grl/vrep/InverseKinematicsVrepPlugin.hpp b/include/grl/vrep/InverseKinematicsVrepPlugin.hpp index ac22c4a..9c5509e 100644 --- a/include/grl/vrep/InverseKinematicsVrepPlugin.hpp +++ b/include/grl/vrep/InverseKinematicsVrepPlugin.hpp @@ -394,10 +394,9 @@ class InverseKinematicsVrepPlugin */ rbd_mbg_.linkBodies(prevBody, to, nextBody, from, curJoint); - - } + // Can't understand this comment, since didn't see the inverse of v-rep? // note: Tasks takes transforms in the successor (child link) frame, so it is the inverse of v-rep @@ -410,6 +409,8 @@ class InverseKinematicsVrepPlugin auto& simArmMultiBody = rbd_mbs_[simulatedRobotIndex]; auto& simArmConfig = rbd_mbcs_[simulatedRobotIndex]; + + // update the simulated arm position for (std::size_t i = 0; i < jointHandles_.size(); ++i) { simSetJointPosition(jointHandles_[i],initialJointAngles[i]); @@ -533,8 +534,9 @@ void applyEstimate(){ auto transform = getObjectTransform(opticalTrackerDetectedObjectHandle, robotTipHandle); std::cout<< "Before resetting the hand eye calibration: " << std::endl << transform.matrix() << std::endl; setObjectTransform(opticalTrackerDetectedObjectHandle, robotTipHandle, transformEstimate); - // auto myFile = boost::filesystem::current_path() /"RoboneSimulation_private_calibration_2.ttt"; - std::string sceneName = "/home/cjiao1/src/robonetracker/modules/roboneprivate/data/RoboneSimulation_private_calibration_2.ttt"; + auto myFile = boost::filesystem::current_path().string(); + std::cout << "current path: " << myFile << std::endl; + std::string sceneName = "/home/chunting/src/robonetracker/modules/roboneprivate/data/RoboneSimulation_private_calibration_2.ttt"; simSaveScene(sceneName.c_str()); transform = getObjectTransform(opticalTrackerDetectedObjectHandle, robotTipHandle); std::cout<< "After resetting the hand eye calibration: " << std::endl << transform.matrix() << std::endl; @@ -645,6 +647,15 @@ void getPoseFromCSV(std::string filename, int time_index){ auto& simArmMultiBody = rbd_mbs_[simulatedRobotIndex]; auto& simArmConfig = rbd_mbcs_[simulatedRobotIndex]; + auto strRobot = grl::getURDFModel(); + rbd::MultiBody mb = strRobot.mb; + rbd::MultiBodyConfig mbc(mb); + rbd::MultiBodyGraph mbg(strRobot.mbg); + std::size_t nrJoints = mb.nrJoints(); + std::size_t nrBodies = strRobot.mb.nrBodies(); + std::vector jointNames; + std::cout<<"Joint Size: "<< nrJoints << std::endl; + std::vector vrepJointAngles; double angle = 0.7; for(auto i : jointHandles_) @@ -654,13 +665,19 @@ void getPoseFromCSV(std::string filename, int time_index){ vrepJointAngles.push_back(angle); } + + /// @todo TODO(ahundt) rethink where/when/how to send command for the joint angles. Return to LUA? Set Directly? Distribute via vrep send message command? //////////////////////////////////////////////////// // Set joints to current arm position in simulation - SetRBDynArmFromVrep(jointNames_,jointHandles_,simArmMultiBody,simArmConfig); - rbd::forwardKinematics(simArmMultiBody, simArmConfig); - rbd::forwardVelocity(simArmMultiBody, simArmConfig); + // SetRBDynArmFromVrep(jointNames_,jointHandles_,simArmMultiBody,simArmConfig); + // rbd::forwardKinematics(simArmMultiBody, simArmConfig); + // rbd::forwardVelocity(simArmMultiBody, simArmConfig); + + SetRBDynArmFromVrep(jointNames_,jointHandles_,mb,mbc); + rbd::forwardKinematics(mb, mbc); + rbd::forwardVelocity(mb, mbc); debugFrames(); } @@ -950,13 +967,12 @@ void getPoseFromCSV(std::string filename, int time_index){ std::string pathName = myFile.string(); std::string kukaJoint = pathName+"/KUKA_Joint.csv"; // KUKA_Joint.csv FTKUKA_TimeEvent.csv std::string markerPose = pathName+"/FT_Pose_Marker22.csv"; - - // std::cout << "Call Run One..." << std::endl; - // testPose(); - // applyEstimate(); - getPoseFromCSV(kukaJoint, time_index); + + testPose(); + applyEstimate(); + // getPoseFromCSV(kukaJoint, time_index); // replayMarker(markerPose, time_index); - time_index++; + // time_index++; } } diff --git a/test/HandEyeCalibration_test.cpp b/test/HandEyeCalibration_test.cpp index 3d005d3..51c7fc6 100644 --- a/test/HandEyeCalibration_test.cpp +++ b/test/HandEyeCalibration_test.cpp @@ -8,12 +8,21 @@ #include "camodocal/calib/HandEyeCalibration.h" #include "camodocal/EigenUtils.h" +// boost::filesystem +#include +#include +#include + + BOOST_AUTO_TEST_SUITE(HandEyeCalibration_test) BOOST_AUTO_TEST_CASE(FullMotion) { - camodocal::HandEyeCalibration::setVerbose(false); + std::string currentPath = boost::filesystem::current_path().string(); + std::cout << "${workspaceFolder}: " << currentPath << std::endl; + camodocal::HandEyeCalibration::setVerbose(false); + Eigen::Matrix4d H_12_expected = Eigen::Matrix4d::Identity(); H_12_expected.block<3,3>(0,0) = Eigen::AngleAxisd(0.4, Eigen::Vector3d(0.1, 0.2, 0.3).normalized()).toRotationMatrix(); H_12_expected.block<3,1>(0,3) << 0.5, 0.6, 0.7; diff --git a/test/KukaPoseEstimationTest.cpp b/test/KukaPoseEstimationTest.cpp index 5e68ed7..d02191a 100644 --- a/test/KukaPoseEstimationTest.cpp +++ b/test/KukaPoseEstimationTest.cpp @@ -59,14 +59,15 @@ const double RadtoDegree = 180/3.14159265359; const double MeterToMM = 1000; -std::string foldname = "/home/cjiao1/src/V-REP_PRO_EDU_V3_4_0_Linux/"; +// "/home/chunting/src/V-REP_PRO_EDU_V3_4_0_Linux/"; +std::string currentPath = boost::filesystem::current_path().string(); std::string foldtimestamp = current_date_and_time_string(); -std::string kukaBinaryfile = foldname + "2018_02_26_14_23_14_Kukaiiwa.iiwa"; -std::string KUKA_TimeEvent_CSVfilename = foldname + foldtimestamp + "/KUKA_TimeEvent.csv"; -std::string KUKA_Joint_CSVfilename = foldname + foldtimestamp + "/KUKA_Joint.csv"; -std::string KUKA_Pose_CSVfilename = foldname + foldtimestamp + "/KUKA_Pose.csv"; +std::string kukaBinaryfile = currentPath + "2018_02_26_14_23_14_Kukaiiwa.iiwa"; +std::string KUKA_TimeEvent_CSVfilename = currentPath + foldtimestamp + "/KUKA_TimeEvent.csv"; +std::string KUKA_Joint_CSVfilename = currentPath + foldtimestamp + "/KUKA_Joint.csv"; +std::string KUKA_Pose_CSVfilename = currentPath + foldtimestamp + "/KUKA_Pose.csv"; diff --git a/test/readFlatbufferTest.cpp b/test/readFlatbufferTest.cpp index 3d1c0c1..5f47fce 100644 --- a/test/readFlatbufferTest.cpp +++ b/test/readFlatbufferTest.cpp @@ -18,7 +18,10 @@ #include #include /// Boost to create an empty folder +// boost::filesystem #include +#include +#include /// split -C 200m --numeric-suffixes 2018_02_28_16_39_13_FusionTrack.json 2018_02_28_16_39_13_FusionTrack @@ -27,41 +30,38 @@ int main(int argc, char* argv[]) { + std::string kukaTimeStamp("2018_03_26_19_06_21_Kukaiiwa.iiwa"); + std::string FTTimeStamp("2018_03_26_19_06_21_FusionTrack.flik"); /// Define the csv file names - std::string foldname = "/home/cjiao1/src/V-REP_PRO_EDU_V3_4_0_Linux/"; - std::string kukaTimeStamp = "2018_03_26_19_06_21_Kukaiiwa"; - std::string FTTimeStamp = "2018_03_26_19_06_21_FusionTrack"; - std::string kukaBinaryfile = foldname + kukaTimeStamp+ ".iiwa"; - std::string fusiontrackBinaryfile = foldname + FTTimeStamp +".flik"; - std::string foldtimestamp = current_date_and_time_string(); - boost::filesystem::path dir{foldname+foldtimestamp}; + if(argc == 2){ + kukaTimeStamp = std::string(argv[1]); + FTTimeStamp = std::string(argv[2]); + + } + std::string currentPath = boost::filesystem::current_path().string(); //"/home/chunting/src/V-REP_PRO_EDU_V3_4_0_Linux/"; + std::string kukaBinaryfile = currentPath + kukaTimeStamp; + std::string fusiontrackBinaryfile = currentPath + FTTimeStamp; + std::string foldtimestamp = current_date_and_time_string(); // Write the generated files into a new fold + boost::filesystem::path dir{currentPath+foldtimestamp}; boost::filesystem::create_directory(dir); - std::string KUKA_FRI_CSVfilename = foldname + foldtimestamp + "/KUKA_FRIMessage.csv"; - std::string KUKA_TimeEvent_CSV = foldname + foldtimestamp + "/KUKA_TimeEvent.csv"; - std::string M_Joint_CSV = foldname + foldtimestamp + "/KUKA_Measured_Joint.csv"; - std::string C_Joint_CSV = foldname + foldtimestamp + "/KUKA_Command_Joint.csv"; - std::string KUKA_Pose_CSV = foldname + foldtimestamp + "/KUKA_Pose.csv"; - std::string KUKA_Inverse_Pose_CSV = foldname + foldtimestamp + "/Inverse_KUKA_Pose.csv"; - std::string FudicialToRobotPose_CSV = foldname + foldtimestamp + "/FudicialToRobot_Pose.csv"; - std::string FudicialToFTPose_CSV = foldname + foldtimestamp + "/FudicialToFT_Pose.csv"; - - std::string FT_TimeEvent_CSV = foldname + foldtimestamp + "/FT_TimeEvent.csv"; - std::string FT_Marker22_CSV = foldname + foldtimestamp + "/FT_Pose_Marker22.csv"; - std::string FT_Marker55_CSV = foldname + foldtimestamp + "/FT_Pose_Marker55.csv"; - std::string FT_Marker50000_CSV = foldname + foldtimestamp + "/FT_Pose_Marker50000.csv"; - - std::string FTKUKA_TimeEvent_CSV = foldname + foldtimestamp + "/FTKUKA_TimeEvent.csv"; - - auto strRobot = grl::getURDFModel(); - rbd::MultiBody mb = strRobot.mb; - rbd::MultiBodyConfig mbc(mb); - rbd::MultiBodyGraph mbg(strRobot.mbg); - std::size_t nrJoints = mb.nrJoints(); - std::size_t nrBodies = strRobot.mb.nrBodies(); - std::vector jointNames; - std::cout<<"Joint Size: "<< nrJoints << std::endl; + std::string KUKA_FRI_CSVfilename = currentPath + foldtimestamp + "/KUKA_FRIMessage.csv"; + std::string KUKA_TimeEvent_CSV = currentPath + foldtimestamp + "/KUKA_TimeEvent.csv"; + std::string M_Joint_CSV = currentPath + foldtimestamp + "/KUKA_Measured_Joint.csv"; + std::string C_Joint_CSV = currentPath + foldtimestamp + "/KUKA_Command_Joint.csv"; + std::string KUKA_Pose_CSV = currentPath + foldtimestamp + "/KUKA_Pose.csv"; + std::string KUKA_Inverse_Pose_CSV = currentPath + foldtimestamp + "/Inverse_KUKA_Pose.csv"; + std::string FudicialToRobotPose_CSV = currentPath + foldtimestamp + "/FudicialToRobot_Pose.csv"; + std::string FudicialToFTPose_CSV = currentPath + foldtimestamp + "/FudicialToFT_Pose.csv"; + + std::string FT_TimeEvent_CSV = currentPath + foldtimestamp + "/FT_TimeEvent.csv"; + std::string FT_Marker22_CSV = currentPath + foldtimestamp + "/FT_Pose_Marker22.csv"; + std::string FT_Marker55_CSV = currentPath + foldtimestamp + "/FT_Pose_Marker55.csv"; + std::string FT_Marker50000_CSV = currentPath + foldtimestamp + "/FT_Pose_Marker50000.csv"; + + std::string FTKUKA_TimeEvent_CSV = currentPath + foldtimestamp + "/FTKUKA_TimeEvent.csv"; + // Put all the data into the same coordinate system --- Marker frame bool inMarkerFrame = false; // Indicate the marker pose is in marker frame or tracker frame. uint32_t markerID_22 = 22; @@ -155,6 +155,17 @@ int main(int argc, char* argv[]) grl::writeMatrixToCSV(M_Joint_CSV, kuka_labels, timeEventM_Kuka, measuredJointPosition); std::copy(std::begin(grl::C_Pos_Joint_Labels), std::end(grl::C_Pos_Joint_Labels),std::begin(kuka_labels)+grl::Time_Labels.size()); grl::writeMatrixToCSV(C_Joint_CSV, kuka_labels, timeEventM_Kuka, commandedJointPosition); + + + auto strRobot = grl::getURDFModel(); + rbd::MultiBody mb = strRobot.mb; + rbd::MultiBodyConfig mbc(mb); + rbd::MultiBodyGraph mbg(strRobot.mbg); + std::size_t nrJoints = mb.nrJoints(); + std::size_t nrBodies = strRobot.mb.nrBodies(); + std::vector jointNames; + std::cout<<"Joint Size: "<< nrJoints << std::endl; + std::vector PK_Pose_Labels = grl::getLabels(grl::LabelsType::Kuka_Pose); // forward kinematic to get the of the end effector // markerPose_FT = true; // Get the marker pose directly, or get the end effector pose. From 0a040c4bba6bec5075bf0279b5761f917b4fd1dc Mon Sep 17 00:00:00 2001 From: Chunting Date: Wed, 4 Apr 2018 23:10:40 -0400 Subject: [PATCH 072/102] Remove hand codes. --- .../grl/vrep/HandEyeCalibrationVrepPlugin.hpp | 12 +++- .../grl/vrep/InverseKinematicsVrepPlugin.hpp | 56 +++---------------- src/lua/robone.lua | 4 +- .../camodocal/HandEyeCalibration.cpp | 1 - .../v_repExtHandEyeCalibration.cpp | 28 +++++++++- test/readFlatbufferTest.cpp | 2 +- 6 files changed, 50 insertions(+), 53 deletions(-) diff --git a/include/grl/vrep/HandEyeCalibrationVrepPlugin.hpp b/include/grl/vrep/HandEyeCalibrationVrepPlugin.hpp index 62c6106..a7789ff 100644 --- a/include/grl/vrep/HandEyeCalibrationVrepPlugin.hpp +++ b/include/grl/vrep/HandEyeCalibrationVrepPlugin.hpp @@ -217,10 +217,20 @@ void estimateHandEyeScrew(){ /// /// A default transform is saved when construct() was called /// so if no estimate has been found that will be used. -void applyEstimate(){ +void applyEstimate(std::string sceneName=""){ // set transform between end effector and fiducial setObjectTransform(opticalTrackerDetectedObjectName, robotTip, transformEstimate); + if(!sceneName.empty()){ + int ret = simSaveScene(sceneName.c_str()); + if(ret == -1){ + logger_->info( "\n{} fails to be saved...", sceneName); + } else { + logger_->info( "\n{} has been saved...", sceneName); + } + + } + } Eigen::Affine3d getEstimate(){ diff --git a/include/grl/vrep/InverseKinematicsVrepPlugin.hpp b/include/grl/vrep/InverseKinematicsVrepPlugin.hpp index 9c5509e..c0b5701 100644 --- a/include/grl/vrep/InverseKinematicsVrepPlugin.hpp +++ b/include/grl/vrep/InverseKinematicsVrepPlugin.hpp @@ -96,6 +96,8 @@ #include "grl/flatbuffer/kukaiiwaURDF.h" #include "grl/flatbuffer/CSVIterator.hpp" +#include + namespace grl { namespace vrep { @@ -508,47 +510,8 @@ class InverseKinematicsVrepPlugin setObjectTransform(simGetObjectHandle("Dummy"),-1,tipTf); } -/// @brief Will apply the stored estimate to the v-rep simulation value -/// -/// A default transform is saved when construct() was called -/// so if no estimate has been found that will be used. -void applyEstimate(){ - - std::string opticalTrackerDetectedObjectName("Fiducial#22"); - std::string robotTipName("RobotFlangeTip"); - int opticalTrackerDetectedObjectHandle = simGetObjectHandle(opticalTrackerDetectedObjectName.c_str()); - int robotTipHandle = simGetObjectHandle(robotTipName.c_str()); - Eigen::Affine3d transformEstimate = getObjectTransform(opticalTrackerDetectedObjectHandle, robotTipHandle); - Eigen::Quaterniond eigenQuat(0.105064, 0.0503505, 0.65479, 0.746777); //wxyz - // 0.0251104, 0.00752018, 0.996255, 0.0823903 - // 0.105045, 0.0502289 , 0.654843 , 0.746742 - - Eigen::Vector3d eigenPos(0.0753178, 0.0973988, 0.088042); - // 0.0754499, 0.0975167, 0.0882869 - // 0.0388255, 0.102084, 0.0902271 - - transformEstimate = eigenQuat; - transformEstimate.translation() = eigenPos; - - // set transform between end effector and fiducial - auto transform = getObjectTransform(opticalTrackerDetectedObjectHandle, robotTipHandle); - std::cout<< "Before resetting the hand eye calibration: " << std::endl << transform.matrix() << std::endl; - setObjectTransform(opticalTrackerDetectedObjectHandle, robotTipHandle, transformEstimate); - auto myFile = boost::filesystem::current_path().string(); - std::cout << "current path: " << myFile << std::endl; - std::string sceneName = "/home/chunting/src/robonetracker/modules/roboneprivate/data/RoboneSimulation_private_calibration_2.ttt"; - simSaveScene(sceneName.c_str()); - transform = getObjectTransform(opticalTrackerDetectedObjectHandle, robotTipHandle); - std::cout<< "After resetting the hand eye calibration: " << std::endl << transform.matrix() << std::endl; -// logger_->info( "Hand Eye Screw Estimation has been set quat wxyz\n: {} , {} , {} , {} \n translation xyz: {} {} {}", -// eigenQuat.w(), eigenQuat.x(), eigenQuat.y(),eigenQuat.z(), transformEstimate.translation().x(), transformEstimate.translation().y(), transformEstimate.translation().z()); -} void getPoseFromCSV(std::string filename, int time_index){ // we only have one robot for the moment so the index of it is 0 - // loadFromBinary(); - // if(time_index == 0){ - // applyEstimate(); - // } const std::size_t simulatedRobotIndex = 0; auto& simArmMultiBody = rbd_mbs_[simulatedRobotIndex]; auto& simArmConfig = rbd_mbcs_[simulatedRobotIndex]; @@ -671,13 +634,13 @@ void getPoseFromCSV(std::string filename, int time_index){ //////////////////////////////////////////////////// // Set joints to current arm position in simulation - // SetRBDynArmFromVrep(jointNames_,jointHandles_,simArmMultiBody,simArmConfig); - // rbd::forwardKinematics(simArmMultiBody, simArmConfig); - // rbd::forwardVelocity(simArmMultiBody, simArmConfig); + SetRBDynArmFromVrep(jointNames_,jointHandles_,simArmMultiBody,simArmConfig); + rbd::forwardKinematics(simArmMultiBody, simArmConfig); + rbd::forwardVelocity(simArmMultiBody, simArmConfig); - SetRBDynArmFromVrep(jointNames_,jointHandles_,mb,mbc); - rbd::forwardKinematics(mb, mbc); - rbd::forwardVelocity(mb, mbc); + // SetRBDynArmFromVrep(jointNames_,jointHandles_,mb,mbc); + // rbd::forwardKinematics(mb, mbc); + // rbd::forwardVelocity(mb, mbc); debugFrames(); } @@ -963,13 +926,12 @@ void getPoseFromCSV(std::string filename, int time_index){ if(ik) { updateKinematics(); } else { - auto myFile = boost::filesystem::current_path() /"2018_03_30_17_07_55"; + auto myFile = boost::filesystem::current_path() /"data_in"; std::string pathName = myFile.string(); std::string kukaJoint = pathName+"/KUKA_Joint.csv"; // KUKA_Joint.csv FTKUKA_TimeEvent.csv std::string markerPose = pathName+"/FT_Pose_Marker22.csv"; testPose(); - applyEstimate(); // getPoseFromCSV(kukaJoint, time_index); // replayMarker(markerPose, time_index); // time_index++; diff --git a/src/lua/robone.lua b/src/lua/robone.lua index c39a7a2..a37555e 100644 --- a/src/lua/robone.lua +++ b/src/lua/robone.lua @@ -392,7 +392,9 @@ robone.handEyeCalibScript=function() -- calculate the transform simExtHandEyeCalibFindTransform() - simExtHandEyeCalibApplyTransform() + -- apply the transform and save the scene + sceneName = "/home/cjiao1/src/robonetracker/modules/roboneprivate/data/RoboneSimulation_private_calibration_5.ttt" + simExtHandEyeCalibApplyTransform(sceneName) -- check for fusiontrack if (not grl.isModuleLoaded('AtracsysFusionTrack')) then diff --git a/src/v_repExtHandEyeCalibration/camodocal/HandEyeCalibration.cpp b/src/v_repExtHandEyeCalibration/camodocal/HandEyeCalibration.cpp index 1591b94..fed3167 100644 --- a/src/v_repExtHandEyeCalibration/camodocal/HandEyeCalibration.cpp +++ b/src/v_repExtHandEyeCalibration/camodocal/HandEyeCalibration.cpp @@ -143,7 +143,6 @@ HandEyeCalibration::estimateHandEyeScrew(const std::vectorapplyEstimate(); + CLuaFunctionData data; + + if (data.readDataFromLua(p,inXArgs_HAND_EYE_CALIB_SCENE_NAME,inXArgs_HAND_EYE_CALIB_SCENE_NAME[0], LUA_SIM_EXT_HAND_EYE_CALIB_APPLY_TRANSFORM_COMMAND)) + { + std::vector* inData=data.getInDataPtr(); + std::string sceneName((inData->at(0 ).stringData[0])); + handEyeCalibrationPG->applyEstimate(sceneName); + } + + } } @@ -217,6 +233,15 @@ VREP_DLLEXPORT unsigned char v_repStart(void* reservedPointer,int reservedInt) LUA_SIM_EXT_HAND_EYE_CALIB_START ); + CLuaFunctionData::getInputDataForFunctionRegistration(inXArgs_HAND_EYE_CALIB_SCENE_NAME,inArgs); + simRegisterCustomLuaFunction + ( + LUA_SIM_EXT_HAND_EYE_CALIB_APPLY_TRANSFORM_COMMAND, + LUA_SIM_EXT_HAND_EYE_CALIB_APPLY_TRANSFORM_CALL_TIP.c_str(), + &inArgs[0], + LUA_SIM_EXT_HAND_EYE_CALIB_APPLY_TRANSFORM + ); + int noArgs[]={0}; // no input arguments //simRegisterCustomLuaFunction("simExtHandEyeCalibStart","number result=simExtHandEyeCalibStart()",noArgs,LUA_SIM_EXT_HAND_EYE_CALIB_START); @@ -225,7 +250,6 @@ VREP_DLLEXPORT unsigned char v_repStart(void* reservedPointer,int reservedInt) simRegisterCustomLuaFunction("simExtHandEyeCalibReset","number result=simExtHandEyeCalibReset()",noArgs,LUA_SIM_EXT_HAND_EYE_CALIB_RESET); simRegisterCustomLuaFunction("simExtHandEyeCalibAddFrame","number result=simExtHandEyeCalibAddFrame()",noArgs,LUA_SIM_EXT_HAND_EYE_CALIB_ADD_FRAME); simRegisterCustomLuaFunction("simExtHandEyeCalibFindTransform","number result=simExtHandEyeCalibFindTransform()",noArgs,LUA_SIM_EXT_HAND_EYE_CALIB_FIND_TRANSFORM); - simRegisterCustomLuaFunction("simExtHandEyeCalibApplyTransform","number result=simExtHandEyeCalibApplyTransform()",noArgs,LUA_SIM_EXT_HAND_EYE_CALIB_APPLY_TRANSFORM); simRegisterCustomLuaFunction("simExtHandEyeCalibRestoreSensorPosition","number result=simExtHandEyeCalibRestoreSensorPosition()",noArgs,LUA_SIM_EXT_HAND_EYE_CALIB_RESTORE_SENSOR_POSITION); diff --git a/test/readFlatbufferTest.cpp b/test/readFlatbufferTest.cpp index 5f47fce..8894eca 100644 --- a/test/readFlatbufferTest.cpp +++ b/test/readFlatbufferTest.cpp @@ -38,7 +38,7 @@ int main(int argc, char* argv[]) FTTimeStamp = std::string(argv[2]); } - std::string currentPath = boost::filesystem::current_path().string(); //"/home/chunting/src/V-REP_PRO_EDU_V3_4_0_Linux/"; + std::string currentPath = boost::filesystem::current_path().string()+"/"; //"/home/cjiao1/src/V-REP_PRO_EDU_V3_4_0_Linux/"; std::string kukaBinaryfile = currentPath + kukaTimeStamp; std::string fusiontrackBinaryfile = currentPath + FTTimeStamp; std::string foldtimestamp = current_date_and_time_string(); // Write the generated files into a new fold From 4ddde2e7f2498fc1c375b972bd902dcb8a5692d4 Mon Sep 17 00:00:00 2001 From: Chunting Date: Thu, 5 Apr 2018 13:33:15 -0400 Subject: [PATCH 073/102] Edit c_cpp_properties.json to fix "can't find source files" --- include/grl/flatbuffer/flatbuffer.hpp | 23 +++++------------------ include/grl/kuka/KukaFRIdriver.hpp | 2 +- 2 files changed, 6 insertions(+), 19 deletions(-) diff --git a/include/grl/flatbuffer/flatbuffer.hpp b/include/grl/flatbuffer/flatbuffer.hpp index 4b2e066..7ce2370 100644 --- a/include/grl/flatbuffer/flatbuffer.hpp +++ b/include/grl/flatbuffer/flatbuffer.hpp @@ -2,30 +2,17 @@ #define GRL_FLATBUFFER_HPP #include -// source: http://www.codebind.com/cpp-tutorial/c-get-current-directory-linuxwindows/ -// #define WINDOWS /* uncomment this line to use it for windows.*/ -#ifdef WINDOWS -#include -#define GetCurrentDir _getcwd -#else -#include -#define GetCurrentDir getcwd -#endif - #include #include +// boost::filesystem +#include +#include +#include namespace grl { - std::string GetCurrentWorkingDir( void ) { - char buff[FILENAME_MAX]; - GetCurrentDir( buff, FILENAME_MAX ); - std::string current_working_dir(buff); - return current_working_dir; - } - // loads a json flatbuffer from a file bool LoadJSONFlatbuffer(flatbuffers::Parser& parser, std::string schemaPath, std::string jsonPath, std::string include_directory, bool binary = false) { @@ -72,7 +59,7 @@ namespace grl { if(includePaths.empty()) { - std::string current_working_dir = GetCurrentWorkingDir(); + std::string current_working_dir = boost::filesystem::current_path().string(); //std::cout << "The current working dir: " << current_working_dir << std::endl; includePaths.push_back(current_working_dir); } diff --git a/include/grl/kuka/KukaFRIdriver.hpp b/include/grl/kuka/KukaFRIdriver.hpp index 7c09740..8b49c17 100644 --- a/include/grl/kuka/KukaFRIdriver.hpp +++ b/include/grl/kuka/KukaFRIdriver.hpp @@ -704,7 +704,7 @@ class KukaFRIdriver : public std::enable_shared_from_thisinfo("currentWorkingDir ...: {}", currentWorkingDir); if(save_fbbP != nullptr && save_KUKAiiwaBufferP != nullptr) { bool success = grl::FinishAndVerifyBuffer(*save_fbbP, *save_KUKAiiwaBufferP); From e84cbaa0eb9e0242511ede0069febb4e003fab3e Mon Sep 17 00:00:00 2001 From: Chunting Date: Thu, 5 Apr 2018 13:35:18 -0400 Subject: [PATCH 074/102] Edit URDF file to make the joint and link name consistent with vrep model --- test/kukaiiwaURDF.h | 108 ++++++++++++++++++++++---------------------- 1 file changed, 54 insertions(+), 54 deletions(-) diff --git a/test/kukaiiwaURDF.h b/test/kukaiiwaURDF.h index 2e33135..f358cf2 100644 --- a/test/kukaiiwaURDF.h +++ b/test/kukaiiwaURDF.h @@ -56,12 +56,12 @@ R"( - + - + - + @@ -89,16 +89,16 @@ R"( - - - + + + - + @@ -120,16 +120,16 @@ R"( - - - + + + - + @@ -151,16 +151,16 @@ R"( - - - + + + - + @@ -182,16 +182,16 @@ R"( - - - + + + - + @@ -213,16 +213,16 @@ R"( - - - + + + - + @@ -244,16 +244,16 @@ R"( - - - + + + - + @@ -275,16 +275,16 @@ R"( - - - + + + - + @@ -305,21 +305,21 @@ R"( - - - + + + - + - - - + + + - + @@ -329,49 +329,49 @@ R"( - + Gazebo/Grey 0.2 0.2 - + Gazebo/Orange 0.2 0.2 - + Gazebo/Orange 0.2 0.2 - + Gazebo/Orange 0.2 0.2 - + Gazebo/Orange 0.2 0.2 - + Gazebo/Orange 0.2 0.2 - + Gazebo/Orange 0.2 0.2 - + Gazebo/Grey 0.2 0.2 @@ -379,7 +379,7 @@ R"( /iiwa transmission_interface/SimpleTransmission - + PositionJointInterface @@ -390,7 +390,7 @@ R"( /iiwa transmission_interface/SimpleTransmission - + PositionJointInterface @@ -401,7 +401,7 @@ R"( /iiwa transmission_interface/SimpleTransmission - + PositionJointInterface @@ -412,7 +412,7 @@ R"( /iiwa transmission_interface/SimpleTransmission - + PositionJointInterface @@ -423,7 +423,7 @@ R"( /iiwa transmission_interface/SimpleTransmission - + PositionJointInterface @@ -434,7 +434,7 @@ R"( /iiwa transmission_interface/SimpleTransmission - + PositionJointInterface @@ -445,7 +445,7 @@ R"( /iiwa transmission_interface/SimpleTransmission - + PositionJointInterface From 2da67fc1abdc832700d510b74a181b05f3e62d4d Mon Sep 17 00:00:00 2001 From: Chunting Date: Thu, 5 Apr 2018 16:47:55 -0400 Subject: [PATCH 075/102] Read urdf model from file Robone_KukaLBRiiwa.urdf --- .../grl/flatbuffer/ParseflatbuffertoCSV.hpp | 11 +- .../grl/flatbuffer/Robone_KukaLBRiiwa.urdf | 426 ++++++++++++++++++ include/grl/flatbuffer/kukaiiwaURDF.h | 18 +- test/KukaPoseEstimationTest.cpp | 2 +- test/readFlatbufferTest.cpp | 17 +- 5 files changed, 460 insertions(+), 14 deletions(-) create mode 100644 include/grl/flatbuffer/Robone_KukaLBRiiwa.urdf diff --git a/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp b/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp index a73b901..f98e540 100644 --- a/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp +++ b/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp @@ -1025,13 +1025,12 @@ namespace grl { return std::move(EEpose); } - mc_rbdyn_urdf::URDFParserResult getURDFModel(){ - using namespace Eigen; - using namespace sva; - using namespace rbd; - + mc_rbdyn_urdf::URDFParserResult getURDFModel(std::string filename = "/Robone_KukaLBRiiwa.urdf"){ namespace cst = boost::math::constants; - auto strRobot = mc_rbdyn_urdf::rbdyn_from_urdf(XYZSarmUrdf); + std::string urdfmodel = readFile(filename); + + auto strRobot = mc_rbdyn_urdf::rbdyn_from_urdf(urdfmodel); + // auto strRobot = mc_rbdyn_urdf::rbdyn_from_urdf(XYZSarmUrdf); return std::move(strRobot); } diff --git a/include/grl/flatbuffer/Robone_KukaLBRiiwa.urdf b/include/grl/flatbuffer/Robone_KukaLBRiiwa.urdf new file mode 100644 index 0000000..e87e42a --- /dev/null +++ b/include/grl/flatbuffer/Robone_KukaLBRiiwa.urdf @@ -0,0 +1,426 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + /iiwa + + + + + Gazebo/Grey + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Orange + 0.2 + 0.2 + + + + Gazebo/Grey + 0.2 + 0.2 + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + + /iiwa + transmission_interface/SimpleTransmission + + PositionJointInterface + + + PositionJointInterface + 1 + + + \ No newline at end of file diff --git a/include/grl/flatbuffer/kukaiiwaURDF.h b/include/grl/flatbuffer/kukaiiwaURDF.h index e8d7553..6f23b6f 100644 --- a/include/grl/flatbuffer/kukaiiwaURDF.h +++ b/include/grl/flatbuffer/kukaiiwaURDF.h @@ -23,10 +23,13 @@ #include #include +#include #include "RBDyn/FK.h" - +// R"( represents a raw string literal in which the escape characters (like \n \t or \" ) of C++ are not processed. +// A raw string literal starts with R"( and ends in )" +// See https://solarianprogrammer.com/2011/10/16/cpp-11-raw-strings-literals-tutorial/ std::string XYZSarmUrdf( R"( @@ -457,6 +460,19 @@ R"( )"); +std::string readFile(const std::string &fileName) +{ + std::ifstream ifs(fileName.c_str(), std::ios::in | std::ios::binary | std::ios::ate); + + std::ifstream::pos_type fileSize = ifs.tellg(); + ifs.seekg(0, ios::beg); + + std::vector bytes(fileSize); + ifs.read(&bytes[0], fileSize); + + return std::string(&bytes[0], fileSize); +} + namespace mc_rbdyn_urdf { bool operator==( const Geometry::Mesh& m1, const Geometry::Mesh& m2) diff --git a/test/KukaPoseEstimationTest.cpp b/test/KukaPoseEstimationTest.cpp index d02191a..5086c48 100644 --- a/test/KukaPoseEstimationTest.cpp +++ b/test/KukaPoseEstimationTest.cpp @@ -59,7 +59,7 @@ const double RadtoDegree = 180/3.14159265359; const double MeterToMM = 1000; -// "/home/chunting/src/V-REP_PRO_EDU_V3_4_0_Linux/"; +// "/home/cjiao1/src/V-REP_PRO_EDU_V3_4_0_Linux/"; std::string currentPath = boost::filesystem::current_path().string(); std::string foldtimestamp = current_date_and_time_string(); diff --git a/test/readFlatbufferTest.cpp b/test/readFlatbufferTest.cpp index 8894eca..09d08d2 100644 --- a/test/readFlatbufferTest.cpp +++ b/test/readFlatbufferTest.cpp @@ -23,6 +23,8 @@ #include #include +#include + /// split -C 200m --numeric-suffixes 2018_02_28_16_39_13_FusionTrack.json 2018_02_28_16_39_13_FusionTrack /// The command to get the json file from flatbuffer binary file, these two files should be located in the same folder. @@ -32,15 +34,18 @@ int main(int argc, char* argv[]) { std::string kukaTimeStamp("2018_03_26_19_06_21_Kukaiiwa.iiwa"); std::string FTTimeStamp("2018_03_26_19_06_21_FusionTrack.flik"); + std::string URDFModrl("Robone_KukaLBRiiwa.urdf"); /// Define the csv file names if(argc == 2){ - kukaTimeStamp = std::string(argv[1]); - FTTimeStamp = std::string(argv[2]); - + kukaTimeStamp = std::string(argv[1]); + FTTimeStamp = std::string(argv[2]); } - std::string currentPath = boost::filesystem::current_path().string()+"/"; //"/home/cjiao1/src/V-REP_PRO_EDU_V3_4_0_Linux/"; + + std::string currentPath = boost::filesystem::current_path().string()+"/"; std::string kukaBinaryfile = currentPath + kukaTimeStamp; + std::cout << kukaBinaryfile << std::endl; std::string fusiontrackBinaryfile = currentPath + FTTimeStamp; + std::string urdfFile = currentPath + URDFModrl; std::string foldtimestamp = current_date_and_time_string(); // Write the generated files into a new fold boost::filesystem::path dir{currentPath+foldtimestamp}; boost::filesystem::create_directory(dir); @@ -156,8 +161,8 @@ int main(int argc, char* argv[]) std::copy(std::begin(grl::C_Pos_Joint_Labels), std::end(grl::C_Pos_Joint_Labels),std::begin(kuka_labels)+grl::Time_Labels.size()); grl::writeMatrixToCSV(C_Joint_CSV, kuka_labels, timeEventM_Kuka, commandedJointPosition); - - auto strRobot = grl::getURDFModel(); + + auto strRobot = grl::getURDFModel(urdfFile); rbd::MultiBody mb = strRobot.mb; rbd::MultiBodyConfig mbc(mb); rbd::MultiBodyGraph mbg(strRobot.mbg); From 7010e39b5d828b99877473d20d15b80cde2d229e Mon Sep 17 00:00:00 2001 From: Chunting Date: Thu, 5 Apr 2018 17:12:50 -0400 Subject: [PATCH 076/102] Removed two test files that won't be used any more. --- test/KinematicsCalibration_test.cpp | 167 ---------------------------- test/KukaPoseEstimationTest.cpp | 139 ----------------------- 2 files changed, 306 deletions(-) delete mode 100644 test/KinematicsCalibration_test.cpp delete mode 100644 test/KukaPoseEstimationTest.cpp diff --git a/test/KinematicsCalibration_test.cpp b/test/KinematicsCalibration_test.cpp deleted file mode 100644 index 729995a..0000000 --- a/test/KinematicsCalibration_test.cpp +++ /dev/null @@ -1,167 +0,0 @@ -#define BOOST_TEST_DYN_LINK -#define BOOST_TEST_MODULE KinematicsCalibration_test -#include -#include - -#include -#include -#include -#include -#include - -#include -#include - -// SpaceVecAlg -// https://github.com/jrl-umi3218/SpaceVecAlg -#include - - -// RBDyn -// https://github.com/jrl-umi3218/RBDyn -#include -#include -#include -#include -#include -#include - -// mc_rbdyn_urdf -// https://github.com/jrl-umi3218/mc_rbdyn_urdf -#include - -//// local includes -// #include "grl/vrep/KukaLBRiiwaVrepPlugin.hpp" - -#include "grl/flatbuffer/FlatbufferToEigen.hpp" -#include "grl/vrep/SpaceVecAlg.hpp" - - -#include "kukaiiwaURDF.h" - -const double degreesToRadians = M_PI/180.0; -const double radiansTodegrees = 180/M_PI; - -BOOST_AUTO_TEST_SUITE(KinematicsCalibration_test) - - -namespace cst = boost::math::constants; -auto strRobot = mc_rbdyn_urdf::rbdyn_from_urdf(XYZSarmUrdf); -rbd::MultiBody mb = strRobot.mb; -rbd::MultiBodyConfig mbc(mb); -rbd::MultiBodyGraph mbg(strRobot.mbg); -std::size_t nrJoints = mb.nrJoints(); -std::size_t nrBodies = strRobot.mb.nrBodies(); - - -BOOST_AUTO_TEST_CASE(ForwardKinematics) -{ - Eigen::MatrixXd testJointAngles(5,7); // Degree - testJointAngles << -0.06, -0.03, 0.01, -0.06, -0.04, -0.02, 0.1, - -0.06, 90.09, 0.01, -0.07, -0.04, -0.01, 0.1, - -0.06, -0.06, 90.06, -0.06, -0.04, -0.02, 0.1, - 90.03, -0.06, 0.06, -0.06, -0.04, -0.02, 0.1, - 0.05, -0.06, 0.06, -91.87, 0, 45.48, 0.1; - - testJointAngles = degreesToRadians*testJointAngles; - - Eigen::MatrixXd cartesianPositions(5,6); // Degree and mm - cartesianPositions << 0.02, 0, 1306, 0, 0.01, 0, - 946, -1.04, 357.97, 154.15, 89.84, 154.22, - -1, 0.5, 1306, 90.06, 0.04, -0.06, - 0, -0.5, 1306, 90.14, -0.02, 0.06, - 484.81, 0.85, 674.72, 179.97, 42.7, 179.91; - - std::size_t testSize = testJointAngles.rows(); - std::size_t jointNum = testJointAngles.cols(); - std::size_t poseDim = cartesianPositions.cols(); - std::vector jointNames; - std::vector EEpose; - - for(int testIdx=0; testIdx 0) { - mbc.q[jointIndex][0] = oneStateJointPosition[jointIdx]; - jointIdx++; - } - std::cout << "Rotation: " << std::endl << transfroms[jointIndex].rotation() << std::endl; - std::cout << "Translation: " << std::endl << transfroms[jointIndex].translation() << std::endl; - } - rbd::forwardKinematics(mb, mbc); - // Pose of the end effector relative to robot base frame. - // body_size-1: end-effector with marker - // body_size-2: Flange, here this should be used. - EEpose.push_back(mbc.bodyPosW[nrBodies-2]); - - - for(int i=0; i(0,0) = 1000*pluckerpose.block<5,3>(0,0); - pluckerpose.block<5,3>(0,3) = radiansTodegrees*pluckerpose.block<5,3>(0,3); - std::cout << pluckerpose << std::endl; - std::cout << "---------------------------------" << std::endl; - - -///////////////////////////////////////////////////////////////////////////////////////// -cartesianPositions.block<5,3>(0,3) = degreesToRadians*cartesianPositions.block<5,3>(0,3); -std::vector PTransform; -for(int testIdx=0; testIdx. - -// includes -// std -#include - -// boost -#define BOOST_TEST_MODULE KukaPoseEstimationTest // master test suite name -#include -#include - - -#include -#include -#include -#include -#include -#include "camodocal/EigenUtils.h" - -#include "grl/flatbuffer/ParseflatbuffertoCSV.hpp" -#include "grl/time.hpp" - -// SpaceVecAlg -// https://github.com/jrl-umi3218/SpaceVecAlg -#include - -// RBDyn -// https://github.com/jrl-umi3218/RBDyn -#include -#include -#include -#include -#include -#include - -// mc_rbdyn_urdf -// https://github.com/jrl-umi3218/mc_rbdyn_urdf -#include -#include "kukaiiwaURDF.h" - -#include - -// const double PI = 3.14159265359; -const double RadtoDegree = 180/3.14159265359; -const double MeterToMM = 1000; - -// "/home/cjiao1/src/V-REP_PRO_EDU_V3_4_0_Linux/"; -std::string currentPath = boost::filesystem::current_path().string(); - -std::string foldtimestamp = current_date_and_time_string(); - -std::string kukaBinaryfile = currentPath + "2018_02_26_14_23_14_Kukaiiwa.iiwa"; -std::string KUKA_TimeEvent_CSVfilename = currentPath + foldtimestamp + "/KUKA_TimeEvent.csv"; -std::string KUKA_Joint_CSVfilename = currentPath + foldtimestamp + "/KUKA_Joint.csv"; -std::string KUKA_Pose_CSVfilename = currentPath + foldtimestamp + "/KUKA_Pose.csv"; - - - - -int main(int argc, char* argv[]) -{ - - // namespace cst = boost::math::constants; - // auto strRobot = mc_rbdyn_urdf::rbdyn_from_urdf(XYZSarmUrdf); - // rbd::MultiBody mb = strRobot.mb; - // rbd::MultiBodyConfig mbc(mb); - // rbd::MultiBodyGraph mbg(strRobot.mbg); - // std::size_t nrJoints = mbg.nrJoints(); - // std::vector jointNames; - // std::cout<<"Joint Size: "<< nrJoints << std::endl; - // fbs_tk::Root kukaStatesP = fbs_tk::open_root(kukaBinaryfile); - - // grl::MatrixXd timeEventM_Kuka = grl::getTimeStamp(kukaStatesP,grl::kuka_tag()); - // grl::regularizeTimeEvent(timeEventM_Kuka); - - // Eigen::MatrixXd jointAngles = grl::getAllJointAngles(kukaStatesP); - // std::vector Kuka_Joint_Labels = grl::getLabels(grl::LabelsType::Joint); - // jointAngles = RadtoDegree*jointAngles; - // if(jointAngles.rows() == timeEventM_Kuka.rows()){ - // grl::writeMatrixToCSV(KUKA_Joint_CSVfilename, Kuka_Joint_Labels, timeEventM_Kuka, jointAngles); - // } - - - // std::size_t row_size = jointAngles.rows(); - // std::size_t body_size = mbc.bodyPosW.size(); - // /// The translation and Euler angles in world coordinate. - // Eigen::MatrixXd poseEE(row_size,6); - - // for(int rowIdx=0; rowIdx 0) { - // mbc.q[jointIndex][0] = oneStateJointPosition[jointIdx]; - // jointIdx++; - // } - // } - // rbd::forwardKinematics(mb, mbc); - // sva::PTransformd pos = mbc.bodyPosW[body_size-1]; - // Eigen::Matrix3d E ; // rotation - - // Eigen::Vector3d r ; // translation - // E = pos.rotation(); - // r = MeterToMM*pos.translation(); - // Eigen::Vector3d eulerAngleEigen = RadtoDegree*E.eulerAngles(0,1,2); - // Eigen::Quaterniond q(E); - // Eigen::RowVectorXd pose(6); - // pose << r.transpose(), eulerAngleEigen.transpose(); - // poseEE.row(rowIdx) = pose; - // // std::cout << "-----------------------------------" << std::endl; - // // std::cout << "Rotation:\n " << E << std::endl - // // << "Translation:\n" << r < Date: Thu, 5 Apr 2018 17:20:57 -0400 Subject: [PATCH 077/102] In order to visualize the urdf model in vrep, change the joint and link names of urdf model to make it consistency with vrep model. --- .../grl/flatbuffer/Robone_KukaLBRiiwa.urdf | 110 +++++++++--------- 1 file changed, 55 insertions(+), 55 deletions(-) diff --git a/include/grl/flatbuffer/Robone_KukaLBRiiwa.urdf b/include/grl/flatbuffer/Robone_KukaLBRiiwa.urdf index e87e42a..233d06e 100644 --- a/include/grl/flatbuffer/Robone_KukaLBRiiwa.urdf +++ b/include/grl/flatbuffer/Robone_KukaLBRiiwa.urdf @@ -26,12 +26,12 @@ - + - + - + @@ -59,16 +59,16 @@ - - - + + + - + @@ -90,16 +90,16 @@ - - - + + + - + @@ -121,16 +121,16 @@ - - - + + + - + @@ -152,16 +152,16 @@ - - - + + + - + @@ -183,16 +183,16 @@ - - - + + + - + @@ -214,16 +214,16 @@ - - - + + + - + @@ -245,16 +245,16 @@ - - - + + + - + @@ -275,21 +275,21 @@ - - - + + + - + - - - - + + + + - + @@ -299,49 +299,49 @@ - + Gazebo/Grey 0.2 0.2 - + Gazebo/Orange 0.2 0.2 - + Gazebo/Orange 0.2 0.2 - + Gazebo/Orange 0.2 0.2 - + Gazebo/Orange 0.2 0.2 - + Gazebo/Orange 0.2 0.2 - + Gazebo/Orange 0.2 0.2 - + Gazebo/Grey 0.2 0.2 @@ -349,7 +349,7 @@ /iiwa transmission_interface/SimpleTransmission - + PositionJointInterface @@ -360,7 +360,7 @@ /iiwa transmission_interface/SimpleTransmission - + PositionJointInterface @@ -371,7 +371,7 @@ /iiwa transmission_interface/SimpleTransmission - + PositionJointInterface @@ -382,7 +382,7 @@ /iiwa transmission_interface/SimpleTransmission - + PositionJointInterface @@ -393,7 +393,7 @@ /iiwa transmission_interface/SimpleTransmission - + PositionJointInterface @@ -404,7 +404,7 @@ /iiwa transmission_interface/SimpleTransmission - + PositionJointInterface @@ -415,7 +415,7 @@ /iiwa transmission_interface/SimpleTransmission - + PositionJointInterface From ad7678731c7aa2735a18007347cbbd76d4459f3b Mon Sep 17 00:00:00 2001 From: Chunting Date: Thu, 5 Apr 2018 17:25:33 -0400 Subject: [PATCH 078/102] Remove some meaningless comments. --- .../camodocal/HandEyeCalibration.cpp | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/src/v_repExtHandEyeCalibration/camodocal/HandEyeCalibration.cpp b/src/v_repExtHandEyeCalibration/camodocal/HandEyeCalibration.cpp index fed3167..a1fc5ce 100644 --- a/src/v_repExtHandEyeCalibration/camodocal/HandEyeCalibration.cpp +++ b/src/v_repExtHandEyeCalibration/camodocal/HandEyeCalibration.cpp @@ -130,9 +130,6 @@ HandEyeCalibration::estimateHandEyeScrew(const std::vector(i * 6, 0) = AxisAngleToSTransposeBlockOfT(rvec1,tvec1,rvec2,tvec2); } - //std::cout<< "Test Matrix: " << T << std::endl; - auto dq = estimateHandEyeScrewInitial(T,planarMotion); @@ -267,7 +262,7 @@ HandEyeCalibration::estimateHandEyeScrewInitial(Eigen::MatrixXd& T, ss << v2 << std::endl; ss << "Not handled yet. Your rotations and translations are probably either not aligned or not passed in properly." << std::endl; - // BOOST_THROW_EXCEPTION(std::runtime_error(ss.str())); + BOOST_THROW_EXCEPTION(std::runtime_error(ss.str())); } } From d8f7c299238defd50cef92f64d6bd29a51fd169c Mon Sep 17 00:00:00 2001 From: Chunting Date: Thu, 5 Apr 2018 17:53:08 -0400 Subject: [PATCH 079/102] Removed some dead code --- .../grl/flatbuffer/ParseflatbuffertoCSV.hpp | 287 +----------------- test/CMakeLists.txt | 11 - test/readFlatbufferTest.cpp | 1 - 3 files changed, 15 insertions(+), 284 deletions(-) diff --git a/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp b/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp index f98e540..bba2a37 100644 --- a/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp +++ b/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp @@ -154,79 +154,6 @@ namespace grl { return states->size(); } - - /// Return the original timeEvent of fusiontracker - /// @param TlogKUKAiiwaFusionTrackP pointer of the root object for fusiontracker. - /// @param fusiontracker_tag tag dispatch - grl::MatrixXd getTimeStamp(const fbs_tk::Root &logKUKAiiwaFusionTrackP, fusiontracker_tag, int markerID = 0){ - - auto states = logKUKAiiwaFusionTrackP->states(); - std::size_t state_size = states->size(); - assert(state_size); - int row = 0; - grl::MatrixXd timeEventM(state_size,col_timeEvent); - - for(int i = 0; iGet(i); - auto FT_Message = kukaiiwaFusionTrackMessage->deviceState_as_FusionTrackMessage(); - auto FT_Frame = FT_Message->frame(); - auto counter = FT_Frame->counter(); - auto Makers = FT_Frame->markers(); - if(Makers!=nullptr) { - auto makerSize = Makers->size(); - for(int markerIndex=0; markerIndexGet(markerIndex); - auto marker_ID = marker->geometryID(); - if(marker_ID == markerID){ - auto timeEvent = kukaiiwaFusionTrackMessage->timeEvent(); - timeEventM(row,TimeType::local_receive_time) = timeEvent->local_receive_time(); - timeEventM(row,TimeType::local_request_time) = timeEvent->local_request_time(); - timeEventM(row,TimeType::device_time) = timeEvent->device_time(); - timeEventM(row,TimeType::time_Y) = timeEventM(row,TimeType::device_time) - timeEventM(row,timeBaseline_index); - timeEventM(row,TimeType::counterIdx) = counter; - row++; - break; - } - - } - } - } - if(row < state_size) { - timeEventM.conservativeResize(row, Eigen::NoChange_t{}); - } - grl::VectorXd timeCol = timeEventM.col(timeBaseline_index); - assert(checkmonotonic(timeCol)); - return timeEventM; - } - /// Return the original timeEvent of Kuka - /// @param kukaStatesP pointer of the root object for kuka. - /// @param kuka_tag tag dispatch - /// @return timeEventM timeEvent matrix which should have four columns - /// local_receive_time_offset = local_receive_time - initial_local_receive_time - /// local_request_time_offset = local_request_time - initial_local_request_time - /// device_time_offset = device_time - initial_device_time - /// time_Y = device_time_offset - local_receive_time_offset - grl::MatrixXd getTimeStamp(const fbs_tk::Root &kukaStatesP, kuka_tag){ - auto states = kukaStatesP->states(); - std::size_t state_size = states->size(); - assert(state_size); - grl::MatrixXd timeEventM(state_size,col_timeEvent); - - for(int i = 0; iGet(i); - auto FRIMessage = KUKAiiwaState->FRIMessage(); - auto identifier = FRIMessage->messageIdentifier(); - auto timeEvent = FRIMessage->timeStamp(); - timeEventM(i,TimeType::local_receive_time) = timeEvent->local_receive_time(); - timeEventM(i,TimeType::local_request_time) = timeEvent->local_request_time(); - timeEventM(i,TimeType::device_time) = timeEvent->device_time(); - timeEventM(i,TimeType::time_Y) = timeEventM(i,TimeType::device_time) - timeEventM(i,timeBaseline_index); - timeEventM(i,TimeType::counterIdx) = identifier; - } - grl::VectorXd timeCol = timeEventM.col(timeBaseline_index); - assert(checkmonotonic(timeCol)); - return timeEventM; - } /// Process the time data to get the time offset based on the starting time point. /// See https://github.com/facontidavide/PlotJuggler/issues/68 /// @param timeEventM timeEvent matrix which should have four columns @@ -435,8 +362,8 @@ namespace grl { } - /// With the same method to process the time data from both Kuka and fusiontracker, combine it and write to a csv file. - /// @param FTKUKA_TimeEvent_CSVfilename, the csv file name + /// Write FRI message into a csv file, all the data are read from flatbuffer binary file. + /// @param KUKA_FRI_CSVfilename, the csv file name void writeFRIMessageToCSV(std::string& KUKA_FRI_CSVfilename, grl::MatrixXd& timeEventM_Kuka, Eigen::VectorXd& sequenceCounterVec, @@ -584,44 +511,7 @@ namespace grl { std::cout << "kuka_time_size: " << kuka_time_size << " kuka_index: " << kuka_index << std::endl; fs.close(); } - /// Get the joint angles of a specific joint (joint_index) - /// @param kukaStatesP pointer of the root object for kuka. - /// @param joint_index, return the joint angles of the indicated joint. - /// @return jointPosition, Eigen vector which contains joint position. - Eigen::VectorXf getJointAnglesFromKUKA(const fbs_tk::Root &kukaStatesP, int joint_index){ - - auto states = kukaStatesP->states(); - std::size_t state_size = states->size(); - Eigen::VectorXf jointPosition(state_size); - for(int i = 0; iGet(i); - auto FRIMessage = KUKAiiwaState->FRIMessage(); - auto joints_Position = FRIMessage->measuredJointPosition(); // flatbuffers::Vector * - jointPosition(i) = joints_Position->Get(joint_index); - } - return jointPosition; - } - /// Get the all the joint angles. - /// @param kukaStatesP pointer of the root object for kuka. - /// @return allJointPosition, Eigen matrix which contains the positions of all the joints. - Eigen::MatrixXd getAllJointAngles(const fbs_tk::Root &kukaStatesP){ - - auto states = kukaStatesP->states(); - std::size_t state_size = states->size(); - Eigen::MatrixXd allJointPosition(state_size, 7); - for(int i = 0; iGet(i); - auto FRIMessage = KUKAiiwaState->FRIMessage(); - auto joints_Position = FRIMessage->measuredJointPosition(); // flatbuffers::Vector * - Eigen::VectorXd oneStateJointPosition(7); - for(int joint_index=0; joint_index<7; joint_index++){ - oneStateJointPosition(joint_index) = joints_Position->Get(joint_index); - } - allJointPosition.row(i) = oneStateJointPosition.transpose(); - } - return allJointPosition; - } /// Write the data to CSV file /// @param CSV_FileName, the file name. /// @param labels, the labels for the data to write, which should be the first row of the csv file @@ -662,8 +552,9 @@ namespace grl { std::cout<< CSV_FileName << ": Rows " << time_rows_size << " Cols: " << cols_size << std::endl; } /// Process the timeEvent to get more information, and write the result into a csv file directly. - /// @param timeEventM - void writeTimeEventToCSV( std::string & CSV_FileName, grl::MatrixXd& timeEventM, int rowIdx) { + /// @param timeEventM, time event matrix + // @param rowIdx, from which row of the matrix to write. + void writeTimeEventToCSV( std::string & CSV_FileName, grl::MatrixXd& timeEventM, int rowIdx=0) { std::size_t time_size = timeEventM.rows(); grl::VectorXd local_request_timeV = timeEventM.col(grl::TimeType::local_request_time); grl::VectorXd local_receive_timeV = timeEventM.col(grl::TimeType::local_receive_time); @@ -706,8 +597,14 @@ namespace grl { } - /// With the same method to process the time data from both Kuka and fusiontracker, combine it and write to a csv file. + /// Write the tracker and kuka messages inito one single csv file. /// @param FTKUKA_TimeEvent_CSVfilename, the csv file name + /// @param timeEventM_FT the tracker time event matrix + /// @param timeEventM_Kuka the kuka time event matrix + /// @param jointAngles, joint angles matrix, collected from real robot + /// @param markerPose, marker pose from tracker + /// @param FT_index, kuka_index, since we skip the first rows to make two devices time consistency, here these two indexs are to indicate from where to read or write data. + void writeFTKUKAMessageToCSV(std::string& FTKUKA_TimeEvent_CSVfilename, grl::MatrixXd& timeEventM_FT, grl::MatrixXd& timeEventM_Kuka, @@ -822,159 +719,14 @@ namespace grl { std::cout << "FT_index: " << FT_index << " kuka_index: " << kuka_index << " Sum: " << FT_index+kuka_index << std::endl; fs.close(); } - /// With the same method to process the time data from both Kuka and fusiontracker, combine it and write to a csv file. - /// @param FTKUKA_TimeEvent_CSVfilename, the csv file name - void writeFTKUKAMessageToCSV(std::string& FTKUKA_TimeEvent_CSVfilename, - const fbs_tk::Root &logKUKAiiwaFusionTrackP, - const fbs_tk::Root &kukaStatesP, - uint32_t &markerID) { - - grl::MatrixXd timeEventM_FT = grl::getTimeStamp(logKUKAiiwaFusionTrackP, grl::fusiontracker_tag(), markerID); - grl::MatrixXd timeEventM_Kuka = grl::getTimeStamp(kukaStatesP, grl::kuka_tag()); - - std::size_t FT_size = grl::getStateSize(logKUKAiiwaFusionTrackP); - std::size_t kuka_size = grl::getStateSize(kukaStatesP); - - std::cout<< "------Kuka_size: "<< kuka_size << std::endl; - std::cout<< "------FT_size: "<< FT_size << std::endl; - - std::size_t FT_time_size = timeEventM_FT.rows(); - std::size_t kuka_time_size = timeEventM_Kuka.rows(); - grl::MatrixXd timeEventM = grl::MatrixXd::Zero(FT_size, timeEventM_FT.cols()); - Eigen::MatrixXd markerPose(FT_size, grl::col_Pose); - // Get the pose of the marker 22. - // Since the bad data is skipped, the timeEventM_FT also needs to be filled out with the orinial time stamp. - - int validsize_22 = grl::getMarkerPose(logKUKAiiwaFusionTrackP, markerID, timeEventM, markerPose); - assert(timeEventM.rows() == timeEventM_FT.rows()); - ////////////////////////////////////////////////////////////////////////////////// - - - - int kuka_index = 0; - int FT_index = 0; - - // Skip the very beginning data,since the tracker starts to work once the scene is loaded in vrep. - // But kuka starts to work only after clicking on the start button. - // To combine the time from two devices, they should have the same starting time point. - - while(kuka_index timeEventM_FT(FT_index,timeBaseline_index ) && kuka_index == 0){ - FT_index++; - } - - auto initial_local_time = std::min(timeEventM_FT(FT_index,timeBaseline_index ), timeEventM_Kuka(kuka_index,timeBaseline_index )); - auto initial_device_time_kuka = timeEventM_Kuka(kuka_index,device_time); - auto initial_device_time_FT = timeEventM_FT(FT_index,device_time); - grl::VectorXd FT_local_request_time = timeEventM_FT.col(local_request_time) - initial_local_time * grl::VectorXd::Ones(FT_time_size); - grl::VectorXd FT_local_receive_time = timeEventM_FT.col(local_receive_time) - initial_local_time * grl::VectorXd::Ones(FT_time_size); - grl::VectorXd FT_device_time = timeEventM_FT.col(device_time) - initial_device_time_FT * grl::VectorXd::Ones(FT_time_size); - - grl::VectorXd kuka_local_request_time = timeEventM_Kuka.col(local_request_time) - initial_local_time * grl::VectorXd::Ones(kuka_time_size); - grl::VectorXd kuka_local_receive_time = timeEventM_Kuka.col(local_receive_time) - initial_local_time * grl::VectorXd::Ones(kuka_time_size); - grl::VectorXd kuka_device_time = timeEventM_Kuka.col(device_time) - initial_device_time_kuka * grl::VectorXd::Ones(kuka_time_size); - - Eigen::MatrixXd jointAngles = grl::getAllJointAngles(kukaStatesP); - - - int64_t kuka_diff = kuka_device_time(kuka_index) - kuka_local_request_time(kuka_index); - int64_t FT_diff = FT_device_time(FT_index) - FT_local_request_time(FT_index); - - grl::VectorXd Y_kuka = kuka_device_time - kuka_local_request_time - grl::VectorXd::Ones(kuka_time_size)*kuka_diff; - grl::VectorXd Y_FT = FT_device_time - FT_local_request_time - grl::VectorXd::Ones(FT_time_size)*FT_diff; - - - std::ofstream fs; - // create a name for the file output - fs.open( FTKUKA_TimeEvent_CSVfilename, std::ofstream::out | std::ofstream::app); - // write the file headers - fs << "local_receive_time_offset_X," - << "FT_local_request_time," - << "KUKA_local_request_time," - << "FT_device_time_offset," - << "device_time_offset_kuka," - << "Y_FT," - << "Y_kuka," - << "FT_X," - << "FT_Y," - << "FT_Z," - << "FT_A," - << "FT_B," - << "FT_C," - << "K_Joint1," - << "K_Joint2," - << "K_Joint3," - << "K_Joint4," - << "K_Joint5," - << "K_Joint6," - << "K_Joint7" - << std::endl; - std::cout << "Start to write ... "<< std::endl <<"FT_index: " << FT_index << " kuka_index: " << kuka_index << std::endl; - - while ( kuka_index < kuka_time_size && FT_index < FT_time_size ) - { - // If the row value is the kuka time, then the FT cells should be left blank. - if ( kuka_local_receive_time(kuka_index) < FT_local_receive_time(FT_index) ){ - // write the data to the output file - auto jointrow = jointAngles.row(kuka_index); - fs << kuka_local_request_time(kuka_index) <<"," - << kuka_local_receive_time(kuka_index) << "," - << "," - << kuka_device_time(kuka_index) <<"," - << "," - << Y_kuka(kuka_index) << "," - << ",,,,,," - << jointrow[0] << "," - << jointrow[1] << "," - << jointrow[2] << "," - << jointrow[3] << "," - << jointrow[4] << "," - << jointrow[5] << "," - << jointrow[6] - << std::endl; - kuka_index++; - } else if( kuka_local_request_time(kuka_index) > FT_local_request_time(FT_index)) { - auto matrixrow = markerPose.row(FT_index); - // If the row value is the FT time, then the kuka cells should be left blank. - fs << FT_local_request_time(FT_index) <<"," - << FT_local_receive_time(FT_index) << "," - << FT_device_time(FT_index) << "," - << "," - << Y_FT(FT_index) << "," - << "," - << matrixrow[0] << "," - << matrixrow[1] << "," - << matrixrow[2] << "," - << matrixrow[3] << "," - << matrixrow[4] << "," - << matrixrow[5] << std::endl; - FT_index++; - } else { - // In case the time is extactly equivent with each other. - fs << kuka_local_request_time(FT_index) <<"," - << kuka_local_receive_time(FT_index) << "," - << FT_device_time(FT_index) << "," - << kuka_device_time(kuka_index) <<"," - << Y_FT(FT_index) << "," - << Y_kuka(kuka_index) - << std::endl; - FT_index++; - kuka_index++; - } - } - std::cout << "FT_index: " << FT_index << " kuka_index: " << kuka_index << " Sum: " << FT_index+kuka_index << std::endl; - fs.close(); - } - - + /// Based on the RBDy and URDF model, get the cartesian pose of the end effector. /// BE CAREFUL THAT THE URDF MODEL HAS BEEN MODIFIED, THE MARKER LINK HAS BEEN ADDED. /// /// This is gotten from VREP, the oritation of the marker dummy is identical with the flange ('Fiducial#22' and 'RobotFlangeTip'). /// SEE kukaiiwaURDF.h /// @param jointAngles, the joint angles matrix + /// @param markerPose, if true put the pose of endeffector into the marker space, otherwise in robot frame. /// @return poseEE, contain the translation and the Euler angle. std::vector getPoseEE(Eigen::MatrixXd& jointAngles, bool markerPose = false){ using namespace Eigen; @@ -989,7 +741,6 @@ namespace grl { std::size_t nrJoints = mb.nrJoints(); std::size_t nrBodies = strRobot.mb.nrBodies(); std::vector jointNames; - std::cout<<"Joint Size: "<< nrJoints << std::endl; std::size_t row_size = jointAngles.rows(); std::size_t body_size = mbc.bodyPosW.size(); @@ -1003,7 +754,6 @@ namespace grl { for(int jointIndex = 0; jointIndex< nrJoints; jointIndex++) { const auto & joint = strRobot.mb.joint(jointIndex); jointNames.push_back(joint.name()); - // std::cout << "Joint Name: " << joint.name() << " Size: " << mbc.q[jointIndex].size() << std::endl; if (mbc.q[jointIndex].size() > 0) { mbc.q[jointIndex][0] = oneStateJointPosition[jointIdx]; jointIdx++; @@ -1017,20 +767,13 @@ namespace grl { EEpose.push_back(mbc.bodyPosW[body_size-2]); } } - // std::cout<< "---------------------" << std::endl; - // for(int i=0; i Date: Thu, 5 Apr 2018 18:24:45 -0400 Subject: [PATCH 080/102] Remove some dead code --- .../grl/sensor/FusionTrackToFlatbuffer.hpp | 23 ------------------- 1 file changed, 23 deletions(-) diff --git a/include/grl/sensor/FusionTrackToFlatbuffer.hpp b/include/grl/sensor/FusionTrackToFlatbuffer.hpp index a24a9f2..77ae721 100644 --- a/include/grl/sensor/FusionTrackToFlatbuffer.hpp +++ b/include/grl/sensor/FusionTrackToFlatbuffer.hpp @@ -54,29 +54,6 @@ grl::flatbuffer::Vector3d toFlatBuffer(const ::ftk3DPoint &pt) return grl::flatbuffer::Vector3d(pt.x, pt.y, pt.z); } -// grl::flatbuffer::Vector3d toFlatBuffer(const Eigen::Vector3d &pt) -// { -// return grl::flatbuffer::Vector3d(pt.x(), pt.y(), pt.z()); -// } - -// grl::flatbuffer::Quaternion toFlatBuffer(Eigen::Quaterniond q) -// { -// return grl::flatbuffer::Quaternion(q.x(), q.y(), q.z(), q.w()); -// } - -// grl::flatbuffer::Pose toFlatBuffer(Eigen::Affine3d tf) -// { -// Eigen::Vector3d pos = tf.translation(); -// Eigen::Quaterniond eigenQuat(tf.rotation()); -// return grl::flatbuffer::Pose(toFlatBuffer(pos), toFlatBuffer(eigenQuat)); -// } - -// grl::flatbuffer::Pose toFlatBuffer(Eigen::Affine3f tf) -// { -// return toFlatBuffer(tf.cast()); -// } - - /// the second parameter is merely a tag to uniquely identify this function so it compiles /// its value is not utilized or modified. grl::flatbuffer::ftkQueryStatus toFlatBuffer(const ::ftkQueryStatus queryStatus) { From 0b3be3f542bbbf6164b4a3ed90caf2d059a62b0d Mon Sep 17 00:00:00 2001 From: Chunting Date: Fri, 6 Apr 2018 18:04:01 -0400 Subject: [PATCH 081/102] Updates urdf file path --- include/grl/flatbuffer/ParseflatbuffertoCSV.hpp | 5 ++++- include/grl/vrep/InverseKinematicsVrepPlugin.hpp | 7 ++++--- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp b/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp index bba2a37..4859035 100644 --- a/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp +++ b/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp @@ -770,7 +770,10 @@ namespace grl { return std::move(EEpose); } - mc_rbdyn_urdf::URDFParserResult getURDFModel(std::string filename = "/Robone_KukaLBRiiwa.urdf"){ + mc_rbdyn_urdf::URDFParserResult getURDFModel(std::string filename = "~/src/robonetracker/modules/grl/include/grl/flatbuffer/Robone_KukaLBRiiwa.urdf"){ + if(!boost::filesystem::exists(filename)){ + std::cerr << filename << " doesn't exist..." << std::endl; + } namespace cst = boost::math::constants; std::string urdfmodel = readFile(filename); auto strRobot = mc_rbdyn_urdf::rbdyn_from_urdf(urdfmodel); diff --git a/include/grl/vrep/InverseKinematicsVrepPlugin.hpp b/include/grl/vrep/InverseKinematicsVrepPlugin.hpp index c0b5701..8d17700 100644 --- a/include/grl/vrep/InverseKinematicsVrepPlugin.hpp +++ b/include/grl/vrep/InverseKinematicsVrepPlugin.hpp @@ -609,16 +609,17 @@ void getPoseFromCSV(std::string filename, int time_index){ const std::size_t simulatedRobotIndex = 0; auto& simArmMultiBody = rbd_mbs_[simulatedRobotIndex]; auto& simArmConfig = rbd_mbcs_[simulatedRobotIndex]; + std::string currentPath = boost::filesystem::current_path().string()+"/data_in/Robone_KukaLBRiiwa.urdf"; + std::cout<<"currentPat: "<< currentPath << std::endl; - auto strRobot = grl::getURDFModel(); + auto strRobot = grl::getURDFModel(currentPath); rbd::MultiBody mb = strRobot.mb; rbd::MultiBodyConfig mbc(mb); rbd::MultiBodyGraph mbg(strRobot.mbg); std::size_t nrJoints = mb.nrJoints(); std::size_t nrBodies = strRobot.mb.nrBodies(); std::vector jointNames; - std::cout<<"Joint Size: "<< nrJoints << std::endl; - + std::vector vrepJointAngles; double angle = 0.7; for(auto i : jointHandles_) From e4aa5be2f16cef0d1b1926aad4d782192eaa873c Mon Sep 17 00:00:00 2001 From: Chunting Date: Fri, 6 Apr 2018 18:29:06 -0400 Subject: [PATCH 082/102] Remove some std::cout --- include/grl/vrep/InverseKinematicsVrepPlugin.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/include/grl/vrep/InverseKinematicsVrepPlugin.hpp b/include/grl/vrep/InverseKinematicsVrepPlugin.hpp index 8d17700..ebdabb9 100644 --- a/include/grl/vrep/InverseKinematicsVrepPlugin.hpp +++ b/include/grl/vrep/InverseKinematicsVrepPlugin.hpp @@ -610,7 +610,6 @@ void getPoseFromCSV(std::string filename, int time_index){ auto& simArmMultiBody = rbd_mbs_[simulatedRobotIndex]; auto& simArmConfig = rbd_mbcs_[simulatedRobotIndex]; std::string currentPath = boost::filesystem::current_path().string()+"/data_in/Robone_KukaLBRiiwa.urdf"; - std::cout<<"currentPat: "<< currentPath << std::endl; auto strRobot = grl::getURDFModel(currentPath); rbd::MultiBody mb = strRobot.mb; From c0d6185a40ec0ebcc3d1b139a8b913a3c20602ce Mon Sep 17 00:00:00 2001 From: Chunting Date: Sat, 7 Apr 2018 17:28:17 -0400 Subject: [PATCH 083/102] Data analysis document --- doc/howto/Data.rst | 104 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 104 insertions(+) create mode 100644 doc/howto/Data.rst diff --git a/doc/howto/Data.rst b/doc/howto/Data.rst new file mode 100644 index 0000000..4a32f9f --- /dev/null +++ b/doc/howto/Data.rst @@ -0,0 +1,104 @@ +========== +Data Setup +========== + +.. note:: Here I will interpret the data file, including the relative location and the main content. + +Flatbuffer fbs file +================================== + +First install `VREP `__ and grl. + +The location of the 'fbs file '__. +They define the data we collect from Kuka and Atracsys. + +The binary files are put in VREP folder (i.e. ~/src/V-REP_PRO_EDU_V3_4_0_Linux), and are named by the time stamp (i.e. 2018_03_26_19_06_21_FusionTrack.flik, 2018_03_26_19_06_21_Kukaiiwa.iiwa). + +The command to generate json file, the binary file and the fbs file should be put in the same folder: +flatc -I . --json LogKUKAiiwaFusionTrack.fbs -- 2018_03_26_19_06_21_FusionTrack.flik +flatc -I . --json KUKAiiwa.fbs -- 2018_03_26_19_06_21_Kukaiiwa.iiwa + +CSV file +================================== + +When Parsing the flatbuffer file to CSV, you need to follow the instructions below: +1. Copy the binary files to the location of the actual 'readFlatbufferTest '__executable; +2. Run 'readFlatbufferTest '__ with the arguments of the name of binary file. + .. code-block:: bash + ./readFlatbufferTest 2018_03_26_19_06_21_Kukaiiwa.iiwa 2018_03_26_19_06_21_FusionTrack.flik + # You can also pass the arguments manually to the main() function in readFlatbufferTest.cpp. +3. The generated csv files will be put into a folder in the name of time stamp. + Label explanation: + .. code-block:: bash + local_request_time_offset: PC time of sending a requesting command to devices; + local_receive_time_offset: PC time of receiving measured data from devices; + device_time_offset: the time from device; + time_Y: time driftting, device_time_offset - local_request_time_offset; + counter: the identifier of message, defined by devices; + X Y Z A B C: the cartesian postion and oritation in Plucker coordinate system; + M_Pos_Ji: measured joint position of joint i from kuka; + C_Pos_J*: command joint position of joint i to kuka; + M_Tor_J*: measured joint torque of joint i form kuka; + C_Tor_J*: comand joint torque of joint i to kuka; + E_Tor_J*: external torque of joint i exerted on kuka; + + # on the first message save local_request_time as the initial_local_request_time. Both kuka and Atracsys share the same initial_local_request_time, which means they have the same time axis. + # on the first message save device_time as the initial_device_time + X = local_request_time + local_request_offset = (local_request_time - initial_local_request_time) + device_offset = (device_time - initial_device_time) + time_Y = device_offset - local_request_offset + CSV file explanation: + FTKUKA_TimeEvent.csv has the information from both Atracsys and kuka. It can help analysis time event. + FT_Pose_Marker*.csv have the time event and pose (in Plucker coordinate) of the specific marker in Atracsys space; + FT_TimeEvent.csv gives more detail information about time event from Atracsys, such as the time step; + KUKA_FRIMessage.csv includes all the FRI message from robot; + KUKA_Command_Joint.csv has the commanding joint angle sent to robot, which should use local_request_time_offset as time axis when plotting; + KUKA_Measured_Joint.csv has the measured joint angles received from robt, which should use local_receive_time_offset as time axis when plotting. + KUKA_TimeEvent.csv gives more detail information about time event from kuka, such as the time step; + + All the CSV files above are generated from binary files. To make it convenient, all the files have time event information, which can be used as X-axis when plotting. + + ForwardKinematics_Pose.csv is created by the replay process(at this moment, it's in the InverseKinematicsVrepPlug.cpp. + In future, It's better to creat a replay plugin to handl this process). It will read the measured joint angles from KUKA_Measured_Joint.csv, excute forward kinematics based on vrep model, + then write the cartesian pose (in Atracsys space) into ForwardKinematics_Pose.csv, which should be in ~/src/V-REP_PRO_EDU_V3_4_0_Linux/. + + + + + + + + + + + + + .. code-block:: bash + FT_Pose_Marker22.csv + + + + +run 'readFlatbufferTest '__. +There is a `SymbolicLinksRoboneSimulation.sh `__ script to assist with this, which you can open and edit for your particular system. + +.. code-block:: bash + + GDIR="/path/to/grl/" + # Mac example directory: VDIR="/Applications/V-REP_PRO_EDU_V3_3_2_Mac/vrep.app/Contents/MacOS/" + # Linux example directory: VDIR="~/V-REP_PRO_EDU_V3_3_2_Linux/" + # + # cd into the appropriate directory, then create the following symlinks + ln -s src/lua/grl.lua ${VDIR}/ + ln -s build/libv_repExtKukaLBRiiwa ${VDIR}/ + # ... continue for all libraries created for grl + + +An example simulation can be found in the `Robone/data `__ project folder. + + +Then simply open the project with V-REP and you can be on your way! + +Please note that this project is currently set up in a fairly specific way, +so you will most \ No newline at end of file From 49b6eb5801830b516c3f7bc84312ad8a195581e9 Mon Sep 17 00:00:00 2001 From: Chunting Date: Sat, 7 Apr 2018 17:33:07 -0400 Subject: [PATCH 084/102] Edit the document --- doc/howto/Data.rst | 42 ++---------------------------------------- 1 file changed, 2 insertions(+), 40 deletions(-) diff --git a/doc/howto/Data.rst b/doc/howto/Data.rst index 4a32f9f..939f633 100644 --- a/doc/howto/Data.rst +++ b/doc/howto/Data.rst @@ -22,7 +22,9 @@ CSV file ================================== When Parsing the flatbuffer file to CSV, you need to follow the instructions below: + 1. Copy the binary files to the location of the actual 'readFlatbufferTest '__executable; + 2. Run 'readFlatbufferTest '__ with the arguments of the name of binary file. .. code-block:: bash ./readFlatbufferTest 2018_03_26_19_06_21_Kukaiiwa.iiwa 2018_03_26_19_06_21_FusionTrack.flik @@ -62,43 +64,3 @@ When Parsing the flatbuffer file to CSV, you need to follow the instructions bel ForwardKinematics_Pose.csv is created by the replay process(at this moment, it's in the InverseKinematicsVrepPlug.cpp. In future, It's better to creat a replay plugin to handl this process). It will read the measured joint angles from KUKA_Measured_Joint.csv, excute forward kinematics based on vrep model, then write the cartesian pose (in Atracsys space) into ForwardKinematics_Pose.csv, which should be in ~/src/V-REP_PRO_EDU_V3_4_0_Linux/. - - - - - - - - - - - - - .. code-block:: bash - FT_Pose_Marker22.csv - - - - -run 'readFlatbufferTest '__. -There is a `SymbolicLinksRoboneSimulation.sh `__ script to assist with this, which you can open and edit for your particular system. - -.. code-block:: bash - - GDIR="/path/to/grl/" - # Mac example directory: VDIR="/Applications/V-REP_PRO_EDU_V3_3_2_Mac/vrep.app/Contents/MacOS/" - # Linux example directory: VDIR="~/V-REP_PRO_EDU_V3_3_2_Linux/" - # - # cd into the appropriate directory, then create the following symlinks - ln -s src/lua/grl.lua ${VDIR}/ - ln -s build/libv_repExtKukaLBRiiwa ${VDIR}/ - # ... continue for all libraries created for grl - - -An example simulation can be found in the `Robone/data `__ project folder. - - -Then simply open the project with V-REP and you can be on your way! - -Please note that this project is currently set up in a fairly specific way, -so you will most \ No newline at end of file From f963ff6ec531284aae3e3783a40460b96af8c186 Mon Sep 17 00:00:00 2001 From: Chunting Date: Sun, 8 Apr 2018 15:30:00 -0400 Subject: [PATCH 085/102] Reorganise the folder structure to make it convenient to read and write. --- include/grl/kuka/KukaFRIdriver.hpp | 4 +- include/grl/sensor/FusionTrackLogAndTrack.hpp | 4 +- .../grl/vrep/InverseKinematicsVrepPlugin.hpp | 25 ++++++----- test/readFlatbufferTest.cpp | 45 ++++++++++--------- 4 files changed, 43 insertions(+), 35 deletions(-) diff --git a/include/grl/kuka/KukaFRIdriver.hpp b/include/grl/kuka/KukaFRIdriver.hpp index 8b49c17..b785746 100644 --- a/include/grl/kuka/KukaFRIdriver.hpp +++ b/include/grl/kuka/KukaFRIdriver.hpp @@ -686,8 +686,10 @@ class KukaFRIdriver : public std::enable_shared_from_thisinfo("Save Recording as in Kuka: {}", filename); diff --git a/include/grl/sensor/FusionTrackLogAndTrack.hpp b/include/grl/sensor/FusionTrackLogAndTrack.hpp index b70dba1..ef5f593 100644 --- a/include/grl/sensor/FusionTrackLogAndTrack.hpp +++ b/include/grl/sensor/FusionTrackLogAndTrack.hpp @@ -367,8 +367,10 @@ class FusionTrackLogAndTrack : public std::enable_shared_from_thisinfo("Save Recording as: {}", filename); diff --git a/include/grl/vrep/InverseKinematicsVrepPlugin.hpp b/include/grl/vrep/InverseKinematicsVrepPlugin.hpp index 8d17700..08ca74c 100644 --- a/include/grl/vrep/InverseKinematicsVrepPlugin.hpp +++ b/include/grl/vrep/InverseKinematicsVrepPlugin.hpp @@ -554,10 +554,8 @@ void getPoseFromCSV(std::string filename, int time_index){ // // Write the hand eye calibration results into a file std::string suffix = "ForwardKinematics_Pose.csv"; auto myFile = boost::filesystem::current_path() /suffix; - // if(boost::filesystem::exists(myFile)){ - // boost::filesystem::remove(myFile); - // } - boost::filesystem::ofstream ofs(myFile, std::ios_base::app); + + boost::filesystem::ofstream ofs(myFile, std::ios_base::trunc | std::ios_base::out); if(time_index == 0) { ofs << "local_receive_time_offset_X, K_X,K_Y,K_Z,K_A,K_B,K_C\n"; } @@ -609,10 +607,13 @@ void getPoseFromCSV(std::string filename, int time_index){ const std::size_t simulatedRobotIndex = 0; auto& simArmMultiBody = rbd_mbs_[simulatedRobotIndex]; auto& simArmConfig = rbd_mbcs_[simulatedRobotIndex]; - std::string currentPath = boost::filesystem::current_path().string()+"/data_in/Robone_KukaLBRiiwa.urdf"; - std::cout<<"currentPat: "<< currentPath << std::endl; - auto strRobot = grl::getURDFModel(currentPath); + std::string homePath = std::getenv("HOME"); + std::string vrepDataPath = homePath + "/src/V-REP_PRO_EDU_V3_4_0_Linux/data/"; + std::string urdfPath = vrepDataPath + "Robone_KukaLBRiiwa.urdf"; + + + auto strRobot = grl::getURDFModel(urdfPath); rbd::MultiBody mb = strRobot.mb; rbd::MultiBodyConfig mbc(mb); rbd::MultiBodyGraph mbg(strRobot.mbg); @@ -927,10 +928,12 @@ void getPoseFromCSV(std::string filename, int time_index){ if(ik) { updateKinematics(); } else { - auto myFile = boost::filesystem::current_path() /"data_in"; - std::string pathName = myFile.string(); - std::string kukaJoint = pathName+"/KUKA_Joint.csv"; // KUKA_Joint.csv FTKUKA_TimeEvent.csv - std::string markerPose = pathName+"/FT_Pose_Marker22.csv"; + std::string homePath = std::getenv("HOME"); + std::string vrepDataPath = homePath + "/src/V-REP_PRO_EDU_V3_4_0_Linux/data/"; + std::string data_inPath = vrepDataPath + "data_in"; + + std::string kukaJoint = data_inPath+"/KUKA_Measured_Joint.csv"; // KUKA_Joint.csv FTKUKA_TimeEvent.csv + std::string markerPose = data_inPath+"/FT_Pose_Marker22.csv"; testPose(); // getPoseFromCSV(kukaJoint, time_index); diff --git a/test/readFlatbufferTest.cpp b/test/readFlatbufferTest.cpp index 0c8d4bd..3e1796c 100644 --- a/test/readFlatbufferTest.cpp +++ b/test/readFlatbufferTest.cpp @@ -40,31 +40,32 @@ int main(int argc, char* argv[]) kukaTimeStamp = std::string(argv[1]); FTTimeStamp = std::string(argv[2]); } - - std::string currentPath = boost::filesystem::current_path().string()+"/"; - std::string kukaBinaryfile = currentPath + kukaTimeStamp; + std::string homePath = std::getenv("HOME"); + std::string vrepPath = homePath + "/src/V-REP_PRO_EDU_V3_4_0_Linux/data/"; + + std::string kukaBinaryfile = vrepPath + kukaTimeStamp; std::cout << kukaBinaryfile << std::endl; - std::string fusiontrackBinaryfile = currentPath + FTTimeStamp; - std::string urdfFile = currentPath + URDFModrl; + std::string fusiontrackBinaryfile = vrepPath + FTTimeStamp; + std::string urdfFile = vrepPath + URDFModrl; std::string foldtimestamp = current_date_and_time_string(); // Write the generated files into a new fold - boost::filesystem::path dir{currentPath+foldtimestamp}; - boost::filesystem::create_directory(dir); + boost::filesystem::path dir{vrepPath+foldtimestamp}; + boost::filesystem::create_directory(vrepPath+foldtimestamp); - std::string KUKA_FRI_CSVfilename = currentPath + foldtimestamp + "/KUKA_FRIMessage.csv"; - std::string KUKA_TimeEvent_CSV = currentPath + foldtimestamp + "/KUKA_TimeEvent.csv"; - std::string M_Joint_CSV = currentPath + foldtimestamp + "/KUKA_Measured_Joint.csv"; - std::string C_Joint_CSV = currentPath + foldtimestamp + "/KUKA_Command_Joint.csv"; - std::string KUKA_Pose_CSV = currentPath + foldtimestamp + "/KUKA_Pose.csv"; - std::string KUKA_Inverse_Pose_CSV = currentPath + foldtimestamp + "/Inverse_KUKA_Pose.csv"; - std::string FudicialToRobotPose_CSV = currentPath + foldtimestamp + "/FudicialToRobot_Pose.csv"; - std::string FudicialToFTPose_CSV = currentPath + foldtimestamp + "/FudicialToFT_Pose.csv"; - - std::string FT_TimeEvent_CSV = currentPath + foldtimestamp + "/FT_TimeEvent.csv"; - std::string FT_Marker22_CSV = currentPath + foldtimestamp + "/FT_Pose_Marker22.csv"; - std::string FT_Marker55_CSV = currentPath + foldtimestamp + "/FT_Pose_Marker55.csv"; - std::string FT_Marker50000_CSV = currentPath + foldtimestamp + "/FT_Pose_Marker50000.csv"; - - std::string FTKUKA_TimeEvent_CSV = currentPath + foldtimestamp + "/FTKUKA_TimeEvent.csv"; + std::string KUKA_FRI_CSVfilename = vrepPath + foldtimestamp + "/KUKA_FRIMessage.csv"; + std::string KUKA_TimeEvent_CSV = vrepPath + foldtimestamp + "/KUKA_TimeEvent.csv"; + std::string M_Joint_CSV = vrepPath + foldtimestamp + "/KUKA_Measured_Joint.csv"; + std::string C_Joint_CSV = vrepPath + foldtimestamp + "/KUKA_Command_Joint.csv"; + std::string KUKA_Pose_CSV = vrepPath + foldtimestamp + "/KUKA_Pose.csv"; + std::string KUKA_Inverse_Pose_CSV = vrepPath + foldtimestamp + "/Inverse_KUKA_Pose.csv"; + std::string FudicialToRobotPose_CSV = vrepPath + foldtimestamp + "/FudicialToRobot_Pose.csv"; + std::string FudicialToFTPose_CSV = vrepPath + foldtimestamp + "/FudicialToFT_Pose.csv"; + + std::string FT_TimeEvent_CSV = vrepPath + foldtimestamp + "/FT_TimeEvent.csv"; + std::string FT_Marker22_CSV = vrepPath + foldtimestamp + "/FT_Pose_Marker22.csv"; + std::string FT_Marker55_CSV = vrepPath + foldtimestamp + "/FT_Pose_Marker55.csv"; + std::string FT_Marker50000_CSV = vrepPath + foldtimestamp + "/FT_Pose_Marker50000.csv"; + + std::string FTKUKA_TimeEvent_CSV = vrepPath + foldtimestamp + "/FTKUKA_TimeEvent.csv"; // Put all the data into the same coordinate system --- Marker frame From 12bfa6869f10db2f462aab698e7054bc52d6a372 Mon Sep 17 00:00:00 2001 From: Chunting Date: Mon, 9 Apr 2018 07:08:49 -0400 Subject: [PATCH 086/102] Document about data analysis --- doc/howto/Data.rst | 22 ++++++++++++++-------- 1 file changed, 14 insertions(+), 8 deletions(-) diff --git a/doc/howto/Data.rst b/doc/howto/Data.rst index 939f633..06e8b3b 100644 --- a/doc/howto/Data.rst +++ b/doc/howto/Data.rst @@ -2,7 +2,7 @@ Data Setup ========== -.. note:: Here I will interpret the data file, including the relative location and the main content. +.. note:: I will interpret the data files, including the relative location and the main content. Flatbuffer fbs file ================================== @@ -10,9 +10,10 @@ Flatbuffer fbs file First install `VREP `__ and grl. The location of the 'fbs file '__. -They define the data we collect from Kuka and Atracsys. +They define the data structure we collect from Kuka and Atracsys. -The binary files are put in VREP folder (i.e. ~/src/V-REP_PRO_EDU_V3_4_0_Linux), and are named by the time stamp (i.e. 2018_03_26_19_06_21_FusionTrack.flik, 2018_03_26_19_06_21_Kukaiiwa.iiwa). +The binary files are put in VREP data folder (i.e. ~/src/V-REP_PRO_EDU_V3_4_0_Linux/data/), +and are named by the time stamp (i.e. 2018_03_26_19_06_21_FusionTrack.flik, 2018_03_26_19_06_21_Kukaiiwa.iiwa). The command to generate json file, the binary file and the fbs file should be put in the same folder: flatc -I . --json LogKUKAiiwaFusionTrack.fbs -- 2018_03_26_19_06_21_FusionTrack.flik @@ -23,13 +24,13 @@ CSV file When Parsing the flatbuffer file to CSV, you need to follow the instructions below: -1. Copy the binary files to the location of the actual 'readFlatbufferTest '__executable; +1. Keep the binary files in the VREP data folder (i.e. ~/src/V-REP_PRO_EDU_V3_4_0_Linux/data/); 2. Run 'readFlatbufferTest '__ with the arguments of the name of binary file. .. code-block:: bash ./readFlatbufferTest 2018_03_26_19_06_21_Kukaiiwa.iiwa 2018_03_26_19_06_21_FusionTrack.flik # You can also pass the arguments manually to the main() function in readFlatbufferTest.cpp. -3. The generated csv files will be put into a folder in the name of time stamp. +3. The generated csv files will be put into the VREP data folder in which the subfolder is named by time stamp. Label explanation: .. code-block:: bash local_request_time_offset: PC time of sending a requesting command to devices; @@ -61,6 +62,11 @@ When Parsing the flatbuffer file to CSV, you need to follow the instructions bel All the CSV files above are generated from binary files. To make it convenient, all the files have time event information, which can be used as X-axis when plotting. - ForwardKinematics_Pose.csv is created by the replay process(at this moment, it's in the InverseKinematicsVrepPlug.cpp. - In future, It's better to creat a replay plugin to handl this process). It will read the measured joint angles from KUKA_Measured_Joint.csv, excute forward kinematics based on vrep model, - then write the cartesian pose (in Atracsys space) into ForwardKinematics_Pose.csv, which should be in ~/src/V-REP_PRO_EDU_V3_4_0_Linux/. + +Replay Process +================================== +At this moment, the replay process is set and called in InverseKinematicsVrepPlug.cpp. +In future, It's better to creat a replay plugin to handl this process. + +Before run it, you should copy the KUKA_Measured_Joint.csv and FT_Pose_Marker22.csv to the ~/src/V-REP_PRO_EDU_V3_4_0_Linux/data/data_in/. +The result will be writen in ForwardKinematics_Pose.csv. From ff48866a1b4118c31f144eb0744d17c9abf6c9d3 Mon Sep 17 00:00:00 2001 From: Chunting Date: Tue, 10 Apr 2018 17:36:05 -0400 Subject: [PATCH 087/102] Improved the code based on the reviews. --- src/lua/robone.lua | 147 +----------------- .../v_repExtAtracsysFusionTrack.cpp | 12 +- .../CMakeLists.txt | 1 - .../v_repExtKukaLBRiiwa.cpp | 8 +- test/KukaFRITest.cpp | 3 +- test/readFlatbufferTest.cpp | 134 ++++++++-------- 6 files changed, 79 insertions(+), 226 deletions(-) diff --git a/src/lua/robone.lua b/src/lua/robone.lua index a37555e..b14b1c4 100644 --- a/src/lua/robone.lua +++ b/src/lua/robone.lua @@ -453,62 +453,6 @@ robone.startRealArmDriverScript=function() else simDisplayDialog('Error','KukaLBRiiwa plugin was not found. (v_repExtKukaLBRiiwa.dll)&&nSimulation will run without hardware',sim_dlgstyle_ok,true,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1}) end -end - -robone.coordinateCalibrateionScript=function() - print("coordinateCalibrateionScript--------------") --- Check if the required plugin is there: - if (grl.isModuleLoaded('KukaLBRiiwa')) then - - -- Now disable IK and control one individual joint: - -- simSetExplicitHandling(ikGroupHandle,0) -- this disables automatic handling of the ik group - - - testJointAngles = { -0.06, -0.03, 0.01, -0.06, -0.04, -0.02, 0.1} - jointHandles={-1,-1,-1,-1,-1,-1,-1} - jointHandles[1]=simGetObjectHandle('LBR_iiwa_14_R820_joint1') - for i=2,7,1 do - jointHandles[i]=simGetObjectHandle('LBR_iiwa_14_R820_joint'..i) - result = simSetJointPosition(jointHandles[i],0) - simWait(0.5) - print(result.."======--------------") - - - - relativePosition = simGetObjectPosition(jointHandles[i], jointHandles[i-1]) - print('XYZ') - for i=1,3,1 do - print(relativePosition[i].." ") - end - - relativeOrientation = simGetObjectOrientation(RobotFlangeTip, robothandle) - print('RPY') - for i=1,3,1 do - print(relativeOrientation[i]*180/math.pi.." ") - end - - end - simWait(50) - --robotFlangeTip = simGetObjectHandle ('RobotFlangeTip') - --robothandle = simGetObjectHandle ('LBR_iiwa_14_R820') - --Position = simGetObjectPosition(robotFlangeTip, robothandle) - --print('The Position of RobotFlangeTip in RobotBase Frame') - --for i=1,3,1 do - -- print(Position[i].." ") - --end - --Orientation = simGetObjectOrientation(robotFlangeTip, robothandle) - --print('The Orientation of RobotFlangeTip in RobotBase Frame') - --for i=1,3,1 do - -- print(Orientation[i].." ") - --end - - else - simDisplayDialog('Error','KukaLBRiiwa plugin was not found. (v_repExtKukaLBRiiwa.dll)&&nSimulation will run without hardware',sim_dlgstyle_ok,true,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1}) - end - - - - end ------------------------------------------ @@ -546,6 +490,7 @@ robone.configureOpticalTracker=function() -------------------------------------------------- + -- Move the Tracker--- -- Move the Tracker -- true enables moving the tracker, false disables it moveTracker = false @@ -571,8 +516,6 @@ robone.configureOpticalTracker=function() end - - -------------------------------------------------- -- Move the Bone -- true enables moving the bone, false disables it @@ -588,7 +531,8 @@ robone.configureOpticalTracker=function() '55' -- GeometryID ) - + -- Get the transform matrix of robot base against world frame. + -- This is also for the calibration purpose, it can print the absolute position and orientation of the robot base. robothandle = simGetObjectHandle ('LBR_iiwa_14_R820#0') --trankerhandle = simGetObjectHandle ('OpticalTrackerBase#0') robotToWorldM = simGetObjectMatrix(robothandle, -1) @@ -604,90 +548,7 @@ robone.configureOpticalTracker=function() -- Test the below one -- Enable the recordDataScript() to call the method below. simExtAtracsysFusionTrackRecordWhileSimulationIsRunning(true, FB_single_buffer_limit_bytes) - -------------------------------------------------- - -- Get the relative transformation matrix between Fiducial#22 and LBR_iiwa_14_R820_link8 - --[[ - if (moveTracker) then - - - -- testJointAngles = { -0.06, -0.03, 0.01, -0.06, -0.04, -0.02, 0.1} - jointHandles={-1,-1,-1,-1,-1,-1,-1} - jointHandles[1]=simGetObjectHandle('LBR_iiwa_14_R820_joint1') - for i=1,7,1 do - jointHandles[i]=simGetObjectHandle('LBR_iiwa_14_R820_joint'..i) - -- result = simSetJointPosition(jointHandles[i],testJointAngles[i]) - end - - - - RobotFlangeTip = simGetObjectHandle ('RobotFlangeTip') - robothandle = simGetObjectHandle ('LBR_iiwa_14_R820') - fiducial22handl = simGetObjectHandle ('Fiducial#22') - relativeTransformationMatrix = simGetObjectMatrix(fiducial22handl, RobotFlangeTip) - print('The relative transformation matrix between Fiducial#22 and RobotFlangeTip') - for i=1,12,1 do - print(relativeTransformationMatrix[i].." ") - end - - - trankerhandle = simGetObjectHandle ('OpticalTrackerBase#0') - robotToTrakerM = simGetObjectMatrix(robothandle, trankerhandle) - print('The relative transformation matrix between LBR_iiwa_14_R820#0 and OpticalTrackerBase#0') - for i=1,12,1 do - print(robotToTrakerM[i].." ") - end - - fudicialToRobotM = simGetObjectMatrix(fiducial22handl, robothandle) - print('The relative transformation matrix between Fiducial#22 and LBR_iiwa_14_R820') - for i=1,12,1 do - print(fudicialToRobotM[i].." ") - end - - relativePosition = simGetObjectPosition(RobotFlangeTip, robothandle) - print('The relative position between RobotFlangeTip and LBR_iiwa_14_R820 ') - for i=1,3,1 do - print(relativePosition[i].." ") - end - - relativeOrientation = simGetObjectOrientation(RobotFlangeTip, robothandle) - print('The relative orientation between RobotFlangeTip and LBR_iiwa_14_R820') - for i=1,3,1 do - print(relativeOrientation[i]*180/math.pi.." ") - end - - - print("======--------------") - relativePosition = simGetObjectPosition(jointHandles[1], -1) - print('XYZ'..1) - for i=1,3,1 do - print(relativePosition[i].." ") - end - - relativeOrientation = simGetObjectOrientation(jointHandles[1], -1) - print('RPY') - for i=1,3,1 do - print(relativeOrientation[i]*180/math.pi.." ") - end - for i=2,7,1 do - --result = simSetJointPosition(jointHandles[i],0) - --simWait(0.5) - print("======--------------") - relativePosition = simGetObjectPosition(jointHandles[i], jointHandles[i-1]) - print('XYZ'..i) - for j=1,3,1 do - print(relativePosition[j].." ") - end - - relativeOrientation = simGetObjectOrientation(jointHandles[i], jointHandles[i-1]) - print('RPY'..i) - for j=1,3,1 do - print(relativeOrientation[j]*180/math.pi.." ") - end - - end - - end - --]] + end diff --git a/src/v_repExtAtracsysFusionTrack/v_repExtAtracsysFusionTrack.cpp b/src/v_repExtAtracsysFusionTrack/v_repExtAtracsysFusionTrack.cpp index 5f413b5..e5fc60c 100755 --- a/src/v_repExtAtracsysFusionTrack/v_repExtAtracsysFusionTrack.cpp +++ b/src/v_repExtAtracsysFusionTrack/v_repExtAtracsysFusionTrack.cpp @@ -325,7 +325,7 @@ void LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_IS_ACTIVE(SLuaCallBack *p) ///////////////////////////////////////////////////////////////////////////////////////// const int inArgs_LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_START_RECORDING[] = { 1, - sim_lua_arg_float,0 // Signle buffer limit + sim_lua_arg_float,0 // Single buffer limit }; // simExtAtracsysFusionTrackStartRecording void LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_START_RECORDING(SLuaCallBack *p) @@ -334,7 +334,7 @@ void LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_START_RECORDING(SLuaCallBack *p) bool success = false; if (fusionTrackPG) { - std::string log_message("Starting the recording of fusiontrack frame data in memory.\n"); + std::string log_message("Starting in-memory recording of fusiontrack frame data.\n"); std::vector *inData = D.getInDataPtr(); simAddStatusbarMessage(log_message.c_str()); loggerPG->info(log_message); @@ -354,7 +354,7 @@ void LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_STOP_RECORDING(SLuaCallBack *p) bool success = false; if (fusionTrackPG) { - std::string log_message("Stopping the recording of fusiontrack frame data in memory.\n"); + std::string log_message("Stoping in-memory recording of fusiontrack frame data.\n"); simAddStatusbarMessage(log_message.c_str()); loggerPG->info(log_message); success = fusionTrackPG->stop_recording(); @@ -422,11 +422,13 @@ void LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_SAVE_RECORDING(SLuaCallBack *p) ///////////////////////////////////////////////////////////////////////////// #define LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_RECORD_WHILE_SIMULATION_IS_RUNNING_COMMAND "simExtAtracsysFusionTrackRecordWhileSimulationIsRunning" - +// 2GB is the hard limit for flatbuffer binary file, so when it hits this hard limit, it should write the data from memory to disk. +// Here to make it convenient to analysis the data, you can set it any capacity based on your purpose. +// When it reaches the capacity, new file will be created. const int inArgs_LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_RECORD_WHILE_SIMULATION_IS_RUNNING[] = { 2, sim_lua_arg_bool, 1, // string file name - sim_lua_arg_float,0 // Signle buffer limit + sim_lua_arg_float,0 // Single buffer limit }; std::string LUA_SIM_EXT_ATRACSYS_FUSION_TRACK_RECORD_WHILE_SIMULATION_IS_RUNNING_CALL_TIP("number result=simExtAtracsysFusionTrackRecordWhileSimulationIsRunning(bool recording, float single_buffer_limit_bytes)"); diff --git a/src/v_repExtGrlCisstInverseKinematics/CMakeLists.txt b/src/v_repExtGrlCisstInverseKinematics/CMakeLists.txt index 653c33e..2ae5470 100644 --- a/src/v_repExtGrlCisstInverseKinematics/CMakeLists.txt +++ b/src/v_repExtGrlCisstInverseKinematics/CMakeLists.txt @@ -32,7 +32,6 @@ #TODO: add cisst-saw found flag and make it work correctly for independent grl https://github.com/jhu-cisst/cisst-saw if(cisst_cisstNumerical_FOUND AND CISSTNETLIB_FOUND AND sawConstraintController_FOUND AND Boost_FOUND) - #TODO: #WARNING: INSTALLER WILL BE BROKEN UNLESS THIS IS CHANGED TO USE basis_target_link_libraries, etc. #@see https://github.com/schuhschuh/cmake-basis/issues/442 basis_include_directories( ${PROJECT_INCLUDE_DIR}/thirdparty/vrep/include diff --git a/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp b/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp index f7ddeb0..86797ca 100755 --- a/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp +++ b/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp @@ -166,7 +166,7 @@ void LUA_SIM_EXT_KUKA_LBR_IIWA_START(SLuaCallBack* p) ///////////////////////////////////////////////////////////////////////////////////////// const int inArgs_LUA_SIM_EXT_KUKA_LBR_IIWA_START_RECORDING[] = { 1, - sim_lua_arg_float,0 // Signle buffer limit + sim_lua_arg_float,0 // Single buffer limit }; // simExtKUKAiiwaStartRecording void LUA_SIM_EXT_KUKA_LBR_IIWA_START_RECORDING(SLuaCallBack *p) @@ -262,7 +262,7 @@ void LUA_SIM_EXT_KUKA_LBR_IIWA_CLEAR_RECORDING(SLuaCallBack *p) const int inArgs_LUA_SIM_EXT_KUKA_LBR_IIWA_RECORD_WHILE_SIMULATION_IS_RUNNING[] = { 2, sim_lua_arg_bool, 1, // string file name - sim_lua_arg_float,0 // Signle buffer limit + sim_lua_arg_float,0 // Single buffer limit }; std::string LUA_SIM_EXT_KUKA_LBR_IIWA_RECORD_WHILE_SIMULATION_IS_RUNNING_CALL_TIP("number result=simExtKukaLBRiiwaRecordWhileSimulationIsRunning(bool recording, float single_buffer_limit_bytes)"); @@ -536,10 +536,6 @@ VREP_DLLEXPORT void* v_repMessage(int message,int* auxiliaryData,void* customDat if(kukaVrepPluginPG && recordWhileSimulationIsRunningG && kukaVrepPluginPG->is_recording()) { bool success = kukaVrepPluginPG->save_recording(); - if(success) { - loggerPG->info("Vrep quits successfully..." ); - } - kukaVrepPluginPG->stop_recording(); } kukaVrepPluginPG->clear_recording(); diff --git a/test/KukaFRITest.cpp b/test/KukaFRITest.cpp index 23091b9..5b91ab5 100644 --- a/test/KukaFRITest.cpp +++ b/test/KukaFRITest.cpp @@ -87,6 +87,7 @@ int main(int argc, char* argv[]) std::string localport("30200"); std::string remotehost("192.170.10.2"); std::string remoteport("30200"); + int single_buffer_limit = 2; std::cout << "argc: " << argc << "\n"; if (argc !=5 && argc !=1) @@ -248,7 +249,7 @@ int main(int argc, char* argv[]) { kukaDriverP->set( jointStateToCommand, grl::revolute_joint_angle_open_chain_command_tag()); - kukaDriverP->start_recording(2); + kukaDriverP->start_recording(single_buffer_limit); kukaDriverP->run_one(); } diff --git a/test/readFlatbufferTest.cpp b/test/readFlatbufferTest.cpp index 3e1796c..06bf7fb 100644 --- a/test/readFlatbufferTest.cpp +++ b/test/readFlatbufferTest.cpp @@ -24,9 +24,8 @@ #include #include - +/// Command of spliting a large file into some smaller ones. /// split -C 200m --numeric-suffixes 2018_02_28_16_39_13_FusionTrack.json 2018_02_28_16_39_13_FusionTrack - /// The command to get the json file from flatbuffer binary file, these two files should be located in the same folder. /// flatc -I . --json LogKUKAiiwaFusionTrack.fbs -- 2018_03_14_22_06_29_FusionTrack.flik @@ -40,9 +39,9 @@ int main(int argc, char* argv[]) kukaTimeStamp = std::string(argv[1]); FTTimeStamp = std::string(argv[2]); } + /// Create a new folder in the name of time stamp for the generated csv files. std::string homePath = std::getenv("HOME"); std::string vrepPath = homePath + "/src/V-REP_PRO_EDU_V3_4_0_Linux/data/"; - std::string kukaBinaryfile = vrepPath + kukaTimeStamp; std::cout << kukaBinaryfile << std::endl; std::string fusiontrackBinaryfile = vrepPath + FTTimeStamp; @@ -50,7 +49,19 @@ int main(int argc, char* argv[]) std::string foldtimestamp = current_date_and_time_string(); // Write the generated files into a new fold boost::filesystem::path dir{vrepPath+foldtimestamp}; boost::filesystem::create_directory(vrepPath+foldtimestamp); - + /* + CSV file explanation: + FTKUKA_TimeEvent.csv has the information from both Atracsys and kuka. It can help analysis time event. + FT_Pose_Marker*.csv have the time event and pose (in Plucker coordinate) of the specific marker in Atracsys space; + FT_TimeEvent.csv gives more detail information about time event from Atracsys, such as the time step; + KUKA_FRIMessage.csv includes all the FRI message from robot; + KUKA_Command_Joint.csv has the commanding joint angle sent to robot, which should use local_request_time_offset as time axis when plotting; + KUKA_Measured_Joint.csv has the measured joint angles received from robt, which should use local_receive_time_offset as time axis when plotting. + KUKA_TimeEvent.csv gives more detail information about time event from kuka, such as the time step; + + All the CSV files above are generated from binary files. + To make it convenient, all the files have time event information, which can be used as X-axis when plotting. + */ std::string KUKA_FRI_CSVfilename = vrepPath + foldtimestamp + "/KUKA_FRIMessage.csv"; std::string KUKA_TimeEvent_CSV = vrepPath + foldtimestamp + "/KUKA_TimeEvent.csv"; std::string M_Joint_CSV = vrepPath + foldtimestamp + "/KUKA_Measured_Joint.csv"; @@ -67,9 +78,6 @@ int main(int argc, char* argv[]) std::string FTKUKA_TimeEvent_CSV = vrepPath + foldtimestamp + "/FTKUKA_TimeEvent.csv"; - - // Put all the data into the same coordinate system --- Marker frame - bool inMarkerFrame = false; // Indicate the marker pose is in marker frame or tracker frame. uint32_t markerID_22 = 22; uint32_t markerID_55 = 55; uint32_t markerID_50000 = 50000; @@ -92,6 +100,9 @@ int main(int argc, char* argv[]) Eigen::MatrixXd commandedTorque = Eigen::MatrixXd::Zero(kuka_size, grl::jointNum); Eigen::MatrixXd externalTorque = Eigen::MatrixXd::Zero(kuka_size, grl::jointNum); Eigen::MatrixXd jointStateInterpolated = Eigen::MatrixXd::Zero(kuka_size, grl::jointNum); + ///////////////////////////////////////////////////////////////////////////////////////////////////// + /// Get KUKA FRI message + //////////////////////////////////////////////////////////////////////////////////////////////////// int ret = grl::getFRIMessage(kukaStatesP, timeEventM_Kuka, sequenceCounterVec, @@ -138,7 +149,7 @@ int main(int argc, char* argv[]) timeEventM_Kuka.col(grl::TimeType::time_Y) = timeEventM_Kuka.col(grl::TimeType::time_Y) - timeEventM_Kuka(kuka_index, grl::TimeType::time_Y) * grl::VectorXd::Ones(kuka_time_size); ///////////////////////////////////////////////////////////////////////////////////////////////////// - /// Write KUKA data to CSV + /// Write KUKA FRI message to CSV //////////////////////////////////////////////////////////////////////////////////////////////////// grl::writeFRIMessageToCSV(KUKA_FRI_CSVfilename, @@ -153,64 +164,50 @@ int main(int argc, char* argv[]) jointStateInterpolated, kuka_index); - timeEventM_Kuka.col(grl::TimeType::counterIdx) = std::move(sequenceCounterVec.cast()); - grl::writeTimeEventToCSV(KUKA_TimeEvent_CSV, timeEventM_Kuka, kuka_index); - std::vector kuka_labels = grl::Time_Labels; - kuka_labels.insert(std::end(kuka_labels), std::begin(grl::M_Pos_Joint_Labels), std::end(grl::M_Pos_Joint_Labels)); - // Write the joint angles into csv - grl::writeMatrixToCSV(M_Joint_CSV, kuka_labels, timeEventM_Kuka, measuredJointPosition); - std::copy(std::begin(grl::C_Pos_Joint_Labels), std::end(grl::C_Pos_Joint_Labels),std::begin(kuka_labels)+grl::Time_Labels.size()); - grl::writeMatrixToCSV(C_Joint_CSV, kuka_labels, timeEventM_Kuka, commandedJointPosition); - - - auto strRobot = grl::getURDFModel(urdfFile); - rbd::MultiBody mb = strRobot.mb; - rbd::MultiBodyConfig mbc(mb); - rbd::MultiBodyGraph mbg(strRobot.mbg); - std::size_t nrJoints = mb.nrJoints(); - std::size_t nrBodies = strRobot.mb.nrBodies(); - std::vector jointNames; - std::cout<<"Joint Size: "<< nrJoints << std::endl; - - std::vector PK_Pose_Labels = grl::getLabels(grl::LabelsType::Kuka_Pose); - // forward kinematic to get the of the end effector - // markerPose_FT = true; // Get the marker pose directly, or get the end effector pose. - // bool markerPose_FT = false; - std::vector PoseEE = grl::getPoseEE(measuredJointPosition); - // convert the sva::PTransformd to Plucker coordinate. - Eigen::MatrixXd PKPose = grl::getPluckerPose(PoseEE); - // Write the plucker coordinate into csv. - grl::writeMatrixToCSV(KUKA_Pose_CSV, PK_Pose_Labels, timeEventM_Kuka, PKPose); - // Invert the EE pose - std::vector inversePose = grl::invertPose(PoseEE); - // convert the sva::PTransformd to Plucker coordinate. - PKPose = Eigen::MatrixXd::Zero(PKPose.rows(), PKPose.cols()); - PKPose = grl::getPluckerPose(inversePose); - // Write the plucker coordinate into csv. - - grl::writeMatrixToCSV(KUKA_Inverse_Pose_CSV, PK_Pose_Labels, timeEventM_Kuka, PKPose); - - - // // Pose of end-effector againt robot base frame - // std::vector Fudicial_Robot_Pose = grl::getEEToFudicial22Matrix(PoseEE, inMarkerFrame); - - // PKPose = Eigen::MatrixXd::Zero(PKPose.rows(), PKPose.cols()); - // PKPose = grl::getPluckerPose(Fudicial_Robot_Pose); - // grl::writeMatrixToCSV(FudicialToRobotPose_CSV, PK_Pose_Labels, timeEventM_Kuka, PKPose); - - - + timeEventM_Kuka.col(grl::TimeType::counterIdx) = std::move(sequenceCounterVec.cast()); + grl::writeTimeEventToCSV(KUKA_TimeEvent_CSV, timeEventM_Kuka, kuka_index); + std::vector kuka_labels = grl::Time_Labels; + kuka_labels.insert(std::end(kuka_labels), std::begin(grl::M_Pos_Joint_Labels), std::end(grl::M_Pos_Joint_Labels)); + // Write the joint angles into csv + grl::writeMatrixToCSV(M_Joint_CSV, kuka_labels, timeEventM_Kuka, measuredJointPosition); + std::copy(std::begin(grl::C_Pos_Joint_Labels), std::end(grl::C_Pos_Joint_Labels),std::begin(kuka_labels)+grl::Time_Labels.size()); + grl::writeMatrixToCSV(C_Joint_CSV, kuka_labels, timeEventM_Kuka, commandedJointPosition); + + auto strRobot = grl::getURDFModel(urdfFile); + rbd::MultiBody mb = strRobot.mb; + rbd::MultiBodyConfig mbc(mb); + rbd::MultiBodyGraph mbg(strRobot.mbg); + std::size_t nrJoints = mb.nrJoints(); + std::size_t nrBodies = strRobot.mb.nrBodies(); + std::vector jointNames; + std::cout<<"Joint Size: "<< nrJoints << std::endl; + std::vector PK_Pose_Labels = grl::getLabels(grl::LabelsType::Kuka_Pose); + + + // forward kinematic to get the of the end effector + std::vector PoseEE = grl::getPoseEE(measuredJointPosition); + // convert the sva::PTransformd to Plucker coordinate. + Eigen::MatrixXd PKPose = grl::getPluckerPose(PoseEE); + // Write the plucker coordinate into csv. + grl::writeMatrixToCSV(KUKA_Pose_CSV, PK_Pose_Labels, timeEventM_Kuka, PKPose); + // Invert the EE pose + std::vector inversePose = grl::invertPose(PoseEE); + // convert the sva::PTransformd to Plucker coordinate. + PKPose = Eigen::MatrixXd::Zero(PKPose.rows(), PKPose.cols()); + PKPose = grl::getPluckerPose(inversePose); + // Write the plucker coordinate into csv. + grl::writeMatrixToCSV(KUKA_Inverse_Pose_CSV, PK_Pose_Labels, timeEventM_Kuka, PKPose); + + ///////////////////////////////////////////////////////////////////////////////////////////////////// + /// Get and write Atracsys message to CSV + //////////////////////////////////////////////////////////////////////////////////////////////////// // Get the pose of the marker 22. // Since the bad data is skipped, the timeEventM_FT also needs to be filled out with the orinial time stamp. - - std::vector FT_Labels_Pose = grl::getLabels(grl::LabelsType::FT_Pose); grl::writeFTKUKAMessageToCSV(FTKUKA_TimeEvent_CSV, timeEventM_FT, timeEventM_Kuka, measuredJointPosition, markerPose_FT, FT_index, kuka_index); - if(validsize_22>2){ - grl::writeTimeEventToCSV(FT_TimeEvent_CSV, timeEventM_FT, FT_index); - grl::writeMatrixToCSV(FT_Marker22_CSV, FT_Labels_Pose, timeEventM_FT, markerPose_FT, FT_index); - } + grl::writeTimeEventToCSV(FT_TimeEvent_CSV, timeEventM_FT, FT_index); + grl::writeMatrixToCSV(FT_Marker22_CSV, FT_Labels_Pose, timeEventM_FT, markerPose_FT, FT_index); // Change it back to the original size timeEventM_FT.resize(FT_size, grl::col_timeEvent); markerPose_FT.resize(FT_size, grl::col_Pose); @@ -218,21 +215,18 @@ int main(int argc, char* argv[]) int validsize_55 = grl::getMarkerPose(logKUKAiiwaFusionTrackP, markerID_55, timeEventM_FT, markerPose_FT); std::cout<<"validsize_55: " << validsize_55 << std::endl; - if(validsize_55>2) { - grl::regularizeTimeEvent(timeEventM_FT, initial_local_time, initial_device_time_FT, FT_index); - grl::writeMatrixToCSV(FT_Marker55_CSV, FT_Labels_Pose, timeEventM_FT, markerPose_FT, FT_index); - } - // Change it back to the original size + grl::regularizeTimeEvent(timeEventM_FT, initial_local_time, initial_device_time_FT, FT_index); + grl::writeMatrixToCSV(FT_Marker55_CSV, FT_Labels_Pose, timeEventM_FT, markerPose_FT, FT_index); + + // Change it back to the original size timeEventM_FT.resize(FT_size, grl::col_timeEvent); markerPose_FT.resize(FT_size, grl::col_Pose); // Get the pose of the bone marker int validsize_50000 = grl::getMarkerPose(logKUKAiiwaFusionTrackP, markerID_50000, timeEventM_FT, markerPose_FT); std::cout<<"validsize_50000: " << validsize_50000 << std::endl; - if(validsize_50000>2) { - grl::regularizeTimeEvent(timeEventM_FT, initial_local_time, initial_device_time_FT, FT_index); - grl::writeMatrixToCSV(FT_Marker50000_CSV, FT_Labels_Pose, timeEventM_FT, markerPose_FT, FT_index); - } + grl::regularizeTimeEvent(timeEventM_FT, initial_local_time, initial_device_time_FT, FT_index); + grl::writeMatrixToCSV(FT_Marker50000_CSV, FT_Labels_Pose, timeEventM_FT, markerPose_FT, FT_index); return 0; } From 9303a2aace8e5cfb12b09c65443c5ff21414e08b Mon Sep 17 00:00:00 2001 From: Chunting Date: Tue, 10 Apr 2018 17:39:05 -0400 Subject: [PATCH 088/102] Also improved the code based on the reviews. --- config/FindFlatBuffers.cmake | 10 +- config/FindFlatBuffers.cmake.deleteme | 160 ------------------ include/grl/flatbuffer/CSVIterator.hpp | 3 +- .../grl/flatbuffer/ParseflatbuffertoCSV.hpp | 31 ++-- include/grl/flatbuffer/flatbuffer.hpp | 2 - 5 files changed, 24 insertions(+), 182 deletions(-) delete mode 100644 config/FindFlatBuffers.cmake.deleteme diff --git a/config/FindFlatBuffers.cmake b/config/FindFlatBuffers.cmake index 1b884aa..f405e46 100644 --- a/config/FindFlatBuffers.cmake +++ b/config/FindFlatBuffers.cmake @@ -97,6 +97,7 @@ FLATBUFFERS_COMPILER if(FLATBUFFERS_FOUND) function(FLATBUFFERS_GENERATE_C_HEADERS Name FLATBUFFERS_DIR OUTPUT_DIR) + # Name is the name of the user defined variable that will be created by this function # Another variable that will be set is ${NAME}_OUTPUTS which will be set to the names # of all output files that have been generated. @@ -114,15 +115,20 @@ if(FLATBUFFERS_FOUND) list(APPEND FLATC_OUTPUTS ${FLATC_OUTPUT} ${FBS_FILE_COPY_INCLUDE} ${FBS_FILE_COPY_BIN}) # this is the absolute path to the actual filename.fbs file - set(ABSOLUTE_FBS_FILE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/${FLATBUFFERS_DIR}/${FILE}) + set(ABSOLUTE_FBS_FILE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/${FLATBUFFERS_DIR}${FILE}) + add_custom_command(OUTPUT ${FLATC_OUTPUT} + COMMAND ${FLATBUFFERS_COMPILER} # Note: We are setting several custom parameters here to make life easier. # see flatbuffers documentation for details. # flatc --gen-name-strings --scoped-enums --gen-object-api -c -j -p -o # see https://google.github.io/flatbuffers/flatbuffers_guide_using_schema_compiler.html - ARGS --gen-name-strings --scoped-enums --gen-object-api -c -j -p -o "${OUTPUT_DIR}" ${ABSOLUTE_FBS_FILE_PATH} + + + #ARGS --gen-name-strings --scoped-enums --gen-object-api -c -j -p -o "${OUTPUT_DIR}" ${ABSOLUTE_FBS_FILE_PATH} + ARGS --gen-name-strings --scoped-enums --gen-object-api -c -j -p -o "./" ${ABSOLUTE_FBS_FILE_PATH} MAIN_DEPENDENCY ${ABSOLUTE_FBS_FILE_PATH} COMMENT "Building C++, Java, and Python header for ${FILE}" WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/${FLATBUFFERS_DIR}) diff --git a/config/FindFlatBuffers.cmake.deleteme b/config/FindFlatBuffers.cmake.deleteme deleted file mode 100644 index 6f3a326..0000000 --- a/config/FindFlatBuffers.cmake.deleteme +++ /dev/null @@ -1,160 +0,0 @@ -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -# -# Tries to find Flatbuffers headers and libraries. -# -# Usage of this module as follows: -# -# find_package(Flatbuffers) -# -# Variables used by this module, they can change the default behaviour and need -# to be set before calling find_package: -# -# Flatbuffers_HOME - -# When set, this path is inspected instead of standard library locations as -# the root of the Flatbuffers installation. The environment variable -# FLATBUFFERS_HOME overrides this veriable. -# -# This module defines -# FLATBUFFERS_INCLUDE_DIR, directory containing headers -# FLATBUFFERS_LIBS, directory containing flatbuffers libraries -# FLATBUFFERS_STATIC_LIB, path to libflatbuffers.a -# FLATBUFFERS_FOUND, whether flatbuffers has been found - -if( NOT "$ENV{FLATBUFFERS_HOME}" STREQUAL "") -file( TO_CMAKE_PATH "$ENV{FLATBUFFERS_HOME}" _native_path ) -list( APPEND _flatbuffers_roots ${_native_path} ) -elseif ( Flatbuffers_HOME ) -list( APPEND _flatbuffers_roots ${Flatbuffers_HOME} ) -endif() - -# Try the parameterized roots, if they exist -if ( _flatbuffers_roots ) -find_path( FLATBUFFERS_INCLUDE_DIR NAMES flatbuffers/flatbuffers.h - PATHS ${_flatbuffers_roots} NO_DEFAULT_PATH - PATH_SUFFIXES "include" ) -find_library( FLATBUFFERS_LIBRARIES NAMES flatbuffers - PATHS ${_flatbuffers_roots} NO_DEFAULT_PATH - PATH_SUFFIXES "lib" ) -else () -find_path( FLATBUFFERS_INCLUDE_DIR NAMES flatbuffers/flatbuffers.h ) -find_library( FLATBUFFERS_LIBRARIES NAMES flatbufferFLATBUFFERS_FLATC_EXECUTABLEs ) -endif () - -find_program(FLATBUFFERS_COMPILER flatc -PATH -$ENV{FLATBUFFERS_HOME}/bin -${_flatbuffers_roots}/bin -/usr/local/bin -/usr/bin -NO_DEFAULT_PATH -) - -if (FLATBUFFERS_INCLUDE_DIR AND FLATBUFFERS_LIBRARIES) -set(FLATBUFFERS_FOUND TRUE) -get_filename_component( FLATBUFFERS_LIBS ${FLATBUFFERS_LIBRARIES} PATH ) -set(FLATBUFFERS_LIB_NAME libflatbuffers) -set(FLATBUFFERS_STATIC_LIB ${FLATBUFFERS_LIBS}/${FLATBUFFERS_LIB_NAME}.a) -else () -set(FLATBUFFERS_FOUND FALSE) -endif () - -if (FLATBUFFERS_FOUND) -if (NOT Flatbuffers_FIND_QUIETLY) -message(STATUS "Found the Flatbuffers library: ${FLATBUFFERS_LIBRARIES}") -endif ()FLATBUFFERS_COMPILER -else () -if (NOT Flatbuffers_FIND_QUIETLY) -set(FLATBUFFERS_ERR_MSG "Could not find the Flatbuffers library. Looked in ") -if ( _flatbuffers_roots ) - set(FLATBUFFERS_ERR_MSG "${FLATBUFFERS_ERR_MSG} in ${_flatbuffers_roots}.") -else () - set(FLATBUFFERS_ERR_MSG "${FLATBUFFERS_ERR_MSG} system search paths.") -endif () -if (Flatbuffers_FIND_REQUIRED) - message(FATAL_ERROR "${FLATBUFFERS_ERR_MSG}") -else (Flatbuffers_FIND_REQUIRED) - message(STATUS "${FLATBUFFERS_ERR_MSG}") -endif (Flatbuffers_FIND_REQUIRED) -endif () -endif () - -mark_as_advanced( -FLATBUFFERS_INCLUDE_DIR -FLATBUFFERS_LIBS -FLATBUFFERS_STATIC_LIB -FLATBUFFERS_COMPILER -) - -if(FLATBUFFERS_FOUND) - function(FLATBUFFERS_GENERATE_C_HEADERS Name FLATBUFFERS_DIR OUTPUT_DIR) - # Name is the name of the user defined variable that will be created by this function - # Another variable that will be set is ${NAME}_OUTPUTS which will be set to the names - # of all output files that have been generated.FLATBUFFERS_COMPILER - # FLATBUFFERS_DIR is the directory in which to look for the .fbs files - # OUTPUT_DIR is the directory in which all output files should be placed - set(FLATC_OUTPUTS) - foreach(FILE ${ARGN}) - get_filename_component(FLATC_OUTPUT ${FILE} NAME_WE) - # create a target for the specific flatbuffers file - set(FBS_FILE_COPY_INCLUDE copy_${FLATC_OUTPUT}_to_include) - set(FBS_FILE_COPY_BIN copy_${FLATC_OUTPUT}_to_bin) - # create a target for the generated output cpp file - set(FLATC_OUTPUT - "${OUTPUT_DIR}/${FLATC_OUTPUT}_generated.h") - list(APPEND FLATC_OUTPUTS ${FLATC_OUTPUT} ${FBS_FILE_COPY_INCLUDE} ${FBS_FILE_COPY_BIN}) - - # this is the absolute path to the actual filename.fbs file - set(ABSOLUTE_FBS_FILE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/${FLATBUFFERS_DIR}/${FILE}) - - add_custom_command(OUTPUT ${FLATC_OUTPUT} - COMMAND ${FLATBUFFERS_COMPILER} - # Note: We are setting several custom parameters here to make life easier. - # see flatbuffers documentation for details. - # flatc --gen-name-strings --scoped-enums --gen-object-api -c -j -p -o - # see https://google.github.io/flatbuffers/flatbuffers_guide_using_schema_compiler.html - ARGS --gen-name-strings --scoped-enums --gen-object-api -c -j -p -o "${OUTPUT_DIR}" ${FILE} - MAIN_DEPENDENCY ${ABSOLUTE_FBS_FILE_PATH} - COMMENT "Building C++, Java, and Python header for ${FILE}" - WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/${FLATBUFFERS_DIR}) - - # need to copy the flatbuffers schema so config files can be loaded - # http://stackoverflow.com/a/13429998/99379 - # CMAKE_CURRENT_SOURCE_DIR - # this is the directory where the currently processed CMakeLists.txt is located in - # terminal copy commands change between OS versions, so we use CMake's built in file - # copy command which runs with "cmake -E copy file_to_copy file_destination" - # we use some variables here so the path is reproducible. - add_custom_command(OUTPUT ${FBS_FILE_COPY_INCLUDE} - COMMAND ${CMAKE_COMMAND} ARGS -E copy ${ABSOLUTE_FBS_FILE_PATH} ${OUTPUT_DIR} - MAIN_DEPENDENCY ${CMAKE_CURRENT_SOURCE_DIR}/${FLATBUFFERS_DIR}/${FILE} - COMMENT "Copying fbs file ${FILE} to ${OUTPUT_DIR}" - WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/${FLATBUFFERS_DIR} - ) - - add_custom_command(OUTPUT ${FBS_FILE_COPY_BIN} - # TODO(ahundt) remove hacky /bin manually set path, this will break for some IDEs - COMMAND ${CMAKE_COMMAND} ARGS -E copy ${ABSOLUTE_FBS_FILE_PATH} ${CMAKE_BINARY_DIR}/bin - MAIN_DEPENDENCY ${CMAKE_CURRENT_SOURCE_DIR}/${FLATBUFFERS_DIR}/${FILE} - COMMENT "Copying fbs file ${FILE} to ${CMAKE_BINARY_DIR}" - WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/${FLATBUFFERS_DIR} - ) - endforeach() - set(${Name}_OUTPUTS ${FLATC_OUTPUTS} PARENT_SCOPE) - endfunction() - - set(FLATBUFFERS_INCLUDE_DIRS ${FLATBUFFERS_INCLUDE_DIR}) - include_directories(${CMAKE_BINARY_DIR}) -else() - set(FLATBUFFERS_INCLUDE_DIR) -endif() diff --git a/include/grl/flatbuffer/CSVIterator.hpp b/include/grl/flatbuffer/CSVIterator.hpp index e421b2f..089ab94 100644 --- a/include/grl/flatbuffer/CSVIterator.hpp +++ b/include/grl/flatbuffer/CSVIterator.hpp @@ -1,6 +1,7 @@ #ifndef GRL_CSV_ITERATOR #define GRL_CSV_ITERATOR - +/// Read and parse CSV files in C++ +/// See https://stackoverflow.com/questions/1120140/how-can-i-read-and-parse-csv-files-in-c #include #include #include diff --git a/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp b/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp index 4859035..3bd01c6 100644 --- a/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp +++ b/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp @@ -109,10 +109,6 @@ namespace grl { const static int jointNum = 7; int col_Pose = PK_Pose_Labels.size(); - - - - /// Get CSV labels /// @param label indicate the type of the label. @@ -172,7 +168,8 @@ namespace grl { /// Get the maker pose based on the markerID. The bad data, which means the frame doesn't have the indicated marker information, has been filtered out. - /// As for the bad data, both the marker pose and the corresponding timeEvent is skipped. + /// Bad data filtering is provided by this functions(both the marker pose and the corresponding timeEvent is skipped). + /// In the method we get the orientation in Euler-Angles /// @param logKUKAiiwaFusionTrackP, pointer of the root object for fusiontracker. /// @param markerID, the indicated marker. /// @param timeEventM, timeEvent without bad data, which is filled out. @@ -188,10 +185,8 @@ namespace grl { assert(state_size>0); // The first columne is counter int row = 0; - // Eigen::MatrixXd markerPose(state_size, grl::col_Pose); - // std::cout <<"State size: " << state_size << " markerPose rows: " << markerPose.rows() << std::endl; int BadCount = 0; - + // Loop through the marker states in flatbuffer binary file and then reach all the parameters we need. for(int i = 0; iGet(i); auto FT_Message = kukaiiwaFusionTrackMessage->deviceState_as_FusionTrackMessage(); @@ -226,17 +221,22 @@ namespace grl { markerPose.row(row++) = pose; - // Once read the specified marker information, skip out of the marker loop. + // Once read the specific marker information, skip out of the marker loop. // It can keep from reading duplicate information. // Sometimes in the same frame, there exists two markers with the same geometryID break; } } } else { + // Count the number of bad data. BadCount++; } } + // Resize the matrix. The size of the matrix is initialized by the number of the states, + // because of the bad data, the final size of the matrix should be smaller than the original one. + // if(row < state_size) { + // For the time_Y axis, all the following value minus the first one, make it start from 0 int64_t FT_diff = timeEventM(0,TimeType::device_time) - timeEventM(0,timeBaseline_index); timeEventM.col(TimeType::time_Y) = timeEventM.col(TimeType::time_Y) - FT_diff * grl::VectorXd::Ones(timeEventM.rows()); markerPose.conservativeResize(row, Eigen::NoChange_t{}); @@ -247,8 +247,9 @@ namespace grl { << " lossing rate " << diff/state_size << " markerID: " << markerID <states(); std::size_t state_size = states->size(); Eigen::VectorXf measurdjointPosVec(state_size); @@ -361,7 +361,6 @@ namespace grl { return 1; } - /// Write FRI message into a csv file, all the data are read from flatbuffer binary file. /// @param KUKA_FRI_CSVfilename, the csv file name void writeFRIMessageToCSV(std::string& KUKA_FRI_CSVfilename, @@ -375,9 +374,7 @@ namespace grl { Eigen::MatrixXd& externalTorque, Eigen::MatrixXd& jointStateInterpolated, int startIndex = 0) { - int kuka_time_size = timeEventM_Kuka.rows(); - assert(kuka_time_size == sequenceCounterVec.size()); assert(kuka_time_size == reflectedSequenceCounterVec.size()); assert(kuka_time_size == measuredJointPosition.rows()); @@ -770,7 +767,7 @@ namespace grl { return std::move(EEpose); } - mc_rbdyn_urdf::URDFParserResult getURDFModel(std::string filename = "~/src/robonetracker/modules/grl/include/grl/flatbuffer/Robone_KukaLBRiiwa.urdf"){ + mc_rbdyn_urdf::URDFParserResult getURDFModel(std::string filename){ if(!boost::filesystem::exists(filename)){ std::cerr << filename << " doesn't exist..." << std::endl; } diff --git a/include/grl/flatbuffer/flatbuffer.hpp b/include/grl/flatbuffer/flatbuffer.hpp index 7ce2370..b0180f2 100644 --- a/include/grl/flatbuffer/flatbuffer.hpp +++ b/include/grl/flatbuffer/flatbuffer.hpp @@ -9,8 +9,6 @@ #include #include - - namespace grl { // loads a json flatbuffer from a file From 688dc3595af39052704660c4616543fec99cffc0 Mon Sep 17 00:00:00 2001 From: Chunting Date: Tue, 10 Apr 2018 17:42:00 -0400 Subject: [PATCH 089/102] DON'T NEED TO REVIEW THESE FILES It seems strange to me that in the folder src/robonetracker/data/, there are tons of new files coming up. Don't need to review them. --- .../flatbuffer/ArmControlState_generated.h | 1447 +++++++++ include/grl/flatbuffer/Euler_generated.h | 529 +++ .../grl/flatbuffer/FusionTrack_generated.h | 1803 ++++++++++ include/grl/flatbuffer/Geometry_generated.h | 200 ++ include/grl/flatbuffer/JointState_generated.h | 163 + include/grl/flatbuffer/KUKAiiwa_generated.h | 2885 +++++++++++++++++ include/grl/flatbuffer/LinkObject_generated.h | 163 + .../LogKUKAiiwaFusionTrack_generated.h | 504 +++ include/grl/flatbuffer/Time_generated.h | 291 ++ .../flatbuffer/VrepControlPoint_generated.h | 306 ++ include/grl/flatbuffer/VrepPath_generated.h | 153 + include/grl/flatbuffer/grl/__init__.py | 0 .../grl/flatbuffer/ArmControlSeries.java | 40 + .../grl/flatbuffer/ArmControlSeries.py | 44 + .../grl/flatbuffer/ArmControlState.java | 51 + .../grl/flatbuffer/ArmControlState.py | 65 + .../flatbuffer/grl/flatbuffer/ArmState.java | 21 + .../grl/flatbuffer/grl/flatbuffer/ArmState.py | 15 + .../CartesianImpedenceControlMode.java | 69 + .../CartesianImpedenceControlMode.py | 114 + .../grl/flatbuffer/DeviceState.java | 15 + .../flatbuffer/grl/flatbuffer/DeviceState.py | 9 + .../flatbuffer/grl/flatbuffer/Disabled.java | 24 + .../grl/flatbuffer/grl/flatbuffer/Disabled.py | 22 + .../grl/flatbuffer/EClientCommandMode.java | 19 + .../grl/flatbuffer/EClientCommandMode.py | 11 + .../grl/flatbuffer/EConnectionQuality.java | 16 + .../grl/flatbuffer/EConnectionQuality.py | 10 + .../grl/flatbuffer/EControlMode.java | 16 + .../flatbuffer/grl/flatbuffer/EControlMode.py | 10 + .../grl/flatbuffer/EDriveState.java | 24 + .../flatbuffer/grl/flatbuffer/EDriveState.py | 12 + .../grl/flatbuffer/EOperationMode.java | 15 + .../grl/flatbuffer/EOperationMode.py | 9 + .../grl/flatbuffer/EOverlayType.java | 15 + .../flatbuffer/grl/flatbuffer/EOverlayType.py | 9 + .../grl/flatbuffer/ESafetyState.java | 16 + .../flatbuffer/grl/flatbuffer/ESafetyState.py | 10 + .../grl/flatbuffer/ESessionState.java | 32 + .../grl/flatbuffer/ESessionState.py | 16 + .../flatbuffer/grl/flatbuffer/EulerOrder.java | 23 + .../flatbuffer/grl/flatbuffer/EulerOrder.py | 17 + .../flatbuffer/grl/flatbuffer/EulerPose.java | 35 + .../flatbuffer/grl/flatbuffer/EulerPose.py | 37 + .../grl/flatbuffer/EulerPoseParams.java | 30 + .../grl/flatbuffer/EulerPoseParams.py | 46 + .../grl/flatbuffer/EulerRotation.java | 30 + .../grl/flatbuffer/EulerRotation.py | 30 + .../grl/flatbuffer/EulerRotationParams.java | 45 + .../grl/flatbuffer/EulerRotationParams.py | 54 + .../flatbuffer/EulerTranslationParams.java | 41 + .../grl/flatbuffer/EulerTranslationParams.py | 46 + .../flatbuffer/grl/flatbuffer/EulerXYZd.java | 27 + .../flatbuffer/grl/flatbuffer/EulerXYZd.py | 26 + .../grl/flatbuffer/grl/flatbuffer/FRI.java | 97 + include/grl/flatbuffer/grl/flatbuffer/FRI.py | 110 + .../grl/flatbuffer/FRIMessageLog.java | 112 + .../grl/flatbuffer/FRIMessageLog.py | 226 ++ .../grl/flatbuffer/FRITimeStamp.java | 37 + .../flatbuffer/grl/flatbuffer/FRITimeStamp.py | 38 + .../grl/flatbuffer/FusionTrackFrame.java | 179 + .../grl/flatbuffer/FusionTrackFrame.py | 330 ++ .../grl/flatbuffer/FusionTrackMessage.java | 48 + .../grl/flatbuffer/FusionTrackMessage.py | 66 + .../grl/flatbuffer/FusionTrackParameters.java | 145 + .../grl/flatbuffer/FusionTrackParameters.py | 222 ++ .../flatbuffer/grl/flatbuffer/Inertia.java | 47 + .../grl/flatbuffer/grl/flatbuffer/Inertia.py | 53 + .../flatbuffer/JointImpedenceControlMode.java | 47 + .../flatbuffer/JointImpedenceControlMode.py | 70 + .../flatbuffer/grl/flatbuffer/JointState.java | 65 + .../flatbuffer/grl/flatbuffer/JointState.py | 118 + .../flatbuffer/KUKAiiwaArmConfiguration.java | 119 + .../flatbuffer/KUKAiiwaArmConfiguration.py | 184 ++ .../KUKAiiwaFusionTrackMessage.java | 46 + .../flatbuffer/KUKAiiwaFusionTrackMessage.py | 61 + .../grl/flatbuffer/KUKAiiwaInterface.java | 16 + .../grl/flatbuffer/KUKAiiwaInterface.py | 10 + .../KUKAiiwaMonitorConfiguration.java | 63 + .../KUKAiiwaMonitorConfiguration.py | 93 + .../grl/flatbuffer/KUKAiiwaMonitorState.java | 57 + .../grl/flatbuffer/KUKAiiwaMonitorState.py | 117 + .../grl/flatbuffer/KUKAiiwaState.java | 93 + .../grl/flatbuffer/KUKAiiwaState.py | 150 + .../grl/flatbuffer/KUKAiiwaStates.java | 40 + .../grl/flatbuffer/KUKAiiwaStates.py | 44 + .../flatbuffer/grl/flatbuffer/LinkObject.java | 38 + .../flatbuffer/grl/flatbuffer/LinkObject.py | 62 + .../flatbuffer/LogKUKAiiwaFusionTrack.java | 40 + .../grl/flatbuffer/LogKUKAiiwaFusionTrack.py | 44 + .../grl/flatbuffer/MoveArmCartesianServo.java | 31 + .../grl/flatbuffer/MoveArmCartesianServo.py | 42 + .../grl/flatbuffer/MoveArmJointServo.java | 34 + .../grl/flatbuffer/MoveArmJointServo.py | 34 + .../grl/flatbuffer/MoveArmTrajectory.java | 37 + .../grl/flatbuffer/MoveArmTrajectory.py | 44 + .../flatbuffer/grl/flatbuffer/PauseArm.java | 24 + .../grl/flatbuffer/grl/flatbuffer/PauseArm.py | 22 + .../grl/flatbuffer/grl/flatbuffer/Pose.java | 34 + include/grl/flatbuffer/grl/flatbuffer/Pose.py | 36 + .../grl/flatbuffer/ProcessData.java | 97 + .../flatbuffer/grl/flatbuffer/ProcessData.py | 108 + .../flatbuffer/grl/flatbuffer/Quaternion.java | 29 + .../flatbuffer/grl/flatbuffer/Quaternion.py | 29 + .../grl/flatbuffer/ShutdownArm.java | 24 + .../flatbuffer/grl/flatbuffer/ShutdownArm.py | 22 + .../flatbuffer/grl/flatbuffer/SmartServo.java | 61 + .../flatbuffer/grl/flatbuffer/SmartServo.py | 88 + .../flatbuffer/grl/flatbuffer/StartArm.java | 24 + .../grl/flatbuffer/grl/flatbuffer/StartArm.py | 22 + .../flatbuffer/grl/flatbuffer/StopArm.java | 24 + .../grl/flatbuffer/grl/flatbuffer/StopArm.py | 22 + .../flatbuffer/grl/flatbuffer/TeachArm.java | 24 + .../grl/flatbuffer/grl/flatbuffer/TeachArm.py | 22 + .../grl/flatbuffer/grl/flatbuffer/Time.java | 23 + include/grl/flatbuffer/grl/flatbuffer/Time.py | 20 + .../flatbuffer/grl/flatbuffer/TimeEvent.java | 112 + .../flatbuffer/grl/flatbuffer/TimeEvent.py | 115 + .../flatbuffer/grl/flatbuffer/Vector3d.java | 27 + .../grl/flatbuffer/grl/flatbuffer/Vector3d.py | 26 + .../grl/flatbuffer/VrepControlPoint.java | 52 + .../grl/flatbuffer/VrepControlPoint.py | 126 + .../flatbuffer/grl/flatbuffer/VrepPath.java | 39 + .../grl/flatbuffer/grl/flatbuffer/VrepPath.py | 44 + .../grl/flatbuffer/grl/flatbuffer/Wrench.java | 39 + .../grl/flatbuffer/grl/flatbuffer/Wrench.py | 44 + .../grl/flatbuffer/grl/flatbuffer/__init__.py | 0 .../grl/flatbuffer/ftk3DFiducial.java | 39 + .../grl/flatbuffer/ftk3DFiducial.py | 82 + .../grl/flatbuffer/ftkDeviceType.java | 13 + .../grl/flatbuffer/ftkDeviceType.py | 11 + .../grl/flatbuffer/ftkGeometry.java | 50 + .../flatbuffer/grl/flatbuffer/ftkGeometry.py | 67 + .../flatbuffer/grl/flatbuffer/ftkMarker.java | 42 + .../flatbuffer/grl/flatbuffer/ftkMarker.py | 82 + .../grl/flatbuffer/ftkQueryStatus.java | 17 + .../grl/flatbuffer/ftkQueryStatus.py | 11 + .../grl/flatbuffer/ftkRegionOfInterest.java | 61 + .../grl/flatbuffer/ftkRegionOfInterest.py | 86 + 139 files changed, 15109 insertions(+) create mode 100644 include/grl/flatbuffer/ArmControlState_generated.h create mode 100644 include/grl/flatbuffer/Euler_generated.h create mode 100644 include/grl/flatbuffer/FusionTrack_generated.h create mode 100644 include/grl/flatbuffer/Geometry_generated.h create mode 100644 include/grl/flatbuffer/JointState_generated.h create mode 100644 include/grl/flatbuffer/KUKAiiwa_generated.h create mode 100644 include/grl/flatbuffer/LinkObject_generated.h create mode 100644 include/grl/flatbuffer/LogKUKAiiwaFusionTrack_generated.h create mode 100644 include/grl/flatbuffer/Time_generated.h create mode 100644 include/grl/flatbuffer/VrepControlPoint_generated.h create mode 100644 include/grl/flatbuffer/VrepPath_generated.h create mode 100644 include/grl/flatbuffer/grl/__init__.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/ArmControlSeries.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/ArmControlSeries.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/ArmControlState.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/ArmControlState.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/ArmState.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/ArmState.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/CartesianImpedenceControlMode.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/CartesianImpedenceControlMode.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/DeviceState.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/DeviceState.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/Disabled.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/Disabled.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/EClientCommandMode.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/EClientCommandMode.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/EConnectionQuality.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/EConnectionQuality.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/EControlMode.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/EControlMode.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/EDriveState.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/EDriveState.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/EOperationMode.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/EOperationMode.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/EOverlayType.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/EOverlayType.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/ESafetyState.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/ESafetyState.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/ESessionState.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/ESessionState.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/EulerOrder.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/EulerOrder.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/EulerPose.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/EulerPose.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/EulerPoseParams.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/EulerPoseParams.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/EulerRotation.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/EulerRotation.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/EulerRotationParams.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/EulerRotationParams.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/EulerTranslationParams.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/EulerTranslationParams.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/EulerXYZd.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/EulerXYZd.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/FRI.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/FRI.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/FRIMessageLog.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/FRIMessageLog.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/FRITimeStamp.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/FRITimeStamp.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/FusionTrackFrame.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/FusionTrackFrame.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/FusionTrackMessage.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/FusionTrackMessage.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/FusionTrackParameters.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/FusionTrackParameters.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/Inertia.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/Inertia.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/JointImpedenceControlMode.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/JointImpedenceControlMode.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/JointState.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/JointState.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaArmConfiguration.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaArmConfiguration.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaFusionTrackMessage.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaFusionTrackMessage.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaInterface.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaInterface.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaMonitorConfiguration.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaMonitorConfiguration.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaMonitorState.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaMonitorState.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaState.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaState.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaStates.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaStates.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/LinkObject.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/LinkObject.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/LogKUKAiiwaFusionTrack.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/LogKUKAiiwaFusionTrack.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/MoveArmCartesianServo.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/MoveArmCartesianServo.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/MoveArmJointServo.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/MoveArmJointServo.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/MoveArmTrajectory.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/MoveArmTrajectory.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/PauseArm.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/PauseArm.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/Pose.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/Pose.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/ProcessData.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/ProcessData.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/Quaternion.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/Quaternion.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/ShutdownArm.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/ShutdownArm.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/SmartServo.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/SmartServo.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/StartArm.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/StartArm.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/StopArm.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/StopArm.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/TeachArm.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/TeachArm.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/Time.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/Time.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/TimeEvent.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/TimeEvent.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/Vector3d.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/Vector3d.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/VrepControlPoint.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/VrepControlPoint.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/VrepPath.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/VrepPath.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/Wrench.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/Wrench.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/__init__.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/ftk3DFiducial.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/ftk3DFiducial.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/ftkDeviceType.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/ftkDeviceType.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/ftkGeometry.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/ftkGeometry.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/ftkMarker.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/ftkMarker.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/ftkQueryStatus.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/ftkQueryStatus.py create mode 100644 include/grl/flatbuffer/grl/flatbuffer/ftkRegionOfInterest.java create mode 100644 include/grl/flatbuffer/grl/flatbuffer/ftkRegionOfInterest.py diff --git a/include/grl/flatbuffer/ArmControlState_generated.h b/include/grl/flatbuffer/ArmControlState_generated.h new file mode 100644 index 0000000..18f323c --- /dev/null +++ b/include/grl/flatbuffer/ArmControlState_generated.h @@ -0,0 +1,1447 @@ +// automatically generated by the FlatBuffers compiler, do not modify + + +#ifndef FLATBUFFERS_GENERATED_ARMCONTROLSTATE_GRL_FLATBUFFER_H_ +#define FLATBUFFERS_GENERATED_ARMCONTROLSTATE_GRL_FLATBUFFER_H_ + +#include "flatbuffers/flatbuffers.h" + +#include "Geometry_generated.h" +#include "JointState_generated.h" + +namespace grl { +namespace flatbuffer { + +struct StartArm; +struct StartArmT; + +struct StopArm; +struct StopArmT; + +struct PauseArm; +struct PauseArmT; + +struct TeachArm; +struct TeachArmT; + +struct ShutdownArm; +struct ShutdownArmT; + +struct MoveArmTrajectory; +struct MoveArmTrajectoryT; + +struct MoveArmJointServo; +struct MoveArmJointServoT; + +struct MoveArmCartesianServo; +struct MoveArmCartesianServoT; + +struct ArmControlState; +struct ArmControlStateT; + +struct ArmControlSeries; +struct ArmControlSeriesT; + +enum class ArmState : uint8_t { + NONE = 0, + StartArm = 1, + StopArm = 2, + PauseArm = 3, + ShutdownArm = 4, + TeachArm = 5, + MoveArmTrajectory = 6, + MoveArmJointServo = 7, + MoveArmCartesianServo = 8, + MIN = NONE, + MAX = MoveArmCartesianServo +}; + +inline const ArmState (&EnumValuesArmState())[9] { + static const ArmState values[] = { + ArmState::NONE, + ArmState::StartArm, + ArmState::StopArm, + ArmState::PauseArm, + ArmState::ShutdownArm, + ArmState::TeachArm, + ArmState::MoveArmTrajectory, + ArmState::MoveArmJointServo, + ArmState::MoveArmCartesianServo + }; + return values; +} + +inline const char * const *EnumNamesArmState() { + static const char * const names[] = { + "NONE", + "StartArm", + "StopArm", + "PauseArm", + "ShutdownArm", + "TeachArm", + "MoveArmTrajectory", + "MoveArmJointServo", + "MoveArmCartesianServo", + nullptr + }; + return names; +} + +inline const char *EnumNameArmState(ArmState e) { + const size_t index = static_cast(e); + return EnumNamesArmState()[index]; +} + +template struct ArmStateTraits { + static const ArmState enum_value = ArmState::NONE; +}; + +template<> struct ArmStateTraits { + static const ArmState enum_value = ArmState::StartArm; +}; + +template<> struct ArmStateTraits { + static const ArmState enum_value = ArmState::StopArm; +}; + +template<> struct ArmStateTraits { + static const ArmState enum_value = ArmState::PauseArm; +}; + +template<> struct ArmStateTraits { + static const ArmState enum_value = ArmState::ShutdownArm; +}; + +template<> struct ArmStateTraits { + static const ArmState enum_value = ArmState::TeachArm; +}; + +template<> struct ArmStateTraits { + static const ArmState enum_value = ArmState::MoveArmTrajectory; +}; + +template<> struct ArmStateTraits { + static const ArmState enum_value = ArmState::MoveArmJointServo; +}; + +template<> struct ArmStateTraits { + static const ArmState enum_value = ArmState::MoveArmCartesianServo; +}; + +struct ArmStateUnion { + ArmState type; + void *value; + + ArmStateUnion() : type(ArmState::NONE), value(nullptr) {} + ArmStateUnion(ArmStateUnion&& u) FLATBUFFERS_NOEXCEPT : + type(ArmState::NONE), value(nullptr) + { std::swap(type, u.type); std::swap(value, u.value); } + ArmStateUnion(const ArmStateUnion &) FLATBUFFERS_NOEXCEPT; + ArmStateUnion &operator=(const ArmStateUnion &u) FLATBUFFERS_NOEXCEPT + { ArmStateUnion t(u); std::swap(type, t.type); std::swap(value, t.value); return *this; } + ArmStateUnion &operator=(ArmStateUnion &&u) FLATBUFFERS_NOEXCEPT + { std::swap(type, u.type); std::swap(value, u.value); return *this; } + ~ArmStateUnion() { Reset(); } + + void Reset(); + +#ifndef FLATBUFFERS_CPP98_STL + template + void Set(T&& val) { + Reset(); + type = ArmStateTraits::enum_value; + if (type != ArmState::NONE) { + value = new T(std::forward(val)); + } + } +#endif // FLATBUFFERS_CPP98_STL + + static void *UnPack(const void *obj, ArmState type, const flatbuffers::resolver_function_t *resolver); + flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const flatbuffers::rehasher_function_t *_rehasher = nullptr) const; + + StartArmT *AsStartArm() { + return type == ArmState::StartArm ? + reinterpret_cast(value) : nullptr; + } + const StartArmT *AsStartArm() const { + return type == ArmState::StartArm ? + reinterpret_cast(value) : nullptr; + } + StopArmT *AsStopArm() { + return type == ArmState::StopArm ? + reinterpret_cast(value) : nullptr; + } + const StopArmT *AsStopArm() const { + return type == ArmState::StopArm ? + reinterpret_cast(value) : nullptr; + } + PauseArmT *AsPauseArm() { + return type == ArmState::PauseArm ? + reinterpret_cast(value) : nullptr; + } + const PauseArmT *AsPauseArm() const { + return type == ArmState::PauseArm ? + reinterpret_cast(value) : nullptr; + } + ShutdownArmT *AsShutdownArm() { + return type == ArmState::ShutdownArm ? + reinterpret_cast(value) : nullptr; + } + const ShutdownArmT *AsShutdownArm() const { + return type == ArmState::ShutdownArm ? + reinterpret_cast(value) : nullptr; + } + TeachArmT *AsTeachArm() { + return type == ArmState::TeachArm ? + reinterpret_cast(value) : nullptr; + } + const TeachArmT *AsTeachArm() const { + return type == ArmState::TeachArm ? + reinterpret_cast(value) : nullptr; + } + MoveArmTrajectoryT *AsMoveArmTrajectory() { + return type == ArmState::MoveArmTrajectory ? + reinterpret_cast(value) : nullptr; + } + const MoveArmTrajectoryT *AsMoveArmTrajectory() const { + return type == ArmState::MoveArmTrajectory ? + reinterpret_cast(value) : nullptr; + } + MoveArmJointServoT *AsMoveArmJointServo() { + return type == ArmState::MoveArmJointServo ? + reinterpret_cast(value) : nullptr; + } + const MoveArmJointServoT *AsMoveArmJointServo() const { + return type == ArmState::MoveArmJointServo ? + reinterpret_cast(value) : nullptr; + } + MoveArmCartesianServoT *AsMoveArmCartesianServo() { + return type == ArmState::MoveArmCartesianServo ? + reinterpret_cast(value) : nullptr; + } + const MoveArmCartesianServoT *AsMoveArmCartesianServo() const { + return type == ArmState::MoveArmCartesianServo ? + reinterpret_cast(value) : nullptr; + } +}; + +bool VerifyArmState(flatbuffers::Verifier &verifier, const void *obj, ArmState type); +bool VerifyArmStateVector(flatbuffers::Verifier &verifier, const flatbuffers::Vector> *values, const flatbuffers::Vector *types); + +struct StartArmT : public flatbuffers::NativeTable { + typedef StartArm TableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.StartArmT"; + } + StartArmT() { + } +}; + +struct StartArm FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { + typedef StartArmT NativeTableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.StartArm"; + } + bool Verify(flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + verifier.EndTable(); + } + StartArmT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; + void UnPackTo(StartArmT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; + static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const StartArmT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); +}; + +struct StartArmBuilder { + flatbuffers::FlatBufferBuilder &fbb_; + flatbuffers::uoffset_t start_; + explicit StartArmBuilder(flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + StartArmBuilder &operator=(const StartArmBuilder &); + flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = flatbuffers::Offset(end); + return o; + } +}; + +inline flatbuffers::Offset CreateStartArm( + flatbuffers::FlatBufferBuilder &_fbb) { + StartArmBuilder builder_(_fbb); + return builder_.Finish(); +} + +flatbuffers::Offset CreateStartArm(flatbuffers::FlatBufferBuilder &_fbb, const StartArmT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); + +struct StopArmT : public flatbuffers::NativeTable { + typedef StopArm TableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.StopArmT"; + } + StopArmT() { + } +}; + +struct StopArm FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { + typedef StopArmT NativeTableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.StopArm"; + } + bool Verify(flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + verifier.EndTable(); + } + StopArmT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; + void UnPackTo(StopArmT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; + static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const StopArmT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); +}; + +struct StopArmBuilder { + flatbuffers::FlatBufferBuilder &fbb_; + flatbuffers::uoffset_t start_; + explicit StopArmBuilder(flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + StopArmBuilder &operator=(const StopArmBuilder &); + flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = flatbuffers::Offset(end); + return o; + } +}; + +inline flatbuffers::Offset CreateStopArm( + flatbuffers::FlatBufferBuilder &_fbb) { + StopArmBuilder builder_(_fbb); + return builder_.Finish(); +} + +flatbuffers::Offset CreateStopArm(flatbuffers::FlatBufferBuilder &_fbb, const StopArmT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); + +struct PauseArmT : public flatbuffers::NativeTable { + typedef PauseArm TableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.PauseArmT"; + } + PauseArmT() { + } +}; + +struct PauseArm FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { + typedef PauseArmT NativeTableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.PauseArm"; + } + bool Verify(flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + verifier.EndTable(); + } + PauseArmT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; + void UnPackTo(PauseArmT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; + static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const PauseArmT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); +}; + +struct PauseArmBuilder { + flatbuffers::FlatBufferBuilder &fbb_; + flatbuffers::uoffset_t start_; + explicit PauseArmBuilder(flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + PauseArmBuilder &operator=(const PauseArmBuilder &); + flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = flatbuffers::Offset(end); + return o; + } +}; + +inline flatbuffers::Offset CreatePauseArm( + flatbuffers::FlatBufferBuilder &_fbb) { + PauseArmBuilder builder_(_fbb); + return builder_.Finish(); +} + +flatbuffers::Offset CreatePauseArm(flatbuffers::FlatBufferBuilder &_fbb, const PauseArmT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); + +struct TeachArmT : public flatbuffers::NativeTable { + typedef TeachArm TableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.TeachArmT"; + } + TeachArmT() { + } +}; + +struct TeachArm FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { + typedef TeachArmT NativeTableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.TeachArm"; + } + bool Verify(flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + verifier.EndTable(); + } + TeachArmT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; + void UnPackTo(TeachArmT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; + static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const TeachArmT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); +}; + +struct TeachArmBuilder { + flatbuffers::FlatBufferBuilder &fbb_; + flatbuffers::uoffset_t start_; + explicit TeachArmBuilder(flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + TeachArmBuilder &operator=(const TeachArmBuilder &); + flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = flatbuffers::Offset(end); + return o; + } +}; + +inline flatbuffers::Offset CreateTeachArm( + flatbuffers::FlatBufferBuilder &_fbb) { + TeachArmBuilder builder_(_fbb); + return builder_.Finish(); +} + +flatbuffers::Offset CreateTeachArm(flatbuffers::FlatBufferBuilder &_fbb, const TeachArmT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); + +struct ShutdownArmT : public flatbuffers::NativeTable { + typedef ShutdownArm TableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.ShutdownArmT"; + } + ShutdownArmT() { + } +}; + +struct ShutdownArm FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { + typedef ShutdownArmT NativeTableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.ShutdownArm"; + } + bool Verify(flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + verifier.EndTable(); + } + ShutdownArmT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; + void UnPackTo(ShutdownArmT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; + static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const ShutdownArmT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); +}; + +struct ShutdownArmBuilder { + flatbuffers::FlatBufferBuilder &fbb_; + flatbuffers::uoffset_t start_; + explicit ShutdownArmBuilder(flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + ShutdownArmBuilder &operator=(const ShutdownArmBuilder &); + flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = flatbuffers::Offset(end); + return o; + } +}; + +inline flatbuffers::Offset CreateShutdownArm( + flatbuffers::FlatBufferBuilder &_fbb) { + ShutdownArmBuilder builder_(_fbb); + return builder_.Finish(); +} + +flatbuffers::Offset CreateShutdownArm(flatbuffers::FlatBufferBuilder &_fbb, const ShutdownArmT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); + +struct MoveArmTrajectoryT : public flatbuffers::NativeTable { + typedef MoveArmTrajectory TableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.MoveArmTrajectoryT"; + } + std::vector> traj; + MoveArmTrajectoryT() { + } +}; + +struct MoveArmTrajectory FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { + typedef MoveArmTrajectoryT NativeTableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.MoveArmTrajectory"; + } + enum { + VT_TRAJ = 4 + }; + const flatbuffers::Vector> *traj() const { + return GetPointer> *>(VT_TRAJ); + } + bool Verify(flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + VerifyOffset(verifier, VT_TRAJ) && + verifier.Verify(traj()) && + verifier.VerifyVectorOfTables(traj()) && + verifier.EndTable(); + } + MoveArmTrajectoryT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; + void UnPackTo(MoveArmTrajectoryT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; + static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const MoveArmTrajectoryT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); +}; + +struct MoveArmTrajectoryBuilder { + flatbuffers::FlatBufferBuilder &fbb_; + flatbuffers::uoffset_t start_; + void add_traj(flatbuffers::Offset>> traj) { + fbb_.AddOffset(MoveArmTrajectory::VT_TRAJ, traj); + } + explicit MoveArmTrajectoryBuilder(flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + MoveArmTrajectoryBuilder &operator=(const MoveArmTrajectoryBuilder &); + flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = flatbuffers::Offset(end); + return o; + } +}; + +inline flatbuffers::Offset CreateMoveArmTrajectory( + flatbuffers::FlatBufferBuilder &_fbb, + flatbuffers::Offset>> traj = 0) { + MoveArmTrajectoryBuilder builder_(_fbb); + builder_.add_traj(traj); + return builder_.Finish(); +} + +inline flatbuffers::Offset CreateMoveArmTrajectoryDirect( + flatbuffers::FlatBufferBuilder &_fbb, + const std::vector> *traj = nullptr) { + return grl::flatbuffer::CreateMoveArmTrajectory( + _fbb, + traj ? _fbb.CreateVector>(*traj) : 0); +} + +flatbuffers::Offset CreateMoveArmTrajectory(flatbuffers::FlatBufferBuilder &_fbb, const MoveArmTrajectoryT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); + +struct MoveArmJointServoT : public flatbuffers::NativeTable { + typedef MoveArmJointServo TableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.MoveArmJointServoT"; + } + std::unique_ptr goal; + MoveArmJointServoT() { + } +}; + +struct MoveArmJointServo FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { + typedef MoveArmJointServoT NativeTableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.MoveArmJointServo"; + } + enum { + VT_GOAL = 4 + }; + const JointState *goal() const { + return GetPointer(VT_GOAL); + } + bool Verify(flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + VerifyOffset(verifier, VT_GOAL) && + verifier.VerifyTable(goal()) && + verifier.EndTable(); + } + MoveArmJointServoT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; + void UnPackTo(MoveArmJointServoT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; + static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const MoveArmJointServoT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); +}; + +struct MoveArmJointServoBuilder { + flatbuffers::FlatBufferBuilder &fbb_; + flatbuffers::uoffset_t start_; + void add_goal(flatbuffers::Offset goal) { + fbb_.AddOffset(MoveArmJointServo::VT_GOAL, goal); + } + explicit MoveArmJointServoBuilder(flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + MoveArmJointServoBuilder &operator=(const MoveArmJointServoBuilder &); + flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = flatbuffers::Offset(end); + return o; + } +}; + +inline flatbuffers::Offset CreateMoveArmJointServo( + flatbuffers::FlatBufferBuilder &_fbb, + flatbuffers::Offset goal = 0) { + MoveArmJointServoBuilder builder_(_fbb); + builder_.add_goal(goal); + return builder_.Finish(); +} + +flatbuffers::Offset CreateMoveArmJointServo(flatbuffers::FlatBufferBuilder &_fbb, const MoveArmJointServoT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); + +struct MoveArmCartesianServoT : public flatbuffers::NativeTable { + typedef MoveArmCartesianServo TableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.MoveArmCartesianServoT"; + } + std::string parent; + std::unique_ptr goal; + MoveArmCartesianServoT() { + } +}; + +struct MoveArmCartesianServo FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { + typedef MoveArmCartesianServoT NativeTableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.MoveArmCartesianServo"; + } + enum { + VT_PARENT = 4, + VT_GOAL = 6 + }; + const flatbuffers::String *parent() const { + return GetPointer(VT_PARENT); + } + const Pose *goal() const { + return GetStruct(VT_GOAL); + } + bool Verify(flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + VerifyOffset(verifier, VT_PARENT) && + verifier.Verify(parent()) && + VerifyField(verifier, VT_GOAL) && + verifier.EndTable(); + } + MoveArmCartesianServoT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; + void UnPackTo(MoveArmCartesianServoT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; + static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const MoveArmCartesianServoT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); +}; + +struct MoveArmCartesianServoBuilder { + flatbuffers::FlatBufferBuilder &fbb_; + flatbuffers::uoffset_t start_; + void add_parent(flatbuffers::Offset parent) { + fbb_.AddOffset(MoveArmCartesianServo::VT_PARENT, parent); + } + void add_goal(const Pose *goal) { + fbb_.AddStruct(MoveArmCartesianServo::VT_GOAL, goal); + } + explicit MoveArmCartesianServoBuilder(flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + MoveArmCartesianServoBuilder &operator=(const MoveArmCartesianServoBuilder &); + flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = flatbuffers::Offset(end); + return o; + } +}; + +inline flatbuffers::Offset CreateMoveArmCartesianServo( + flatbuffers::FlatBufferBuilder &_fbb, + flatbuffers::Offset parent = 0, + const Pose *goal = 0) { + MoveArmCartesianServoBuilder builder_(_fbb); + builder_.add_goal(goal); + builder_.add_parent(parent); + return builder_.Finish(); +} + +inline flatbuffers::Offset CreateMoveArmCartesianServoDirect( + flatbuffers::FlatBufferBuilder &_fbb, + const char *parent = nullptr, + const Pose *goal = 0) { + return grl::flatbuffer::CreateMoveArmCartesianServo( + _fbb, + parent ? _fbb.CreateString(parent) : 0, + goal); +} + +flatbuffers::Offset CreateMoveArmCartesianServo(flatbuffers::FlatBufferBuilder &_fbb, const MoveArmCartesianServoT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); + +struct ArmControlStateT : public flatbuffers::NativeTable { + typedef ArmControlState TableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.ArmControlStateT"; + } + std::string name; + int64_t sequenceNumber; + double timeStamp; + ArmStateUnion state; + ArmControlStateT() + : sequenceNumber(0), + timeStamp(0.0) { + } +}; + +struct ArmControlState FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { + typedef ArmControlStateT NativeTableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.ArmControlState"; + } + enum { + VT_NAME = 4, + VT_SEQUENCENUMBER = 6, + VT_TIMESTAMP = 8, + VT_STATE_TYPE = 10, + VT_STATE = 12 + }; + const flatbuffers::String *name() const { + return GetPointer(VT_NAME); + } + int64_t sequenceNumber() const { + return GetField(VT_SEQUENCENUMBER, 0); + } + double timeStamp() const { + return GetField(VT_TIMESTAMP, 0.0); + } + ArmState state_type() const { + return static_cast(GetField(VT_STATE_TYPE, 0)); + } + const void *state() const { + return GetPointer(VT_STATE); + } + template const T *state_as() const; + const StartArm *state_as_StartArm() const { + return state_type() == ArmState::StartArm ? static_cast(state()) : nullptr; + } + const StopArm *state_as_StopArm() const { + return state_type() == ArmState::StopArm ? static_cast(state()) : nullptr; + } + const PauseArm *state_as_PauseArm() const { + return state_type() == ArmState::PauseArm ? static_cast(state()) : nullptr; + } + const ShutdownArm *state_as_ShutdownArm() const { + return state_type() == ArmState::ShutdownArm ? static_cast(state()) : nullptr; + } + const TeachArm *state_as_TeachArm() const { + return state_type() == ArmState::TeachArm ? static_cast(state()) : nullptr; + } + const MoveArmTrajectory *state_as_MoveArmTrajectory() const { + return state_type() == ArmState::MoveArmTrajectory ? static_cast(state()) : nullptr; + } + const MoveArmJointServo *state_as_MoveArmJointServo() const { + return state_type() == ArmState::MoveArmJointServo ? static_cast(state()) : nullptr; + } + const MoveArmCartesianServo *state_as_MoveArmCartesianServo() const { + return state_type() == ArmState::MoveArmCartesianServo ? static_cast(state()) : nullptr; + } + bool Verify(flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + VerifyOffset(verifier, VT_NAME) && + verifier.Verify(name()) && + VerifyField(verifier, VT_SEQUENCENUMBER) && + VerifyField(verifier, VT_TIMESTAMP) && + VerifyField(verifier, VT_STATE_TYPE) && + VerifyOffset(verifier, VT_STATE) && + VerifyArmState(verifier, state(), state_type()) && + verifier.EndTable(); + } + ArmControlStateT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; + void UnPackTo(ArmControlStateT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; + static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const ArmControlStateT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); +}; + +template<> inline const StartArm *ArmControlState::state_as() const { + return state_as_StartArm(); +} + +template<> inline const StopArm *ArmControlState::state_as() const { + return state_as_StopArm(); +} + +template<> inline const PauseArm *ArmControlState::state_as() const { + return state_as_PauseArm(); +} + +template<> inline const ShutdownArm *ArmControlState::state_as() const { + return state_as_ShutdownArm(); +} + +template<> inline const TeachArm *ArmControlState::state_as() const { + return state_as_TeachArm(); +} + +template<> inline const MoveArmTrajectory *ArmControlState::state_as() const { + return state_as_MoveArmTrajectory(); +} + +template<> inline const MoveArmJointServo *ArmControlState::state_as() const { + return state_as_MoveArmJointServo(); +} + +template<> inline const MoveArmCartesianServo *ArmControlState::state_as() const { + return state_as_MoveArmCartesianServo(); +} + +struct ArmControlStateBuilder { + flatbuffers::FlatBufferBuilder &fbb_; + flatbuffers::uoffset_t start_; + void add_name(flatbuffers::Offset name) { + fbb_.AddOffset(ArmControlState::VT_NAME, name); + } + void add_sequenceNumber(int64_t sequenceNumber) { + fbb_.AddElement(ArmControlState::VT_SEQUENCENUMBER, sequenceNumber, 0); + } + void add_timeStamp(double timeStamp) { + fbb_.AddElement(ArmControlState::VT_TIMESTAMP, timeStamp, 0.0); + } + void add_state_type(ArmState state_type) { + fbb_.AddElement(ArmControlState::VT_STATE_TYPE, static_cast(state_type), 0); + } + void add_state(flatbuffers::Offset state) { + fbb_.AddOffset(ArmControlState::VT_STATE, state); + } + explicit ArmControlStateBuilder(flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + ArmControlStateBuilder &operator=(const ArmControlStateBuilder &); + flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = flatbuffers::Offset(end); + return o; + } +}; + +inline flatbuffers::Offset CreateArmControlState( + flatbuffers::FlatBufferBuilder &_fbb, + flatbuffers::Offset name = 0, + int64_t sequenceNumber = 0, + double timeStamp = 0.0, + ArmState state_type = ArmState::NONE, + flatbuffers::Offset state = 0) { + ArmControlStateBuilder builder_(_fbb); + builder_.add_timeStamp(timeStamp); + builder_.add_sequenceNumber(sequenceNumber); + builder_.add_state(state); + builder_.add_name(name); + builder_.add_state_type(state_type); + return builder_.Finish(); +} + +inline flatbuffers::Offset CreateArmControlStateDirect( + flatbuffers::FlatBufferBuilder &_fbb, + const char *name = nullptr, + int64_t sequenceNumber = 0, + double timeStamp = 0.0, + ArmState state_type = ArmState::NONE, + flatbuffers::Offset state = 0) { + return grl::flatbuffer::CreateArmControlState( + _fbb, + name ? _fbb.CreateString(name) : 0, + sequenceNumber, + timeStamp, + state_type, + state); +} + +flatbuffers::Offset CreateArmControlState(flatbuffers::FlatBufferBuilder &_fbb, const ArmControlStateT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); + +struct ArmControlSeriesT : public flatbuffers::NativeTable { + typedef ArmControlSeries TableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.ArmControlSeriesT"; + } + std::vector> states; + ArmControlSeriesT() { + } +}; + +struct ArmControlSeries FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { + typedef ArmControlSeriesT NativeTableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.ArmControlSeries"; + } + enum { + VT_STATES = 4 + }; + const flatbuffers::Vector> *states() const { + return GetPointer> *>(VT_STATES); + } + bool Verify(flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + VerifyOffset(verifier, VT_STATES) && + verifier.Verify(states()) && + verifier.VerifyVectorOfTables(states()) && + verifier.EndTable(); + } + ArmControlSeriesT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; + void UnPackTo(ArmControlSeriesT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; + static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const ArmControlSeriesT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); +}; + +struct ArmControlSeriesBuilder { + flatbuffers::FlatBufferBuilder &fbb_; + flatbuffers::uoffset_t start_; + void add_states(flatbuffers::Offset>> states) { + fbb_.AddOffset(ArmControlSeries::VT_STATES, states); + } + explicit ArmControlSeriesBuilder(flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + ArmControlSeriesBuilder &operator=(const ArmControlSeriesBuilder &); + flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = flatbuffers::Offset(end); + return o; + } +}; + +inline flatbuffers::Offset CreateArmControlSeries( + flatbuffers::FlatBufferBuilder &_fbb, + flatbuffers::Offset>> states = 0) { + ArmControlSeriesBuilder builder_(_fbb); + builder_.add_states(states); + return builder_.Finish(); +} + +inline flatbuffers::Offset CreateArmControlSeriesDirect( + flatbuffers::FlatBufferBuilder &_fbb, + const std::vector> *states = nullptr) { + return grl::flatbuffer::CreateArmControlSeries( + _fbb, + states ? _fbb.CreateVector>(*states) : 0); +} + +flatbuffers::Offset CreateArmControlSeries(flatbuffers::FlatBufferBuilder &_fbb, const ArmControlSeriesT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); + +inline StartArmT *StartArm::UnPack(const flatbuffers::resolver_function_t *_resolver) const { + auto _o = new StartArmT(); + UnPackTo(_o, _resolver); + return _o; +} + +inline void StartArm::UnPackTo(StartArmT *_o, const flatbuffers::resolver_function_t *_resolver) const { + (void)_o; + (void)_resolver; +} + +inline flatbuffers::Offset StartArm::Pack(flatbuffers::FlatBufferBuilder &_fbb, const StartArmT* _o, const flatbuffers::rehasher_function_t *_rehasher) { + return CreateStartArm(_fbb, _o, _rehasher); +} + +inline flatbuffers::Offset CreateStartArm(flatbuffers::FlatBufferBuilder &_fbb, const StartArmT *_o, const flatbuffers::rehasher_function_t *_rehasher) { + (void)_rehasher; + (void)_o; + struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const StartArmT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; + return grl::flatbuffer::CreateStartArm( + _fbb); +} + +inline StopArmT *StopArm::UnPack(const flatbuffers::resolver_function_t *_resolver) const { + auto _o = new StopArmT(); + UnPackTo(_o, _resolver); + return _o; +} + +inline void StopArm::UnPackTo(StopArmT *_o, const flatbuffers::resolver_function_t *_resolver) const { + (void)_o; + (void)_resolver; +} + +inline flatbuffers::Offset StopArm::Pack(flatbuffers::FlatBufferBuilder &_fbb, const StopArmT* _o, const flatbuffers::rehasher_function_t *_rehasher) { + return CreateStopArm(_fbb, _o, _rehasher); +} + +inline flatbuffers::Offset CreateStopArm(flatbuffers::FlatBufferBuilder &_fbb, const StopArmT *_o, const flatbuffers::rehasher_function_t *_rehasher) { + (void)_rehasher; + (void)_o; + struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const StopArmT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; + return grl::flatbuffer::CreateStopArm( + _fbb); +} + +inline PauseArmT *PauseArm::UnPack(const flatbuffers::resolver_function_t *_resolver) const { + auto _o = new PauseArmT(); + UnPackTo(_o, _resolver); + return _o; +} + +inline void PauseArm::UnPackTo(PauseArmT *_o, const flatbuffers::resolver_function_t *_resolver) const { + (void)_o; + (void)_resolver; +} + +inline flatbuffers::Offset PauseArm::Pack(flatbuffers::FlatBufferBuilder &_fbb, const PauseArmT* _o, const flatbuffers::rehasher_function_t *_rehasher) { + return CreatePauseArm(_fbb, _o, _rehasher); +} + +inline flatbuffers::Offset CreatePauseArm(flatbuffers::FlatBufferBuilder &_fbb, const PauseArmT *_o, const flatbuffers::rehasher_function_t *_rehasher) { + (void)_rehasher; + (void)_o; + struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const PauseArmT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; + return grl::flatbuffer::CreatePauseArm( + _fbb); +} + +inline TeachArmT *TeachArm::UnPack(const flatbuffers::resolver_function_t *_resolver) const { + auto _o = new TeachArmT(); + UnPackTo(_o, _resolver); + return _o; +} + +inline void TeachArm::UnPackTo(TeachArmT *_o, const flatbuffers::resolver_function_t *_resolver) const { + (void)_o; + (void)_resolver; +} + +inline flatbuffers::Offset TeachArm::Pack(flatbuffers::FlatBufferBuilder &_fbb, const TeachArmT* _o, const flatbuffers::rehasher_function_t *_rehasher) { + return CreateTeachArm(_fbb, _o, _rehasher); +} + +inline flatbuffers::Offset CreateTeachArm(flatbuffers::FlatBufferBuilder &_fbb, const TeachArmT *_o, const flatbuffers::rehasher_function_t *_rehasher) { + (void)_rehasher; + (void)_o; + struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const TeachArmT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; + return grl::flatbuffer::CreateTeachArm( + _fbb); +} + +inline ShutdownArmT *ShutdownArm::UnPack(const flatbuffers::resolver_function_t *_resolver) const { + auto _o = new ShutdownArmT(); + UnPackTo(_o, _resolver); + return _o; +} + +inline void ShutdownArm::UnPackTo(ShutdownArmT *_o, const flatbuffers::resolver_function_t *_resolver) const { + (void)_o; + (void)_resolver; +} + +inline flatbuffers::Offset ShutdownArm::Pack(flatbuffers::FlatBufferBuilder &_fbb, const ShutdownArmT* _o, const flatbuffers::rehasher_function_t *_rehasher) { + return CreateShutdownArm(_fbb, _o, _rehasher); +} + +inline flatbuffers::Offset CreateShutdownArm(flatbuffers::FlatBufferBuilder &_fbb, const ShutdownArmT *_o, const flatbuffers::rehasher_function_t *_rehasher) { + (void)_rehasher; + (void)_o; + struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const ShutdownArmT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; + return grl::flatbuffer::CreateShutdownArm( + _fbb); +} + +inline MoveArmTrajectoryT *MoveArmTrajectory::UnPack(const flatbuffers::resolver_function_t *_resolver) const { + auto _o = new MoveArmTrajectoryT(); + UnPackTo(_o, _resolver); + return _o; +} + +inline void MoveArmTrajectory::UnPackTo(MoveArmTrajectoryT *_o, const flatbuffers::resolver_function_t *_resolver) const { + (void)_o; + (void)_resolver; + { auto _e = traj(); if (_e) { _o->traj.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->traj[_i] = std::unique_ptr(_e->Get(_i)->UnPack(_resolver)); } } }; +} + +inline flatbuffers::Offset MoveArmTrajectory::Pack(flatbuffers::FlatBufferBuilder &_fbb, const MoveArmTrajectoryT* _o, const flatbuffers::rehasher_function_t *_rehasher) { + return CreateMoveArmTrajectory(_fbb, _o, _rehasher); +} + +inline flatbuffers::Offset CreateMoveArmTrajectory(flatbuffers::FlatBufferBuilder &_fbb, const MoveArmTrajectoryT *_o, const flatbuffers::rehasher_function_t *_rehasher) { + (void)_rehasher; + (void)_o; + struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const MoveArmTrajectoryT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; + auto _traj = _o->traj.size() ? _fbb.CreateVector> (_o->traj.size(), [](size_t i, _VectorArgs *__va) { return CreateJointState(*__va->__fbb, __va->__o->traj[i].get(), __va->__rehasher); }, &_va ) : 0; + return grl::flatbuffer::CreateMoveArmTrajectory( + _fbb, + _traj); +} + +inline MoveArmJointServoT *MoveArmJointServo::UnPack(const flatbuffers::resolver_function_t *_resolver) const { + auto _o = new MoveArmJointServoT(); + UnPackTo(_o, _resolver); + return _o; +} + +inline void MoveArmJointServo::UnPackTo(MoveArmJointServoT *_o, const flatbuffers::resolver_function_t *_resolver) const { + (void)_o; + (void)_resolver; + { auto _e = goal(); if (_e) _o->goal = std::unique_ptr(_e->UnPack(_resolver)); }; +} + +inline flatbuffers::Offset MoveArmJointServo::Pack(flatbuffers::FlatBufferBuilder &_fbb, const MoveArmJointServoT* _o, const flatbuffers::rehasher_function_t *_rehasher) { + return CreateMoveArmJointServo(_fbb, _o, _rehasher); +} + +inline flatbuffers::Offset CreateMoveArmJointServo(flatbuffers::FlatBufferBuilder &_fbb, const MoveArmJointServoT *_o, const flatbuffers::rehasher_function_t *_rehasher) { + (void)_rehasher; + (void)_o; + struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const MoveArmJointServoT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; + auto _goal = _o->goal ? CreateJointState(_fbb, _o->goal.get(), _rehasher) : 0; + return grl::flatbuffer::CreateMoveArmJointServo( + _fbb, + _goal); +} + +inline MoveArmCartesianServoT *MoveArmCartesianServo::UnPack(const flatbuffers::resolver_function_t *_resolver) const { + auto _o = new MoveArmCartesianServoT(); + UnPackTo(_o, _resolver); + return _o; +} + +inline void MoveArmCartesianServo::UnPackTo(MoveArmCartesianServoT *_o, const flatbuffers::resolver_function_t *_resolver) const { + (void)_o; + (void)_resolver; + { auto _e = parent(); if (_e) _o->parent = _e->str(); }; + { auto _e = goal(); if (_e) _o->goal = std::unique_ptr(new Pose(*_e)); }; +} + +inline flatbuffers::Offset MoveArmCartesianServo::Pack(flatbuffers::FlatBufferBuilder &_fbb, const MoveArmCartesianServoT* _o, const flatbuffers::rehasher_function_t *_rehasher) { + return CreateMoveArmCartesianServo(_fbb, _o, _rehasher); +} + +inline flatbuffers::Offset CreateMoveArmCartesianServo(flatbuffers::FlatBufferBuilder &_fbb, const MoveArmCartesianServoT *_o, const flatbuffers::rehasher_function_t *_rehasher) { + (void)_rehasher; + (void)_o; + struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const MoveArmCartesianServoT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; + auto _parent = _o->parent.empty() ? 0 : _fbb.CreateString(_o->parent); + auto _goal = _o->goal ? _o->goal.get() : 0; + return grl::flatbuffer::CreateMoveArmCartesianServo( + _fbb, + _parent, + _goal); +} + +inline ArmControlStateT *ArmControlState::UnPack(const flatbuffers::resolver_function_t *_resolver) const { + auto _o = new ArmControlStateT(); + UnPackTo(_o, _resolver); + return _o; +} + +inline void ArmControlState::UnPackTo(ArmControlStateT *_o, const flatbuffers::resolver_function_t *_resolver) const { + (void)_o; + (void)_resolver; + { auto _e = name(); if (_e) _o->name = _e->str(); }; + { auto _e = sequenceNumber(); _o->sequenceNumber = _e; }; + { auto _e = timeStamp(); _o->timeStamp = _e; }; + { auto _e = state_type(); _o->state.type = _e; }; + { auto _e = state(); if (_e) _o->state.value = ArmStateUnion::UnPack(_e, state_type(), _resolver); }; +} + +inline flatbuffers::Offset ArmControlState::Pack(flatbuffers::FlatBufferBuilder &_fbb, const ArmControlStateT* _o, const flatbuffers::rehasher_function_t *_rehasher) { + return CreateArmControlState(_fbb, _o, _rehasher); +} + +inline flatbuffers::Offset CreateArmControlState(flatbuffers::FlatBufferBuilder &_fbb, const ArmControlStateT *_o, const flatbuffers::rehasher_function_t *_rehasher) { + (void)_rehasher; + (void)_o; + struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const ArmControlStateT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; + auto _name = _o->name.empty() ? 0 : _fbb.CreateString(_o->name); + auto _sequenceNumber = _o->sequenceNumber; + auto _timeStamp = _o->timeStamp; + auto _state_type = _o->state.type; + auto _state = _o->state.Pack(_fbb); + return grl::flatbuffer::CreateArmControlState( + _fbb, + _name, + _sequenceNumber, + _timeStamp, + _state_type, + _state); +} + +inline ArmControlSeriesT *ArmControlSeries::UnPack(const flatbuffers::resolver_function_t *_resolver) const { + auto _o = new ArmControlSeriesT(); + UnPackTo(_o, _resolver); + return _o; +} + +inline void ArmControlSeries::UnPackTo(ArmControlSeriesT *_o, const flatbuffers::resolver_function_t *_resolver) const { + (void)_o; + (void)_resolver; + { auto _e = states(); if (_e) { _o->states.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->states[_i] = std::unique_ptr(_e->Get(_i)->UnPack(_resolver)); } } }; +} + +inline flatbuffers::Offset ArmControlSeries::Pack(flatbuffers::FlatBufferBuilder &_fbb, const ArmControlSeriesT* _o, const flatbuffers::rehasher_function_t *_rehasher) { + return CreateArmControlSeries(_fbb, _o, _rehasher); +} + +inline flatbuffers::Offset CreateArmControlSeries(flatbuffers::FlatBufferBuilder &_fbb, const ArmControlSeriesT *_o, const flatbuffers::rehasher_function_t *_rehasher) { + (void)_rehasher; + (void)_o; + struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const ArmControlSeriesT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; + auto _states = _o->states.size() ? _fbb.CreateVector> (_o->states.size(), [](size_t i, _VectorArgs *__va) { return CreateArmControlState(*__va->__fbb, __va->__o->states[i].get(), __va->__rehasher); }, &_va ) : 0; + return grl::flatbuffer::CreateArmControlSeries( + _fbb, + _states); +} + +inline bool VerifyArmState(flatbuffers::Verifier &verifier, const void *obj, ArmState type) { + switch (type) { + case ArmState::NONE: { + return true; + } + case ArmState::StartArm: { + auto ptr = reinterpret_cast(obj); + return verifier.VerifyTable(ptr); + } + case ArmState::StopArm: { + auto ptr = reinterpret_cast(obj); + return verifier.VerifyTable(ptr); + } + case ArmState::PauseArm: { + auto ptr = reinterpret_cast(obj); + return verifier.VerifyTable(ptr); + } + case ArmState::ShutdownArm: { + auto ptr = reinterpret_cast(obj); + return verifier.VerifyTable(ptr); + } + case ArmState::TeachArm: { + auto ptr = reinterpret_cast(obj); + return verifier.VerifyTable(ptr); + } + case ArmState::MoveArmTrajectory: { + auto ptr = reinterpret_cast(obj); + return verifier.VerifyTable(ptr); + } + case ArmState::MoveArmJointServo: { + auto ptr = reinterpret_cast(obj); + return verifier.VerifyTable(ptr); + } + case ArmState::MoveArmCartesianServo: { + auto ptr = reinterpret_cast(obj); + return verifier.VerifyTable(ptr); + } + default: return false; + } +} + +inline bool VerifyArmStateVector(flatbuffers::Verifier &verifier, const flatbuffers::Vector> *values, const flatbuffers::Vector *types) { + if (!values || !types) return !values && !types; + if (values->size() != types->size()) return false; + for (flatbuffers::uoffset_t i = 0; i < values->size(); ++i) { + if (!VerifyArmState( + verifier, values->Get(i), types->GetEnum(i))) { + return false; + } + } + return true; +} + +inline void *ArmStateUnion::UnPack(const void *obj, ArmState type, const flatbuffers::resolver_function_t *resolver) { + switch (type) { + case ArmState::StartArm: { + auto ptr = reinterpret_cast(obj); + return ptr->UnPack(resolver); + } + case ArmState::StopArm: { + auto ptr = reinterpret_cast(obj); + return ptr->UnPack(resolver); + } + case ArmState::PauseArm: { + auto ptr = reinterpret_cast(obj); + return ptr->UnPack(resolver); + } + case ArmState::ShutdownArm: { + auto ptr = reinterpret_cast(obj); + return ptr->UnPack(resolver); + } + case ArmState::TeachArm: { + auto ptr = reinterpret_cast(obj); + return ptr->UnPack(resolver); + } + case ArmState::MoveArmTrajectory: { + auto ptr = reinterpret_cast(obj); + return ptr->UnPack(resolver); + } + case ArmState::MoveArmJointServo: { + auto ptr = reinterpret_cast(obj); + return ptr->UnPack(resolver); + } + case ArmState::MoveArmCartesianServo: { + auto ptr = reinterpret_cast(obj); + return ptr->UnPack(resolver); + } + default: return nullptr; + } +} + +inline flatbuffers::Offset ArmStateUnion::Pack(flatbuffers::FlatBufferBuilder &_fbb, const flatbuffers::rehasher_function_t *_rehasher) const { + switch (type) { + case ArmState::StartArm: { + auto ptr = reinterpret_cast(value); + return CreateStartArm(_fbb, ptr, _rehasher).Union(); + } + case ArmState::StopArm: { + auto ptr = reinterpret_cast(value); + return CreateStopArm(_fbb, ptr, _rehasher).Union(); + } + case ArmState::PauseArm: { + auto ptr = reinterpret_cast(value); + return CreatePauseArm(_fbb, ptr, _rehasher).Union(); + } + case ArmState::ShutdownArm: { + auto ptr = reinterpret_cast(value); + return CreateShutdownArm(_fbb, ptr, _rehasher).Union(); + } + case ArmState::TeachArm: { + auto ptr = reinterpret_cast(value); + return CreateTeachArm(_fbb, ptr, _rehasher).Union(); + } + case ArmState::MoveArmTrajectory: { + auto ptr = reinterpret_cast(value); + return CreateMoveArmTrajectory(_fbb, ptr, _rehasher).Union(); + } + case ArmState::MoveArmJointServo: { + auto ptr = reinterpret_cast(value); + return CreateMoveArmJointServo(_fbb, ptr, _rehasher).Union(); + } + case ArmState::MoveArmCartesianServo: { + auto ptr = reinterpret_cast(value); + return CreateMoveArmCartesianServo(_fbb, ptr, _rehasher).Union(); + } + default: return 0; + } +} + +inline ArmStateUnion::ArmStateUnion(const ArmStateUnion &u) FLATBUFFERS_NOEXCEPT : type(u.type), value(nullptr) { + switch (type) { + case ArmState::StartArm: { + value = new StartArmT(*reinterpret_cast(u.value)); + break; + } + case ArmState::StopArm: { + value = new StopArmT(*reinterpret_cast(u.value)); + break; + } + case ArmState::PauseArm: { + value = new PauseArmT(*reinterpret_cast(u.value)); + break; + } + case ArmState::ShutdownArm: { + value = new ShutdownArmT(*reinterpret_cast(u.value)); + break; + } + case ArmState::TeachArm: { + value = new TeachArmT(*reinterpret_cast(u.value)); + break; + } + case ArmState::MoveArmTrajectory: { + assert(false); // MoveArmTrajectoryT not copyable. + break; + } + case ArmState::MoveArmJointServo: { + assert(false); // MoveArmJointServoT not copyable. + break; + } + case ArmState::MoveArmCartesianServo: { + assert(false); // MoveArmCartesianServoT not copyable. + break; + } + default: + break; + } +} + +inline void ArmStateUnion::Reset() { + switch (type) { + case ArmState::StartArm: { + auto ptr = reinterpret_cast(value); + delete ptr; + break; + } + case ArmState::StopArm: { + auto ptr = reinterpret_cast(value); + delete ptr; + break; + } + case ArmState::PauseArm: { + auto ptr = reinterpret_cast(value); + delete ptr; + break; + } + case ArmState::ShutdownArm: { + auto ptr = reinterpret_cast(value); + delete ptr; + break; + } + case ArmState::TeachArm: { + auto ptr = reinterpret_cast(value); + delete ptr; + break; + } + case ArmState::MoveArmTrajectory: { + auto ptr = reinterpret_cast(value); + delete ptr; + break; + } + case ArmState::MoveArmJointServo: { + auto ptr = reinterpret_cast(value); + delete ptr; + break; + } + case ArmState::MoveArmCartesianServo: { + auto ptr = reinterpret_cast(value); + delete ptr; + break; + } + default: break; + } + value = nullptr; + type = ArmState::NONE; +} + +inline const grl::flatbuffer::ArmControlSeries *GetArmControlSeries(const void *buf) { + return flatbuffers::GetRoot(buf); +} + +inline const grl::flatbuffer::ArmControlSeries *GetSizePrefixedArmControlSeries(const void *buf) { + return flatbuffers::GetSizePrefixedRoot(buf); +} + +inline const char *ArmControlSeriesIdentifier() { + return "armc"; +} + +inline bool ArmControlSeriesBufferHasIdentifier(const void *buf) { + return flatbuffers::BufferHasIdentifier( + buf, ArmControlSeriesIdentifier()); +} + +inline bool VerifyArmControlSeriesBuffer( + flatbuffers::Verifier &verifier) { + return verifier.VerifyBuffer(ArmControlSeriesIdentifier()); +} + +inline bool VerifySizePrefixedArmControlSeriesBuffer( + flatbuffers::Verifier &verifier) { + return verifier.VerifySizePrefixedBuffer(ArmControlSeriesIdentifier()); +} + +inline const char *ArmControlSeriesExtension() { + return "armc"; +} + +inline void FinishArmControlSeriesBuffer( + flatbuffers::FlatBufferBuilder &fbb, + flatbuffers::Offset root) { + fbb.Finish(root, ArmControlSeriesIdentifier()); +} + +inline void FinishSizePrefixedArmControlSeriesBuffer( + flatbuffers::FlatBufferBuilder &fbb, + flatbuffers::Offset root) { + fbb.FinishSizePrefixed(root, ArmControlSeriesIdentifier()); +} + +inline std::unique_ptr UnPackArmControlSeries( + const void *buf, + const flatbuffers::resolver_function_t *res = nullptr) { + return std::unique_ptr(GetArmControlSeries(buf)->UnPack(res)); +} + +} // namespace flatbuffer +} // namespace grl + +#endif // FLATBUFFERS_GENERATED_ARMCONTROLSTATE_GRL_FLATBUFFER_H_ diff --git a/include/grl/flatbuffer/Euler_generated.h b/include/grl/flatbuffer/Euler_generated.h new file mode 100644 index 0000000..2c3785c --- /dev/null +++ b/include/grl/flatbuffer/Euler_generated.h @@ -0,0 +1,529 @@ +// automatically generated by the FlatBuffers compiler, do not modify + + +#ifndef FLATBUFFERS_GENERATED_EULER_GRL_FLATBUFFER_H_ +#define FLATBUFFERS_GENERATED_EULER_GRL_FLATBUFFER_H_ + +#include "flatbuffers/flatbuffers.h" + +#include "Geometry_generated.h" + +namespace grl { +namespace flatbuffer { + +struct EulerXYZd; + +struct EulerRotation; + +struct EulerPose; + +struct EulerTranslationParams; +struct EulerTranslationParamsT; + +struct EulerRotationParams; +struct EulerRotationParamsT; + +struct EulerPoseParams; +struct EulerPoseParamsT; + +enum class EulerOrder : int8_t { + xyz = 0, + yzx = 1, + zxy = 2, + xzy = 3, + zyx = 4, + yxz = 5, + zxz = 6, + xyx = 7, + yzy = 8, + xzx = 9, + yxy = 10, + MIN = xyz, + MAX = yxy +}; + +inline const EulerOrder (&EnumValuesEulerOrder())[11] { + static const EulerOrder values[] = { + EulerOrder::xyz, + EulerOrder::yzx, + EulerOrder::zxy, + EulerOrder::xzy, + EulerOrder::zyx, + EulerOrder::yxz, + EulerOrder::zxz, + EulerOrder::xyx, + EulerOrder::yzy, + EulerOrder::xzx, + EulerOrder::yxy + }; + return values; +} + +inline const char * const *EnumNamesEulerOrder() { + static const char * const names[] = { + "xyz", + "yzx", + "zxy", + "xzy", + "zyx", + "yxz", + "zxz", + "xyx", + "yzy", + "xzx", + "yxy", + nullptr + }; + return names; +} + +inline const char *EnumNameEulerOrder(EulerOrder e) { + const size_t index = static_cast(e); + return EnumNamesEulerOrder()[index]; +} + +FLATBUFFERS_MANUALLY_ALIGNED_STRUCT(8) EulerXYZd FLATBUFFERS_FINAL_CLASS { + private: + double rx_; + double ry_; + double rz_; + + public: + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.EulerXYZd"; + } + EulerXYZd() { + memset(this, 0, sizeof(EulerXYZd)); + } + EulerXYZd(double _rx, double _ry, double _rz) + : rx_(flatbuffers::EndianScalar(_rx)), + ry_(flatbuffers::EndianScalar(_ry)), + rz_(flatbuffers::EndianScalar(_rz)) { + } + double rx() const { + return flatbuffers::EndianScalar(rx_); + } + double ry() const { + return flatbuffers::EndianScalar(ry_); + } + double rz() const { + return flatbuffers::EndianScalar(rz_); + } +}; +FLATBUFFERS_STRUCT_END(EulerXYZd, 24); + +FLATBUFFERS_MANUALLY_ALIGNED_STRUCT(8) EulerRotation FLATBUFFERS_FINAL_CLASS { + private: + double r1_; + double r2_; + double r3_; + int8_t eulerOrder_; + int8_t padding0__; int16_t padding1__; int32_t padding2__; + + public: + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.EulerRotation"; + } + EulerRotation() { + memset(this, 0, sizeof(EulerRotation)); + } + EulerRotation(double _r1, double _r2, double _r3, EulerOrder _eulerOrder) + : r1_(flatbuffers::EndianScalar(_r1)), + r2_(flatbuffers::EndianScalar(_r2)), + r3_(flatbuffers::EndianScalar(_r3)), + eulerOrder_(flatbuffers::EndianScalar(static_cast(_eulerOrder))), + padding0__(0), + padding1__(0), + padding2__(0) { + (void)padding0__; (void)padding1__; (void)padding2__; + } + double r1() const { + return flatbuffers::EndianScalar(r1_); + } + double r2() const { + return flatbuffers::EndianScalar(r2_); + } + double r3() const { + return flatbuffers::EndianScalar(r3_); + } + EulerOrder eulerOrder() const { + return static_cast(flatbuffers::EndianScalar(eulerOrder_)); + } +}; +FLATBUFFERS_STRUCT_END(EulerRotation, 32); + +FLATBUFFERS_MANUALLY_ALIGNED_STRUCT(8) EulerPose FLATBUFFERS_FINAL_CLASS { + private: + Vector3d position_; + EulerRotation rotation_; + + public: + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.EulerPose"; + } + EulerPose() { + memset(this, 0, sizeof(EulerPose)); + } + EulerPose(const Vector3d &_position, const EulerRotation &_rotation) + : position_(_position), + rotation_(_rotation) { + } + const Vector3d &position() const { + return position_; + } + const EulerRotation &rotation() const { + return rotation_; + } +}; +FLATBUFFERS_STRUCT_END(EulerPose, 56); + +struct EulerTranslationParamsT : public flatbuffers::NativeTable { + typedef EulerTranslationParams TableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.EulerTranslationParamsT"; + } + double x; + double y; + double z; + EulerTranslationParamsT() + : x(0.0), + y(0.0), + z(0.0) { + } +}; + +struct EulerTranslationParams FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { + typedef EulerTranslationParamsT NativeTableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.EulerTranslationParams"; + } + enum { + VT_X = 4, + VT_Y = 6, + VT_Z = 8 + }; + double x() const { + return GetField(VT_X, 0.0); + } + double y() const { + return GetField(VT_Y, 0.0); + } + double z() const { + return GetField(VT_Z, 0.0); + } + bool Verify(flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + VerifyField(verifier, VT_X) && + VerifyField(verifier, VT_Y) && + VerifyField(verifier, VT_Z) && + verifier.EndTable(); + } + EulerTranslationParamsT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; + void UnPackTo(EulerTranslationParamsT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; + static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const EulerTranslationParamsT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); +}; + +struct EulerTranslationParamsBuilder { + flatbuffers::FlatBufferBuilder &fbb_; + flatbuffers::uoffset_t start_; + void add_x(double x) { + fbb_.AddElement(EulerTranslationParams::VT_X, x, 0.0); + } + void add_y(double y) { + fbb_.AddElement(EulerTranslationParams::VT_Y, y, 0.0); + } + void add_z(double z) { + fbb_.AddElement(EulerTranslationParams::VT_Z, z, 0.0); + } + explicit EulerTranslationParamsBuilder(flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + EulerTranslationParamsBuilder &operator=(const EulerTranslationParamsBuilder &); + flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = flatbuffers::Offset(end); + return o; + } +}; + +inline flatbuffers::Offset CreateEulerTranslationParams( + flatbuffers::FlatBufferBuilder &_fbb, + double x = 0.0, + double y = 0.0, + double z = 0.0) { + EulerTranslationParamsBuilder builder_(_fbb); + builder_.add_z(z); + builder_.add_y(y); + builder_.add_x(x); + return builder_.Finish(); +} + +flatbuffers::Offset CreateEulerTranslationParams(flatbuffers::FlatBufferBuilder &_fbb, const EulerTranslationParamsT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); + +struct EulerRotationParamsT : public flatbuffers::NativeTable { + typedef EulerRotationParams TableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.EulerRotationParamsT"; + } + double r1; + double r2; + double r3; + EulerOrder eulerOrder; + EulerRotationParamsT() + : r1(0.0), + r2(0.0), + r3(0.0), + eulerOrder(EulerOrder::xyz) { + } +}; + +struct EulerRotationParams FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { + typedef EulerRotationParamsT NativeTableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.EulerRotationParams"; + } + enum { + VT_R1 = 4, + VT_R2 = 6, + VT_R3 = 8, + VT_EULERORDER = 10 + }; + double r1() const { + return GetField(VT_R1, 0.0); + } + double r2() const { + return GetField(VT_R2, 0.0); + } + double r3() const { + return GetField(VT_R3, 0.0); + } + EulerOrder eulerOrder() const { + return static_cast(GetField(VT_EULERORDER, 0)); + } + bool Verify(flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + VerifyField(verifier, VT_R1) && + VerifyField(verifier, VT_R2) && + VerifyField(verifier, VT_R3) && + VerifyField(verifier, VT_EULERORDER) && + verifier.EndTable(); + } + EulerRotationParamsT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; + void UnPackTo(EulerRotationParamsT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; + static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const EulerRotationParamsT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); +}; + +struct EulerRotationParamsBuilder { + flatbuffers::FlatBufferBuilder &fbb_; + flatbuffers::uoffset_t start_; + void add_r1(double r1) { + fbb_.AddElement(EulerRotationParams::VT_R1, r1, 0.0); + } + void add_r2(double r2) { + fbb_.AddElement(EulerRotationParams::VT_R2, r2, 0.0); + } + void add_r3(double r3) { + fbb_.AddElement(EulerRotationParams::VT_R3, r3, 0.0); + } + void add_eulerOrder(EulerOrder eulerOrder) { + fbb_.AddElement(EulerRotationParams::VT_EULERORDER, static_cast(eulerOrder), 0); + } + explicit EulerRotationParamsBuilder(flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + EulerRotationParamsBuilder &operator=(const EulerRotationParamsBuilder &); + flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = flatbuffers::Offset(end); + return o; + } +}; + +inline flatbuffers::Offset CreateEulerRotationParams( + flatbuffers::FlatBufferBuilder &_fbb, + double r1 = 0.0, + double r2 = 0.0, + double r3 = 0.0, + EulerOrder eulerOrder = EulerOrder::xyz) { + EulerRotationParamsBuilder builder_(_fbb); + builder_.add_r3(r3); + builder_.add_r2(r2); + builder_.add_r1(r1); + builder_.add_eulerOrder(eulerOrder); + return builder_.Finish(); +} + +flatbuffers::Offset CreateEulerRotationParams(flatbuffers::FlatBufferBuilder &_fbb, const EulerRotationParamsT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); + +struct EulerPoseParamsT : public flatbuffers::NativeTable { + typedef EulerPoseParams TableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.EulerPoseParamsT"; + } + std::unique_ptr position; + std::unique_ptr rotation; + EulerPoseParamsT() { + } +}; + +struct EulerPoseParams FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { + typedef EulerPoseParamsT NativeTableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.EulerPoseParams"; + } + enum { + VT_POSITION = 4, + VT_ROTATION = 6 + }; + const Vector3d *position() const { + return GetStruct(VT_POSITION); + } + const EulerRotation *rotation() const { + return GetStruct(VT_ROTATION); + } + bool Verify(flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + VerifyField(verifier, VT_POSITION) && + VerifyField(verifier, VT_ROTATION) && + verifier.EndTable(); + } + EulerPoseParamsT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; + void UnPackTo(EulerPoseParamsT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; + static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const EulerPoseParamsT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); +}; + +struct EulerPoseParamsBuilder { + flatbuffers::FlatBufferBuilder &fbb_; + flatbuffers::uoffset_t start_; + void add_position(const Vector3d *position) { + fbb_.AddStruct(EulerPoseParams::VT_POSITION, position); + } + void add_rotation(const EulerRotation *rotation) { + fbb_.AddStruct(EulerPoseParams::VT_ROTATION, rotation); + } + explicit EulerPoseParamsBuilder(flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + EulerPoseParamsBuilder &operator=(const EulerPoseParamsBuilder &); + flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = flatbuffers::Offset(end); + return o; + } +}; + +inline flatbuffers::Offset CreateEulerPoseParams( + flatbuffers::FlatBufferBuilder &_fbb, + const Vector3d *position = 0, + const EulerRotation *rotation = 0) { + EulerPoseParamsBuilder builder_(_fbb); + builder_.add_rotation(rotation); + builder_.add_position(position); + return builder_.Finish(); +} + +flatbuffers::Offset CreateEulerPoseParams(flatbuffers::FlatBufferBuilder &_fbb, const EulerPoseParamsT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); + +inline EulerTranslationParamsT *EulerTranslationParams::UnPack(const flatbuffers::resolver_function_t *_resolver) const { + auto _o = new EulerTranslationParamsT(); + UnPackTo(_o, _resolver); + return _o; +} + +inline void EulerTranslationParams::UnPackTo(EulerTranslationParamsT *_o, const flatbuffers::resolver_function_t *_resolver) const { + (void)_o; + (void)_resolver; + { auto _e = x(); _o->x = _e; }; + { auto _e = y(); _o->y = _e; }; + { auto _e = z(); _o->z = _e; }; +} + +inline flatbuffers::Offset EulerTranslationParams::Pack(flatbuffers::FlatBufferBuilder &_fbb, const EulerTranslationParamsT* _o, const flatbuffers::rehasher_function_t *_rehasher) { + return CreateEulerTranslationParams(_fbb, _o, _rehasher); +} + +inline flatbuffers::Offset CreateEulerTranslationParams(flatbuffers::FlatBufferBuilder &_fbb, const EulerTranslationParamsT *_o, const flatbuffers::rehasher_function_t *_rehasher) { + (void)_rehasher; + (void)_o; + struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const EulerTranslationParamsT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; + auto _x = _o->x; + auto _y = _o->y; + auto _z = _o->z; + return grl::flatbuffer::CreateEulerTranslationParams( + _fbb, + _x, + _y, + _z); +} + +inline EulerRotationParamsT *EulerRotationParams::UnPack(const flatbuffers::resolver_function_t *_resolver) const { + auto _o = new EulerRotationParamsT(); + UnPackTo(_o, _resolver); + return _o; +} + +inline void EulerRotationParams::UnPackTo(EulerRotationParamsT *_o, const flatbuffers::resolver_function_t *_resolver) const { + (void)_o; + (void)_resolver; + { auto _e = r1(); _o->r1 = _e; }; + { auto _e = r2(); _o->r2 = _e; }; + { auto _e = r3(); _o->r3 = _e; }; + { auto _e = eulerOrder(); _o->eulerOrder = _e; }; +} + +inline flatbuffers::Offset EulerRotationParams::Pack(flatbuffers::FlatBufferBuilder &_fbb, const EulerRotationParamsT* _o, const flatbuffers::rehasher_function_t *_rehasher) { + return CreateEulerRotationParams(_fbb, _o, _rehasher); +} + +inline flatbuffers::Offset CreateEulerRotationParams(flatbuffers::FlatBufferBuilder &_fbb, const EulerRotationParamsT *_o, const flatbuffers::rehasher_function_t *_rehasher) { + (void)_rehasher; + (void)_o; + struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const EulerRotationParamsT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; + auto _r1 = _o->r1; + auto _r2 = _o->r2; + auto _r3 = _o->r3; + auto _eulerOrder = _o->eulerOrder; + return grl::flatbuffer::CreateEulerRotationParams( + _fbb, + _r1, + _r2, + _r3, + _eulerOrder); +} + +inline EulerPoseParamsT *EulerPoseParams::UnPack(const flatbuffers::resolver_function_t *_resolver) const { + auto _o = new EulerPoseParamsT(); + UnPackTo(_o, _resolver); + return _o; +} + +inline void EulerPoseParams::UnPackTo(EulerPoseParamsT *_o, const flatbuffers::resolver_function_t *_resolver) const { + (void)_o; + (void)_resolver; + { auto _e = position(); if (_e) _o->position = std::unique_ptr(new Vector3d(*_e)); }; + { auto _e = rotation(); if (_e) _o->rotation = std::unique_ptr(new EulerRotation(*_e)); }; +} + +inline flatbuffers::Offset EulerPoseParams::Pack(flatbuffers::FlatBufferBuilder &_fbb, const EulerPoseParamsT* _o, const flatbuffers::rehasher_function_t *_rehasher) { + return CreateEulerPoseParams(_fbb, _o, _rehasher); +} + +inline flatbuffers::Offset CreateEulerPoseParams(flatbuffers::FlatBufferBuilder &_fbb, const EulerPoseParamsT *_o, const flatbuffers::rehasher_function_t *_rehasher) { + (void)_rehasher; + (void)_o; + struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const EulerPoseParamsT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; + auto _position = _o->position ? _o->position.get() : 0; + auto _rotation = _o->rotation ? _o->rotation.get() : 0; + return grl::flatbuffer::CreateEulerPoseParams( + _fbb, + _position, + _rotation); +} + +} // namespace flatbuffer +} // namespace grl + +#endif // FLATBUFFERS_GENERATED_EULER_GRL_FLATBUFFER_H_ diff --git a/include/grl/flatbuffer/FusionTrack_generated.h b/include/grl/flatbuffer/FusionTrack_generated.h new file mode 100644 index 0000000..ab0b966 --- /dev/null +++ b/include/grl/flatbuffer/FusionTrack_generated.h @@ -0,0 +1,1803 @@ +// automatically generated by the FlatBuffers compiler, do not modify + + +#ifndef FLATBUFFERS_GENERATED_FUSIONTRACK_GRL_FLATBUFFER_H_ +#define FLATBUFFERS_GENERATED_FUSIONTRACK_GRL_FLATBUFFER_H_ + +#include "flatbuffers/flatbuffers.h" + +#include "Geometry_generated.h" +#include "Time_generated.h" + +namespace grl { +namespace flatbuffer { + +struct ftkGeometry; +struct ftkGeometryT; + +struct ftkMarker; +struct ftkMarkerT; + +struct ftk3DFiducial; +struct ftk3DFiducialT; + +struct ftkRegionOfInterest; +struct ftkRegionOfInterestT; + +struct FusionTrackFrame; +struct FusionTrackFrameT; + +struct FusionTrackParameters; +struct FusionTrackParametersT; + +struct FusionTrackMessage; +struct FusionTrackMessageT; + +enum class ftkQueryStatus : int32_t { + QS_WAR_SKIPPED = -1, + QS_OK = 0, + QS_ERR_OVERFLOW = 1, + QS_ERR_INVALID_RESERVED_SIZE = 2, + QS_REPROCESS = 10, + MIN = QS_WAR_SKIPPED, + MAX = QS_REPROCESS +}; + +inline const ftkQueryStatus (&EnumValuesftkQueryStatus())[5] { + static const ftkQueryStatus values[] = { + ftkQueryStatus::QS_WAR_SKIPPED, + ftkQueryStatus::QS_OK, + ftkQueryStatus::QS_ERR_OVERFLOW, + ftkQueryStatus::QS_ERR_INVALID_RESERVED_SIZE, + ftkQueryStatus::QS_REPROCESS + }; + return values; +} + +inline const char * const *EnumNamesftkQueryStatus() { + static const char * const names[] = { + "QS_WAR_SKIPPED", + "QS_OK", + "QS_ERR_OVERFLOW", + "QS_ERR_INVALID_RESERVED_SIZE", + "", + "", + "", + "", + "", + "", + "", + "QS_REPROCESS", + nullptr + }; + return names; +} + +inline const char *EnumNameftkQueryStatus(ftkQueryStatus e) { + const size_t index = static_cast(e) - static_cast(ftkQueryStatus::QS_WAR_SKIPPED); + return EnumNamesftkQueryStatus()[index]; +} + +enum class ftkDeviceType : int32_t { + DEV_SIMULATOR = 0, + DEV_INFINITRACK = 1, + DEV_FUSIONTRACK_500 = 2, + DEV_FUSIONTRACK_250 = 3, + DEV_UNKNOWN_DEVICE = 127, + MIN = DEV_SIMULATOR, + MAX = DEV_UNKNOWN_DEVICE +}; + +inline const ftkDeviceType (&EnumValuesftkDeviceType())[5] { + static const ftkDeviceType values[] = { + ftkDeviceType::DEV_SIMULATOR, + ftkDeviceType::DEV_INFINITRACK, + ftkDeviceType::DEV_FUSIONTRACK_500, + ftkDeviceType::DEV_FUSIONTRACK_250, + ftkDeviceType::DEV_UNKNOWN_DEVICE + }; + return values; +} + +struct ftkGeometryT : public flatbuffers::NativeTable { + typedef ftkGeometry TableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.ftkGeometryT"; + } + std::string name; + uint32_t geometryID; + uint32_t version; + std::vector positions; + ftkGeometryT() + : geometryID(0), + version(0) { + } +}; + +struct ftkGeometry FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { + typedef ftkGeometryT NativeTableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.ftkGeometry"; + } + enum { + VT_NAME = 4, + VT_GEOMETRYID = 6, + VT_VERSION = 8, + VT_POSITIONS = 10 + }; + const flatbuffers::String *name() const { + return GetPointer(VT_NAME); + } + uint32_t geometryID() const { + return GetField(VT_GEOMETRYID, 0); + } + uint32_t version() const { + return GetField(VT_VERSION, 0); + } + const flatbuffers::Vector *positions() const { + return GetPointer *>(VT_POSITIONS); + } + bool Verify(flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + VerifyOffset(verifier, VT_NAME) && + verifier.Verify(name()) && + VerifyField(verifier, VT_GEOMETRYID) && + VerifyField(verifier, VT_VERSION) && + VerifyOffset(verifier, VT_POSITIONS) && + verifier.Verify(positions()) && + verifier.EndTable(); + } + ftkGeometryT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; + void UnPackTo(ftkGeometryT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; + static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const ftkGeometryT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); +}; + +struct ftkGeometryBuilder { + flatbuffers::FlatBufferBuilder &fbb_; + flatbuffers::uoffset_t start_; + void add_name(flatbuffers::Offset name) { + fbb_.AddOffset(ftkGeometry::VT_NAME, name); + } + void add_geometryID(uint32_t geometryID) { + fbb_.AddElement(ftkGeometry::VT_GEOMETRYID, geometryID, 0); + } + void add_version(uint32_t version) { + fbb_.AddElement(ftkGeometry::VT_VERSION, version, 0); + } + void add_positions(flatbuffers::Offset> positions) { + fbb_.AddOffset(ftkGeometry::VT_POSITIONS, positions); + } + explicit ftkGeometryBuilder(flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + ftkGeometryBuilder &operator=(const ftkGeometryBuilder &); + flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = flatbuffers::Offset(end); + return o; + } +}; + +inline flatbuffers::Offset CreateftkGeometry( + flatbuffers::FlatBufferBuilder &_fbb, + flatbuffers::Offset name = 0, + uint32_t geometryID = 0, + uint32_t version = 0, + flatbuffers::Offset> positions = 0) { + ftkGeometryBuilder builder_(_fbb); + builder_.add_positions(positions); + builder_.add_version(version); + builder_.add_geometryID(geometryID); + builder_.add_name(name); + return builder_.Finish(); +} + +inline flatbuffers::Offset CreateftkGeometryDirect( + flatbuffers::FlatBufferBuilder &_fbb, + const char *name = nullptr, + uint32_t geometryID = 0, + uint32_t version = 0, + const std::vector *positions = nullptr) { + return grl::flatbuffer::CreateftkGeometry( + _fbb, + name ? _fbb.CreateString(name) : 0, + geometryID, + version, + positions ? _fbb.CreateVectorOfStructs(*positions) : 0); +} + +flatbuffers::Offset CreateftkGeometry(flatbuffers::FlatBufferBuilder &_fbb, const ftkGeometryT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); + +struct ftkMarkerT : public flatbuffers::NativeTable { + typedef ftkMarker TableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.ftkMarkerT"; + } + std::string name; + uint32_t ID; + uint32_t geometryID; + std::vector geometryPresenceMask; + std::unique_ptr transform; + ftkMarkerT() + : ID(0), + geometryID(0) { + } +}; + +struct ftkMarker FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { + typedef ftkMarkerT NativeTableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.ftkMarker"; + } + enum { + VT_NAME = 4, + VT_ID = 6, + VT_GEOMETRYID = 8, + VT_GEOMETRYPRESENCEMASK = 10, + VT_TRANSFORM = 12 + }; + const flatbuffers::String *name() const { + return GetPointer(VT_NAME); + } + uint32_t ID() const { + return GetField(VT_ID, 0); + } + uint32_t geometryID() const { + return GetField(VT_GEOMETRYID, 0); + } + const flatbuffers::Vector *geometryPresenceMask() const { + return GetPointer *>(VT_GEOMETRYPRESENCEMASK); + } + const Pose *transform() const { + return GetStruct(VT_TRANSFORM); + } + bool Verify(flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + VerifyOffset(verifier, VT_NAME) && + verifier.Verify(name()) && + VerifyField(verifier, VT_ID) && + VerifyField(verifier, VT_GEOMETRYID) && + VerifyOffset(verifier, VT_GEOMETRYPRESENCEMASK) && + verifier.Verify(geometryPresenceMask()) && + VerifyField(verifier, VT_TRANSFORM) && + verifier.EndTable(); + } + ftkMarkerT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; + void UnPackTo(ftkMarkerT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; + static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const ftkMarkerT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); +}; + +struct ftkMarkerBuilder { + flatbuffers::FlatBufferBuilder &fbb_; + flatbuffers::uoffset_t start_; + void add_name(flatbuffers::Offset name) { + fbb_.AddOffset(ftkMarker::VT_NAME, name); + } + void add_ID(uint32_t ID) { + fbb_.AddElement(ftkMarker::VT_ID, ID, 0); + } + void add_geometryID(uint32_t geometryID) { + fbb_.AddElement(ftkMarker::VT_GEOMETRYID, geometryID, 0); + } + void add_geometryPresenceMask(flatbuffers::Offset> geometryPresenceMask) { + fbb_.AddOffset(ftkMarker::VT_GEOMETRYPRESENCEMASK, geometryPresenceMask); + } + void add_transform(const Pose *transform) { + fbb_.AddStruct(ftkMarker::VT_TRANSFORM, transform); + } + explicit ftkMarkerBuilder(flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + ftkMarkerBuilder &operator=(const ftkMarkerBuilder &); + flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = flatbuffers::Offset(end); + return o; + } +}; + +inline flatbuffers::Offset CreateftkMarker( + flatbuffers::FlatBufferBuilder &_fbb, + flatbuffers::Offset name = 0, + uint32_t ID = 0, + uint32_t geometryID = 0, + flatbuffers::Offset> geometryPresenceMask = 0, + const Pose *transform = 0) { + ftkMarkerBuilder builder_(_fbb); + builder_.add_transform(transform); + builder_.add_geometryPresenceMask(geometryPresenceMask); + builder_.add_geometryID(geometryID); + builder_.add_ID(ID); + builder_.add_name(name); + return builder_.Finish(); +} + +inline flatbuffers::Offset CreateftkMarkerDirect( + flatbuffers::FlatBufferBuilder &_fbb, + const char *name = nullptr, + uint32_t ID = 0, + uint32_t geometryID = 0, + const std::vector *geometryPresenceMask = nullptr, + const Pose *transform = 0) { + return grl::flatbuffer::CreateftkMarker( + _fbb, + name ? _fbb.CreateString(name) : 0, + ID, + geometryID, + geometryPresenceMask ? _fbb.CreateVector(*geometryPresenceMask) : 0, + transform); +} + +flatbuffers::Offset CreateftkMarker(flatbuffers::FlatBufferBuilder &_fbb, const ftkMarkerT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); + +struct ftk3DFiducialT : public flatbuffers::NativeTable { + typedef ftk3DFiducial TableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.ftk3DFiducialT"; + } + uint32_t markerID; + uint32_t leftIndex; + uint32_t rightIndex; + std::unique_ptr position; + double epipolarErrorPixels; + double triangulationError; + double probability; + ftk3DFiducialT() + : markerID(0), + leftIndex(0), + rightIndex(0), + epipolarErrorPixels(0.0), + triangulationError(0.0), + probability(0.0) { + } +}; + +struct ftk3DFiducial FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { + typedef ftk3DFiducialT NativeTableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.ftk3DFiducial"; + } + enum { + VT_MARKERID = 4, + VT_LEFTINDEX = 6, + VT_RIGHTINDEX = 8, + VT_POSITION = 10, + VT_EPIPOLARERRORPIXELS = 12, + VT_TRIANGULATIONERROR = 14, + VT_PROBABILITY = 16 + }; + uint32_t markerID() const { + return GetField(VT_MARKERID, 0); + } + uint32_t leftIndex() const { + return GetField(VT_LEFTINDEX, 0); + } + uint32_t rightIndex() const { + return GetField(VT_RIGHTINDEX, 0); + } + const Vector3d *position() const { + return GetStruct(VT_POSITION); + } + double epipolarErrorPixels() const { + return GetField(VT_EPIPOLARERRORPIXELS, 0.0); + } + double triangulationError() const { + return GetField(VT_TRIANGULATIONERROR, 0.0); + } + double probability() const { + return GetField(VT_PROBABILITY, 0.0); + } + bool Verify(flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + VerifyField(verifier, VT_MARKERID) && + VerifyField(verifier, VT_LEFTINDEX) && + VerifyField(verifier, VT_RIGHTINDEX) && + VerifyField(verifier, VT_POSITION) && + VerifyField(verifier, VT_EPIPOLARERRORPIXELS) && + VerifyField(verifier, VT_TRIANGULATIONERROR) && + VerifyField(verifier, VT_PROBABILITY) && + verifier.EndTable(); + } + ftk3DFiducialT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; + void UnPackTo(ftk3DFiducialT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; + static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const ftk3DFiducialT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); +}; + +struct ftk3DFiducialBuilder { + flatbuffers::FlatBufferBuilder &fbb_; + flatbuffers::uoffset_t start_; + void add_markerID(uint32_t markerID) { + fbb_.AddElement(ftk3DFiducial::VT_MARKERID, markerID, 0); + } + void add_leftIndex(uint32_t leftIndex) { + fbb_.AddElement(ftk3DFiducial::VT_LEFTINDEX, leftIndex, 0); + } + void add_rightIndex(uint32_t rightIndex) { + fbb_.AddElement(ftk3DFiducial::VT_RIGHTINDEX, rightIndex, 0); + } + void add_position(const Vector3d *position) { + fbb_.AddStruct(ftk3DFiducial::VT_POSITION, position); + } + void add_epipolarErrorPixels(double epipolarErrorPixels) { + fbb_.AddElement(ftk3DFiducial::VT_EPIPOLARERRORPIXELS, epipolarErrorPixels, 0.0); + } + void add_triangulationError(double triangulationError) { + fbb_.AddElement(ftk3DFiducial::VT_TRIANGULATIONERROR, triangulationError, 0.0); + } + void add_probability(double probability) { + fbb_.AddElement(ftk3DFiducial::VT_PROBABILITY, probability, 0.0); + } + explicit ftk3DFiducialBuilder(flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + ftk3DFiducialBuilder &operator=(const ftk3DFiducialBuilder &); + flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = flatbuffers::Offset(end); + return o; + } +}; + +inline flatbuffers::Offset Createftk3DFiducial( + flatbuffers::FlatBufferBuilder &_fbb, + uint32_t markerID = 0, + uint32_t leftIndex = 0, + uint32_t rightIndex = 0, + const Vector3d *position = 0, + double epipolarErrorPixels = 0.0, + double triangulationError = 0.0, + double probability = 0.0) { + ftk3DFiducialBuilder builder_(_fbb); + builder_.add_probability(probability); + builder_.add_triangulationError(triangulationError); + builder_.add_epipolarErrorPixels(epipolarErrorPixels); + builder_.add_position(position); + builder_.add_rightIndex(rightIndex); + builder_.add_leftIndex(leftIndex); + builder_.add_markerID(markerID); + return builder_.Finish(); +} + +flatbuffers::Offset Createftk3DFiducial(flatbuffers::FlatBufferBuilder &_fbb, const ftk3DFiducialT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); + +struct ftkRegionOfInterestT : public flatbuffers::NativeTable { + typedef ftkRegionOfInterest TableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.ftkRegionOfInterestT"; + } + double centerXPixels; + double centerYPixels; + uint32_t RightEdge; + uint32_t BottomEdge; + uint32_t LeftEdge; + uint32_t TopEdge; + uint32_t pixelsCount; + double probability; + ftkRegionOfInterestT() + : centerXPixels(0.0), + centerYPixels(0.0), + RightEdge(0), + BottomEdge(0), + LeftEdge(0), + TopEdge(0), + pixelsCount(0), + probability(0.0) { + } +}; + +struct ftkRegionOfInterest FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { + typedef ftkRegionOfInterestT NativeTableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.ftkRegionOfInterest"; + } + enum { + VT_CENTERXPIXELS = 4, + VT_CENTERYPIXELS = 6, + VT_RIGHTEDGE = 8, + VT_BOTTOMEDGE = 10, + VT_LEFTEDGE = 12, + VT_TOPEDGE = 14, + VT_PIXELSCOUNT = 16, + VT_PROBABILITY = 18 + }; + double centerXPixels() const { + return GetField(VT_CENTERXPIXELS, 0.0); + } + double centerYPixels() const { + return GetField(VT_CENTERYPIXELS, 0.0); + } + uint32_t RightEdge() const { + return GetField(VT_RIGHTEDGE, 0); + } + uint32_t BottomEdge() const { + return GetField(VT_BOTTOMEDGE, 0); + } + uint32_t LeftEdge() const { + return GetField(VT_LEFTEDGE, 0); + } + uint32_t TopEdge() const { + return GetField(VT_TOPEDGE, 0); + } + uint32_t pixelsCount() const { + return GetField(VT_PIXELSCOUNT, 0); + } + double probability() const { + return GetField(VT_PROBABILITY, 0.0); + } + bool Verify(flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + VerifyField(verifier, VT_CENTERXPIXELS) && + VerifyField(verifier, VT_CENTERYPIXELS) && + VerifyField(verifier, VT_RIGHTEDGE) && + VerifyField(verifier, VT_BOTTOMEDGE) && + VerifyField(verifier, VT_LEFTEDGE) && + VerifyField(verifier, VT_TOPEDGE) && + VerifyField(verifier, VT_PIXELSCOUNT) && + VerifyField(verifier, VT_PROBABILITY) && + verifier.EndTable(); + } + ftkRegionOfInterestT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; + void UnPackTo(ftkRegionOfInterestT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; + static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const ftkRegionOfInterestT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); +}; + +struct ftkRegionOfInterestBuilder { + flatbuffers::FlatBufferBuilder &fbb_; + flatbuffers::uoffset_t start_; + void add_centerXPixels(double centerXPixels) { + fbb_.AddElement(ftkRegionOfInterest::VT_CENTERXPIXELS, centerXPixels, 0.0); + } + void add_centerYPixels(double centerYPixels) { + fbb_.AddElement(ftkRegionOfInterest::VT_CENTERYPIXELS, centerYPixels, 0.0); + } + void add_RightEdge(uint32_t RightEdge) { + fbb_.AddElement(ftkRegionOfInterest::VT_RIGHTEDGE, RightEdge, 0); + } + void add_BottomEdge(uint32_t BottomEdge) { + fbb_.AddElement(ftkRegionOfInterest::VT_BOTTOMEDGE, BottomEdge, 0); + } + void add_LeftEdge(uint32_t LeftEdge) { + fbb_.AddElement(ftkRegionOfInterest::VT_LEFTEDGE, LeftEdge, 0); + } + void add_TopEdge(uint32_t TopEdge) { + fbb_.AddElement(ftkRegionOfInterest::VT_TOPEDGE, TopEdge, 0); + } + void add_pixelsCount(uint32_t pixelsCount) { + fbb_.AddElement(ftkRegionOfInterest::VT_PIXELSCOUNT, pixelsCount, 0); + } + void add_probability(double probability) { + fbb_.AddElement(ftkRegionOfInterest::VT_PROBABILITY, probability, 0.0); + } + explicit ftkRegionOfInterestBuilder(flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + ftkRegionOfInterestBuilder &operator=(const ftkRegionOfInterestBuilder &); + flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = flatbuffers::Offset(end); + return o; + } +}; + +inline flatbuffers::Offset CreateftkRegionOfInterest( + flatbuffers::FlatBufferBuilder &_fbb, + double centerXPixels = 0.0, + double centerYPixels = 0.0, + uint32_t RightEdge = 0, + uint32_t BottomEdge = 0, + uint32_t LeftEdge = 0, + uint32_t TopEdge = 0, + uint32_t pixelsCount = 0, + double probability = 0.0) { + ftkRegionOfInterestBuilder builder_(_fbb); + builder_.add_probability(probability); + builder_.add_centerYPixels(centerYPixels); + builder_.add_centerXPixels(centerXPixels); + builder_.add_pixelsCount(pixelsCount); + builder_.add_TopEdge(TopEdge); + builder_.add_LeftEdge(LeftEdge); + builder_.add_BottomEdge(BottomEdge); + builder_.add_RightEdge(RightEdge); + return builder_.Finish(); +} + +flatbuffers::Offset CreateftkRegionOfInterest(flatbuffers::FlatBufferBuilder &_fbb, const ftkRegionOfInterestT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); + +struct FusionTrackFrameT : public flatbuffers::NativeTable { + typedef FusionTrackFrame TableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.FusionTrackFrameT"; + } + double timestamp; + uint64_t serialNumber; + uint64_t hardwareTimestampUS; + uint64_t desynchroUS; + uint32_t counter; + uint32_t format; + uint32_t width; + uint32_t height; + int32_t imageStrideInBytes; + uint32_t imageHeaderVersion; + int32_t imageHeaderStatus; + std::string imageLeftPixels; + uint32_t imageLeftPixelsVersion; + int32_t imageLeftStatus; + std::string imageRightPixels; + uint32_t imageRightPixelsVersion; + int32_t imageRightStatus; + std::vector> regionsOfInterestLeft; + uint32_t regionsOfInterestLeftVersion; + int32_t regionsOfInterestLeftStatus; + std::vector> regionsOfInterestRight; + uint32_t regionsOfInterestRightVersion; + int32_t regionsOfInterestRightStatus; + std::vector> threeDFiducials; + uint32_t threeDFiducialsVersion; + int32_t threeDFiducialsStatus; + std::vector> markers; + uint32_t markersVersion; + int32_t markersStatus; + int32_t deviceType; + int64_t ftkError; + FusionTrackFrameT() + : timestamp(0.0), + serialNumber(0), + hardwareTimestampUS(0), + desynchroUS(0), + counter(0), + format(0), + width(0), + height(0), + imageStrideInBytes(0), + imageHeaderVersion(0), + imageHeaderStatus(0), + imageLeftPixelsVersion(0), + imageLeftStatus(0), + imageRightPixelsVersion(0), + imageRightStatus(0), + regionsOfInterestLeftVersion(0), + regionsOfInterestLeftStatus(0), + regionsOfInterestRightVersion(0), + regionsOfInterestRightStatus(0), + threeDFiducialsVersion(0), + threeDFiducialsStatus(0), + markersVersion(0), + markersStatus(0), + deviceType(0), + ftkError(0) { + } +}; + +/// Data for one frame capture +/// On the Atracsys FusionTrack optical tracker +/// look at ftkInterface.h for details +/// Frame class is defined in FusionTrack.hpp +struct FusionTrackFrame FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { + typedef FusionTrackFrameT NativeTableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.FusionTrackFrame"; + } + enum { + VT_TIMESTAMP = 4, + VT_SERIALNUMBER = 6, + VT_HARDWARETIMESTAMPUS = 8, + VT_DESYNCHROUS = 10, + VT_COUNTER = 12, + VT_FORMAT = 14, + VT_WIDTH = 16, + VT_HEIGHT = 18, + VT_IMAGESTRIDEINBYTES = 20, + VT_IMAGEHEADERVERSION = 22, + VT_IMAGEHEADERSTATUS = 24, + VT_IMAGELEFTPIXELS = 26, + VT_IMAGELEFTPIXELSVERSION = 28, + VT_IMAGELEFTSTATUS = 30, + VT_IMAGERIGHTPIXELS = 32, + VT_IMAGERIGHTPIXELSVERSION = 34, + VT_IMAGERIGHTSTATUS = 36, + VT_REGIONSOFINTERESTLEFT = 38, + VT_REGIONSOFINTERESTLEFTVERSION = 40, + VT_REGIONSOFINTERESTLEFTSTATUS = 42, + VT_REGIONSOFINTERESTRIGHT = 44, + VT_REGIONSOFINTERESTRIGHTVERSION = 46, + VT_REGIONSOFINTERESTRIGHTSTATUS = 48, + VT_THREEDFIDUCIALS = 50, + VT_THREEDFIDUCIALSVERSION = 52, + VT_THREEDFIDUCIALSSTATUS = 54, + VT_MARKERS = 56, + VT_MARKERSVERSION = 58, + VT_MARKERSSTATUS = 60, + VT_DEVICETYPE = 62, + VT_FTKERROR = 64 + }; + double timestamp() const { + return GetField(VT_TIMESTAMP, 0.0); + } + uint64_t serialNumber() const { + return GetField(VT_SERIALNUMBER, 0); + } + uint64_t hardwareTimestampUS() const { + return GetField(VT_HARDWARETIMESTAMPUS, 0); + } + uint64_t desynchroUS() const { + return GetField(VT_DESYNCHROUS, 0); + } + uint32_t counter() const { + return GetField(VT_COUNTER, 0); + } + uint32_t format() const { + return GetField(VT_FORMAT, 0); + } + uint32_t width() const { + return GetField(VT_WIDTH, 0); + } + uint32_t height() const { + return GetField(VT_HEIGHT, 0); + } + int32_t imageStrideInBytes() const { + return GetField(VT_IMAGESTRIDEINBYTES, 0); + } + uint32_t imageHeaderVersion() const { + return GetField(VT_IMAGEHEADERVERSION, 0); + } + int32_t imageHeaderStatus() const { + return GetField(VT_IMAGEHEADERSTATUS, 0); + } + const flatbuffers::String *imageLeftPixels() const { + return GetPointer(VT_IMAGELEFTPIXELS); + } + uint32_t imageLeftPixelsVersion() const { + return GetField(VT_IMAGELEFTPIXELSVERSION, 0); + } + int32_t imageLeftStatus() const { + return GetField(VT_IMAGELEFTSTATUS, 0); + } + const flatbuffers::String *imageRightPixels() const { + return GetPointer(VT_IMAGERIGHTPIXELS); + } + uint32_t imageRightPixelsVersion() const { + return GetField(VT_IMAGERIGHTPIXELSVERSION, 0); + } + int32_t imageRightStatus() const { + return GetField(VT_IMAGERIGHTSTATUS, 0); + } + const flatbuffers::Vector> *regionsOfInterestLeft() const { + return GetPointer> *>(VT_REGIONSOFINTERESTLEFT); + } + uint32_t regionsOfInterestLeftVersion() const { + return GetField(VT_REGIONSOFINTERESTLEFTVERSION, 0); + } + int32_t regionsOfInterestLeftStatus() const { + return GetField(VT_REGIONSOFINTERESTLEFTSTATUS, 0); + } + const flatbuffers::Vector> *regionsOfInterestRight() const { + return GetPointer> *>(VT_REGIONSOFINTERESTRIGHT); + } + uint32_t regionsOfInterestRightVersion() const { + return GetField(VT_REGIONSOFINTERESTRIGHTVERSION, 0); + } + int32_t regionsOfInterestRightStatus() const { + return GetField(VT_REGIONSOFINTERESTRIGHTSTATUS, 0); + } + const flatbuffers::Vector> *threeDFiducials() const { + return GetPointer> *>(VT_THREEDFIDUCIALS); + } + uint32_t threeDFiducialsVersion() const { + return GetField(VT_THREEDFIDUCIALSVERSION, 0); + } + int32_t threeDFiducialsStatus() const { + return GetField(VT_THREEDFIDUCIALSSTATUS, 0); + } + const flatbuffers::Vector> *markers() const { + return GetPointer> *>(VT_MARKERS); + } + uint32_t markersVersion() const { + return GetField(VT_MARKERSVERSION, 0); + } + int32_t markersStatus() const { + return GetField(VT_MARKERSSTATUS, 0); + } + int32_t deviceType() const { + return GetField(VT_DEVICETYPE, 0); + } + int64_t ftkError() const { + return GetField(VT_FTKERROR, 0); + } + bool Verify(flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + VerifyField(verifier, VT_TIMESTAMP) && + VerifyField(verifier, VT_SERIALNUMBER) && + VerifyField(verifier, VT_HARDWARETIMESTAMPUS) && + VerifyField(verifier, VT_DESYNCHROUS) && + VerifyField(verifier, VT_COUNTER) && + VerifyField(verifier, VT_FORMAT) && + VerifyField(verifier, VT_WIDTH) && + VerifyField(verifier, VT_HEIGHT) && + VerifyField(verifier, VT_IMAGESTRIDEINBYTES) && + VerifyField(verifier, VT_IMAGEHEADERVERSION) && + VerifyField(verifier, VT_IMAGEHEADERSTATUS) && + VerifyOffset(verifier, VT_IMAGELEFTPIXELS) && + verifier.Verify(imageLeftPixels()) && + VerifyField(verifier, VT_IMAGELEFTPIXELSVERSION) && + VerifyField(verifier, VT_IMAGELEFTSTATUS) && + VerifyOffset(verifier, VT_IMAGERIGHTPIXELS) && + verifier.Verify(imageRightPixels()) && + VerifyField(verifier, VT_IMAGERIGHTPIXELSVERSION) && + VerifyField(verifier, VT_IMAGERIGHTSTATUS) && + VerifyOffset(verifier, VT_REGIONSOFINTERESTLEFT) && + verifier.Verify(regionsOfInterestLeft()) && + verifier.VerifyVectorOfTables(regionsOfInterestLeft()) && + VerifyField(verifier, VT_REGIONSOFINTERESTLEFTVERSION) && + VerifyField(verifier, VT_REGIONSOFINTERESTLEFTSTATUS) && + VerifyOffset(verifier, VT_REGIONSOFINTERESTRIGHT) && + verifier.Verify(regionsOfInterestRight()) && + verifier.VerifyVectorOfTables(regionsOfInterestRight()) && + VerifyField(verifier, VT_REGIONSOFINTERESTRIGHTVERSION) && + VerifyField(verifier, VT_REGIONSOFINTERESTRIGHTSTATUS) && + VerifyOffset(verifier, VT_THREEDFIDUCIALS) && + verifier.Verify(threeDFiducials()) && + verifier.VerifyVectorOfTables(threeDFiducials()) && + VerifyField(verifier, VT_THREEDFIDUCIALSVERSION) && + VerifyField(verifier, VT_THREEDFIDUCIALSSTATUS) && + VerifyOffset(verifier, VT_MARKERS) && + verifier.Verify(markers()) && + verifier.VerifyVectorOfTables(markers()) && + VerifyField(verifier, VT_MARKERSVERSION) && + VerifyField(verifier, VT_MARKERSSTATUS) && + VerifyField(verifier, VT_DEVICETYPE) && + VerifyField(verifier, VT_FTKERROR) && + verifier.EndTable(); + } + FusionTrackFrameT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; + void UnPackTo(FusionTrackFrameT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; + static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const FusionTrackFrameT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); +}; + +struct FusionTrackFrameBuilder { + flatbuffers::FlatBufferBuilder &fbb_; + flatbuffers::uoffset_t start_; + void add_timestamp(double timestamp) { + fbb_.AddElement(FusionTrackFrame::VT_TIMESTAMP, timestamp, 0.0); + } + void add_serialNumber(uint64_t serialNumber) { + fbb_.AddElement(FusionTrackFrame::VT_SERIALNUMBER, serialNumber, 0); + } + void add_hardwareTimestampUS(uint64_t hardwareTimestampUS) { + fbb_.AddElement(FusionTrackFrame::VT_HARDWARETIMESTAMPUS, hardwareTimestampUS, 0); + } + void add_desynchroUS(uint64_t desynchroUS) { + fbb_.AddElement(FusionTrackFrame::VT_DESYNCHROUS, desynchroUS, 0); + } + void add_counter(uint32_t counter) { + fbb_.AddElement(FusionTrackFrame::VT_COUNTER, counter, 0); + } + void add_format(uint32_t format) { + fbb_.AddElement(FusionTrackFrame::VT_FORMAT, format, 0); + } + void add_width(uint32_t width) { + fbb_.AddElement(FusionTrackFrame::VT_WIDTH, width, 0); + } + void add_height(uint32_t height) { + fbb_.AddElement(FusionTrackFrame::VT_HEIGHT, height, 0); + } + void add_imageStrideInBytes(int32_t imageStrideInBytes) { + fbb_.AddElement(FusionTrackFrame::VT_IMAGESTRIDEINBYTES, imageStrideInBytes, 0); + } + void add_imageHeaderVersion(uint32_t imageHeaderVersion) { + fbb_.AddElement(FusionTrackFrame::VT_IMAGEHEADERVERSION, imageHeaderVersion, 0); + } + void add_imageHeaderStatus(int32_t imageHeaderStatus) { + fbb_.AddElement(FusionTrackFrame::VT_IMAGEHEADERSTATUS, imageHeaderStatus, 0); + } + void add_imageLeftPixels(flatbuffers::Offset imageLeftPixels) { + fbb_.AddOffset(FusionTrackFrame::VT_IMAGELEFTPIXELS, imageLeftPixels); + } + void add_imageLeftPixelsVersion(uint32_t imageLeftPixelsVersion) { + fbb_.AddElement(FusionTrackFrame::VT_IMAGELEFTPIXELSVERSION, imageLeftPixelsVersion, 0); + } + void add_imageLeftStatus(int32_t imageLeftStatus) { + fbb_.AddElement(FusionTrackFrame::VT_IMAGELEFTSTATUS, imageLeftStatus, 0); + } + void add_imageRightPixels(flatbuffers::Offset imageRightPixels) { + fbb_.AddOffset(FusionTrackFrame::VT_IMAGERIGHTPIXELS, imageRightPixels); + } + void add_imageRightPixelsVersion(uint32_t imageRightPixelsVersion) { + fbb_.AddElement(FusionTrackFrame::VT_IMAGERIGHTPIXELSVERSION, imageRightPixelsVersion, 0); + } + void add_imageRightStatus(int32_t imageRightStatus) { + fbb_.AddElement(FusionTrackFrame::VT_IMAGERIGHTSTATUS, imageRightStatus, 0); + } + void add_regionsOfInterestLeft(flatbuffers::Offset>> regionsOfInterestLeft) { + fbb_.AddOffset(FusionTrackFrame::VT_REGIONSOFINTERESTLEFT, regionsOfInterestLeft); + } + void add_regionsOfInterestLeftVersion(uint32_t regionsOfInterestLeftVersion) { + fbb_.AddElement(FusionTrackFrame::VT_REGIONSOFINTERESTLEFTVERSION, regionsOfInterestLeftVersion, 0); + } + void add_regionsOfInterestLeftStatus(int32_t regionsOfInterestLeftStatus) { + fbb_.AddElement(FusionTrackFrame::VT_REGIONSOFINTERESTLEFTSTATUS, regionsOfInterestLeftStatus, 0); + } + void add_regionsOfInterestRight(flatbuffers::Offset>> regionsOfInterestRight) { + fbb_.AddOffset(FusionTrackFrame::VT_REGIONSOFINTERESTRIGHT, regionsOfInterestRight); + } + void add_regionsOfInterestRightVersion(uint32_t regionsOfInterestRightVersion) { + fbb_.AddElement(FusionTrackFrame::VT_REGIONSOFINTERESTRIGHTVERSION, regionsOfInterestRightVersion, 0); + } + void add_regionsOfInterestRightStatus(int32_t regionsOfInterestRightStatus) { + fbb_.AddElement(FusionTrackFrame::VT_REGIONSOFINTERESTRIGHTSTATUS, regionsOfInterestRightStatus, 0); + } + void add_threeDFiducials(flatbuffers::Offset>> threeDFiducials) { + fbb_.AddOffset(FusionTrackFrame::VT_THREEDFIDUCIALS, threeDFiducials); + } + void add_threeDFiducialsVersion(uint32_t threeDFiducialsVersion) { + fbb_.AddElement(FusionTrackFrame::VT_THREEDFIDUCIALSVERSION, threeDFiducialsVersion, 0); + } + void add_threeDFiducialsStatus(int32_t threeDFiducialsStatus) { + fbb_.AddElement(FusionTrackFrame::VT_THREEDFIDUCIALSSTATUS, threeDFiducialsStatus, 0); + } + void add_markers(flatbuffers::Offset>> markers) { + fbb_.AddOffset(FusionTrackFrame::VT_MARKERS, markers); + } + void add_markersVersion(uint32_t markersVersion) { + fbb_.AddElement(FusionTrackFrame::VT_MARKERSVERSION, markersVersion, 0); + } + void add_markersStatus(int32_t markersStatus) { + fbb_.AddElement(FusionTrackFrame::VT_MARKERSSTATUS, markersStatus, 0); + } + void add_deviceType(int32_t deviceType) { + fbb_.AddElement(FusionTrackFrame::VT_DEVICETYPE, deviceType, 0); + } + void add_ftkError(int64_t ftkError) { + fbb_.AddElement(FusionTrackFrame::VT_FTKERROR, ftkError, 0); + } + explicit FusionTrackFrameBuilder(flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + FusionTrackFrameBuilder &operator=(const FusionTrackFrameBuilder &); + flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = flatbuffers::Offset(end); + return o; + } +}; + +inline flatbuffers::Offset CreateFusionTrackFrame( + flatbuffers::FlatBufferBuilder &_fbb, + double timestamp = 0.0, + uint64_t serialNumber = 0, + uint64_t hardwareTimestampUS = 0, + uint64_t desynchroUS = 0, + uint32_t counter = 0, + uint32_t format = 0, + uint32_t width = 0, + uint32_t height = 0, + int32_t imageStrideInBytes = 0, + uint32_t imageHeaderVersion = 0, + int32_t imageHeaderStatus = 0, + flatbuffers::Offset imageLeftPixels = 0, + uint32_t imageLeftPixelsVersion = 0, + int32_t imageLeftStatus = 0, + flatbuffers::Offset imageRightPixels = 0, + uint32_t imageRightPixelsVersion = 0, + int32_t imageRightStatus = 0, + flatbuffers::Offset>> regionsOfInterestLeft = 0, + uint32_t regionsOfInterestLeftVersion = 0, + int32_t regionsOfInterestLeftStatus = 0, + flatbuffers::Offset>> regionsOfInterestRight = 0, + uint32_t regionsOfInterestRightVersion = 0, + int32_t regionsOfInterestRightStatus = 0, + flatbuffers::Offset>> threeDFiducials = 0, + uint32_t threeDFiducialsVersion = 0, + int32_t threeDFiducialsStatus = 0, + flatbuffers::Offset>> markers = 0, + uint32_t markersVersion = 0, + int32_t markersStatus = 0, + int32_t deviceType = 0, + int64_t ftkError = 0) { + FusionTrackFrameBuilder builder_(_fbb); + builder_.add_ftkError(ftkError); + builder_.add_desynchroUS(desynchroUS); + builder_.add_hardwareTimestampUS(hardwareTimestampUS); + builder_.add_serialNumber(serialNumber); + builder_.add_timestamp(timestamp); + builder_.add_deviceType(deviceType); + builder_.add_markersStatus(markersStatus); + builder_.add_markersVersion(markersVersion); + builder_.add_markers(markers); + builder_.add_threeDFiducialsStatus(threeDFiducialsStatus); + builder_.add_threeDFiducialsVersion(threeDFiducialsVersion); + builder_.add_threeDFiducials(threeDFiducials); + builder_.add_regionsOfInterestRightStatus(regionsOfInterestRightStatus); + builder_.add_regionsOfInterestRightVersion(regionsOfInterestRightVersion); + builder_.add_regionsOfInterestRight(regionsOfInterestRight); + builder_.add_regionsOfInterestLeftStatus(regionsOfInterestLeftStatus); + builder_.add_regionsOfInterestLeftVersion(regionsOfInterestLeftVersion); + builder_.add_regionsOfInterestLeft(regionsOfInterestLeft); + builder_.add_imageRightStatus(imageRightStatus); + builder_.add_imageRightPixelsVersion(imageRightPixelsVersion); + builder_.add_imageRightPixels(imageRightPixels); + builder_.add_imageLeftStatus(imageLeftStatus); + builder_.add_imageLeftPixelsVersion(imageLeftPixelsVersion); + builder_.add_imageLeftPixels(imageLeftPixels); + builder_.add_imageHeaderStatus(imageHeaderStatus); + builder_.add_imageHeaderVersion(imageHeaderVersion); + builder_.add_imageStrideInBytes(imageStrideInBytes); + builder_.add_height(height); + builder_.add_width(width); + builder_.add_format(format); + builder_.add_counter(counter); + return builder_.Finish(); +} + +inline flatbuffers::Offset CreateFusionTrackFrameDirect( + flatbuffers::FlatBufferBuilder &_fbb, + double timestamp = 0.0, + uint64_t serialNumber = 0, + uint64_t hardwareTimestampUS = 0, + uint64_t desynchroUS = 0, + uint32_t counter = 0, + uint32_t format = 0, + uint32_t width = 0, + uint32_t height = 0, + int32_t imageStrideInBytes = 0, + uint32_t imageHeaderVersion = 0, + int32_t imageHeaderStatus = 0, + const char *imageLeftPixels = nullptr, + uint32_t imageLeftPixelsVersion = 0, + int32_t imageLeftStatus = 0, + const char *imageRightPixels = nullptr, + uint32_t imageRightPixelsVersion = 0, + int32_t imageRightStatus = 0, + const std::vector> *regionsOfInterestLeft = nullptr, + uint32_t regionsOfInterestLeftVersion = 0, + int32_t regionsOfInterestLeftStatus = 0, + const std::vector> *regionsOfInterestRight = nullptr, + uint32_t regionsOfInterestRightVersion = 0, + int32_t regionsOfInterestRightStatus = 0, + const std::vector> *threeDFiducials = nullptr, + uint32_t threeDFiducialsVersion = 0, + int32_t threeDFiducialsStatus = 0, + const std::vector> *markers = nullptr, + uint32_t markersVersion = 0, + int32_t markersStatus = 0, + int32_t deviceType = 0, + int64_t ftkError = 0) { + return grl::flatbuffer::CreateFusionTrackFrame( + _fbb, + timestamp, + serialNumber, + hardwareTimestampUS, + desynchroUS, + counter, + format, + width, + height, + imageStrideInBytes, + imageHeaderVersion, + imageHeaderStatus, + imageLeftPixels ? _fbb.CreateString(imageLeftPixels) : 0, + imageLeftPixelsVersion, + imageLeftStatus, + imageRightPixels ? _fbb.CreateString(imageRightPixels) : 0, + imageRightPixelsVersion, + imageRightStatus, + regionsOfInterestLeft ? _fbb.CreateVector>(*regionsOfInterestLeft) : 0, + regionsOfInterestLeftVersion, + regionsOfInterestLeftStatus, + regionsOfInterestRight ? _fbb.CreateVector>(*regionsOfInterestRight) : 0, + regionsOfInterestRightVersion, + regionsOfInterestRightStatus, + threeDFiducials ? _fbb.CreateVector>(*threeDFiducials) : 0, + threeDFiducialsVersion, + threeDFiducialsStatus, + markers ? _fbb.CreateVector>(*markers) : 0, + markersVersion, + markersStatus, + deviceType, + ftkError); +} + +flatbuffers::Offset CreateFusionTrackFrame(flatbuffers::FlatBufferBuilder &_fbb, const FusionTrackFrameT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); + +struct FusionTrackParametersT : public flatbuffers::NativeTable { + typedef FusionTrackParameters TableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.FusionTrackParametersT"; + } + std::string name; + std::string deviceClockID; + std::string localClockID; + std::vector> geometries; + std::vector geometryFilenames; + std::string geometryDir; + std::vector TrackerDeviceIDs; + std::vector markerIDs; + std::vector markerNames; + std::vector m_deviceSerialNumbers; + std::vector m_device_types; + FusionTrackParametersT() { + } +}; + +struct FusionTrackParameters FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { + typedef FusionTrackParametersT NativeTableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.FusionTrackParameters"; + } + enum { + VT_NAME = 4, + VT_DEVICECLOCKID = 6, + VT_LOCALCLOCKID = 8, + VT_GEOMETRIES = 10, + VT_GEOMETRYFILENAMES = 12, + VT_GEOMETRYDIR = 14, + VT_TRACKERDEVICEIDS = 16, + VT_MARKERIDS = 18, + VT_MARKERNAMES = 20, + VT_M_DEVICESERIALNUMBERS = 22, + VT_M_DEVICE_TYPES = 24 + }; + /// Name for this connection / FusionTrack driver instance + /// useful for debugging and when multiple data sources are used + const flatbuffers::String *name() const { + return GetPointer(VT_NAME); + } + /// Name for the clock on the FusionTrack + /// Useful for timing calculations and debugging. + const flatbuffers::String *deviceClockID() const { + return GetPointer(VT_DEVICECLOCKID); + } + /// Name for the local clock on which this driver runs + /// Useful for timing calculations and debugging. + const flatbuffers::String *localClockID() const { + return GetPointer(VT_LOCALCLOCKID); + } + /// dimensions of the markers that may be present + const flatbuffers::Vector> *geometries() const { + return GetPointer> *>(VT_GEOMETRIES); + } + /// Geometries aka fiducials aka markers to be loaded from ini files. + /// The data loaded should not repeat IDs from MarkerIDs. + const flatbuffers::Vector> *geometryFilenames() const { + return GetPointer> *>(VT_GEOMETRYFILENAMES); + } + /// Path to the directory with the marker ini files listed above + /// Uses the default current working directory if empty + /// geometryDir:[string]; + const flatbuffers::String *geometryDir() const { + return GetPointer(VT_GEOMETRYDIR); + } + /// Optional list of optical tracker device ids to expect + /// will be loaded automatically if empty + const flatbuffers::Vector *TrackerDeviceIDs() const { + return GetPointer *>(VT_TRACKERDEVICEIDS); + } + /// Marker geometry unique integer IDs + const flatbuffers::Vector *markerIDs() const { + return GetPointer *>(VT_MARKERIDS); + } + /// Optional Marker geometry names with one for each ID, none otherwise + const flatbuffers::Vector> *markerNames() const { + return GetPointer> *>(VT_MARKERNAMES); + } + const flatbuffers::Vector *m_deviceSerialNumbers() const { + return GetPointer *>(VT_M_DEVICESERIALNUMBERS); + } + const flatbuffers::Vector *m_device_types() const { + return GetPointer *>(VT_M_DEVICE_TYPES); + } + bool Verify(flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + VerifyOffset(verifier, VT_NAME) && + verifier.Verify(name()) && + VerifyOffset(verifier, VT_DEVICECLOCKID) && + verifier.Verify(deviceClockID()) && + VerifyOffset(verifier, VT_LOCALCLOCKID) && + verifier.Verify(localClockID()) && + VerifyOffset(verifier, VT_GEOMETRIES) && + verifier.Verify(geometries()) && + verifier.VerifyVectorOfTables(geometries()) && + VerifyOffset(verifier, VT_GEOMETRYFILENAMES) && + verifier.Verify(geometryFilenames()) && + verifier.VerifyVectorOfStrings(geometryFilenames()) && + VerifyOffset(verifier, VT_GEOMETRYDIR) && + verifier.Verify(geometryDir()) && + VerifyOffset(verifier, VT_TRACKERDEVICEIDS) && + verifier.Verify(TrackerDeviceIDs()) && + VerifyOffset(verifier, VT_MARKERIDS) && + verifier.Verify(markerIDs()) && + VerifyOffset(verifier, VT_MARKERNAMES) && + verifier.Verify(markerNames()) && + verifier.VerifyVectorOfStrings(markerNames()) && + VerifyOffset(verifier, VT_M_DEVICESERIALNUMBERS) && + verifier.Verify(m_deviceSerialNumbers()) && + VerifyOffset(verifier, VT_M_DEVICE_TYPES) && + verifier.Verify(m_device_types()) && + verifier.EndTable(); + } + FusionTrackParametersT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; + void UnPackTo(FusionTrackParametersT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; + static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const FusionTrackParametersT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); +}; + +struct FusionTrackParametersBuilder { + flatbuffers::FlatBufferBuilder &fbb_; + flatbuffers::uoffset_t start_; + void add_name(flatbuffers::Offset name) { + fbb_.AddOffset(FusionTrackParameters::VT_NAME, name); + } + void add_deviceClockID(flatbuffers::Offset deviceClockID) { + fbb_.AddOffset(FusionTrackParameters::VT_DEVICECLOCKID, deviceClockID); + } + void add_localClockID(flatbuffers::Offset localClockID) { + fbb_.AddOffset(FusionTrackParameters::VT_LOCALCLOCKID, localClockID); + } + void add_geometries(flatbuffers::Offset>> geometries) { + fbb_.AddOffset(FusionTrackParameters::VT_GEOMETRIES, geometries); + } + void add_geometryFilenames(flatbuffers::Offset>> geometryFilenames) { + fbb_.AddOffset(FusionTrackParameters::VT_GEOMETRYFILENAMES, geometryFilenames); + } + void add_geometryDir(flatbuffers::Offset geometryDir) { + fbb_.AddOffset(FusionTrackParameters::VT_GEOMETRYDIR, geometryDir); + } + void add_TrackerDeviceIDs(flatbuffers::Offset> TrackerDeviceIDs) { + fbb_.AddOffset(FusionTrackParameters::VT_TRACKERDEVICEIDS, TrackerDeviceIDs); + } + void add_markerIDs(flatbuffers::Offset> markerIDs) { + fbb_.AddOffset(FusionTrackParameters::VT_MARKERIDS, markerIDs); + } + void add_markerNames(flatbuffers::Offset>> markerNames) { + fbb_.AddOffset(FusionTrackParameters::VT_MARKERNAMES, markerNames); + } + void add_m_deviceSerialNumbers(flatbuffers::Offset> m_deviceSerialNumbers) { + fbb_.AddOffset(FusionTrackParameters::VT_M_DEVICESERIALNUMBERS, m_deviceSerialNumbers); + } + void add_m_device_types(flatbuffers::Offset> m_device_types) { + fbb_.AddOffset(FusionTrackParameters::VT_M_DEVICE_TYPES, m_device_types); + } + explicit FusionTrackParametersBuilder(flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + FusionTrackParametersBuilder &operator=(const FusionTrackParametersBuilder &); + flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = flatbuffers::Offset(end); + return o; + } +}; + +inline flatbuffers::Offset CreateFusionTrackParameters( + flatbuffers::FlatBufferBuilder &_fbb, + flatbuffers::Offset name = 0, + flatbuffers::Offset deviceClockID = 0, + flatbuffers::Offset localClockID = 0, + flatbuffers::Offset>> geometries = 0, + flatbuffers::Offset>> geometryFilenames = 0, + flatbuffers::Offset geometryDir = 0, + flatbuffers::Offset> TrackerDeviceIDs = 0, + flatbuffers::Offset> markerIDs = 0, + flatbuffers::Offset>> markerNames = 0, + flatbuffers::Offset> m_deviceSerialNumbers = 0, + flatbuffers::Offset> m_device_types = 0) { + FusionTrackParametersBuilder builder_(_fbb); + builder_.add_m_device_types(m_device_types); + builder_.add_m_deviceSerialNumbers(m_deviceSerialNumbers); + builder_.add_markerNames(markerNames); + builder_.add_markerIDs(markerIDs); + builder_.add_TrackerDeviceIDs(TrackerDeviceIDs); + builder_.add_geometryDir(geometryDir); + builder_.add_geometryFilenames(geometryFilenames); + builder_.add_geometries(geometries); + builder_.add_localClockID(localClockID); + builder_.add_deviceClockID(deviceClockID); + builder_.add_name(name); + return builder_.Finish(); +} + +inline flatbuffers::Offset CreateFusionTrackParametersDirect( + flatbuffers::FlatBufferBuilder &_fbb, + const char *name = nullptr, + const char *deviceClockID = nullptr, + const char *localClockID = nullptr, + const std::vector> *geometries = nullptr, + const std::vector> *geometryFilenames = nullptr, + const char *geometryDir = nullptr, + const std::vector *TrackerDeviceIDs = nullptr, + const std::vector *markerIDs = nullptr, + const std::vector> *markerNames = nullptr, + const std::vector *m_deviceSerialNumbers = nullptr, + const std::vector *m_device_types = nullptr) { + return grl::flatbuffer::CreateFusionTrackParameters( + _fbb, + name ? _fbb.CreateString(name) : 0, + deviceClockID ? _fbb.CreateString(deviceClockID) : 0, + localClockID ? _fbb.CreateString(localClockID) : 0, + geometries ? _fbb.CreateVector>(*geometries) : 0, + geometryFilenames ? _fbb.CreateVector>(*geometryFilenames) : 0, + geometryDir ? _fbb.CreateString(geometryDir) : 0, + TrackerDeviceIDs ? _fbb.CreateVector(*TrackerDeviceIDs) : 0, + markerIDs ? _fbb.CreateVector(*markerIDs) : 0, + markerNames ? _fbb.CreateVector>(*markerNames) : 0, + m_deviceSerialNumbers ? _fbb.CreateVector(*m_deviceSerialNumbers) : 0, + m_device_types ? _fbb.CreateVector(*m_device_types) : 0); +} + +flatbuffers::Offset CreateFusionTrackParameters(flatbuffers::FlatBufferBuilder &_fbb, const FusionTrackParametersT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); + +struct FusionTrackMessageT : public flatbuffers::NativeTable { + typedef FusionTrackMessage TableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.FusionTrackMessageT"; + } + double timestamp; + std::unique_ptr parameters; + std::unique_ptr timeEvent; + std::unique_ptr frame; + FusionTrackMessageT() + : timestamp(0.0) { + } +}; + +struct FusionTrackMessage FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { + typedef FusionTrackMessageT NativeTableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.FusionTrackMessage"; + } + enum { + VT_TIMESTAMP = 4, + VT_PARAMETERS = 6, + VT_TIMEEVENT = 8, + VT_FRAME = 10 + }; + double timestamp() const { + return GetField(VT_TIMESTAMP, 0.0); + } + const FusionTrackParameters *parameters() const { + return GetPointer(VT_PARAMETERS); + } + const TimeEvent *timeEvent() const { + return GetPointer(VT_TIMEEVENT); + } + const FusionTrackFrame *frame() const { + return GetPointer(VT_FRAME); + } + bool Verify(flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + VerifyField(verifier, VT_TIMESTAMP) && + VerifyOffset(verifier, VT_PARAMETERS) && + verifier.VerifyTable(parameters()) && + VerifyOffset(verifier, VT_TIMEEVENT) && + verifier.VerifyTable(timeEvent()) && + VerifyOffset(verifier, VT_FRAME) && + verifier.VerifyTable(frame()) && + verifier.EndTable(); + } + FusionTrackMessageT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; + void UnPackTo(FusionTrackMessageT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; + static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const FusionTrackMessageT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); +}; + +struct FusionTrackMessageBuilder { + flatbuffers::FlatBufferBuilder &fbb_; + flatbuffers::uoffset_t start_; + void add_timestamp(double timestamp) { + fbb_.AddElement(FusionTrackMessage::VT_TIMESTAMP, timestamp, 0.0); + } + void add_parameters(flatbuffers::Offset parameters) { + fbb_.AddOffset(FusionTrackMessage::VT_PARAMETERS, parameters); + } + void add_timeEvent(flatbuffers::Offset timeEvent) { + fbb_.AddOffset(FusionTrackMessage::VT_TIMEEVENT, timeEvent); + } + void add_frame(flatbuffers::Offset frame) { + fbb_.AddOffset(FusionTrackMessage::VT_FRAME, frame); + } + explicit FusionTrackMessageBuilder(flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + FusionTrackMessageBuilder &operator=(const FusionTrackMessageBuilder &); + flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = flatbuffers::Offset(end); + return o; + } +}; + +inline flatbuffers::Offset CreateFusionTrackMessage( + flatbuffers::FlatBufferBuilder &_fbb, + double timestamp = 0.0, + flatbuffers::Offset parameters = 0, + flatbuffers::Offset timeEvent = 0, + flatbuffers::Offset frame = 0) { + FusionTrackMessageBuilder builder_(_fbb); + builder_.add_timestamp(timestamp); + builder_.add_frame(frame); + builder_.add_timeEvent(timeEvent); + builder_.add_parameters(parameters); + return builder_.Finish(); +} + +flatbuffers::Offset CreateFusionTrackMessage(flatbuffers::FlatBufferBuilder &_fbb, const FusionTrackMessageT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); + +inline ftkGeometryT *ftkGeometry::UnPack(const flatbuffers::resolver_function_t *_resolver) const { + auto _o = new ftkGeometryT(); + UnPackTo(_o, _resolver); + return _o; +} + +inline void ftkGeometry::UnPackTo(ftkGeometryT *_o, const flatbuffers::resolver_function_t *_resolver) const { + (void)_o; + (void)_resolver; + { auto _e = name(); if (_e) _o->name = _e->str(); }; + { auto _e = geometryID(); _o->geometryID = _e; }; + { auto _e = version(); _o->version = _e; }; + { auto _e = positions(); if (_e) { _o->positions.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->positions[_i] = *_e->Get(_i); } } }; +} + +inline flatbuffers::Offset ftkGeometry::Pack(flatbuffers::FlatBufferBuilder &_fbb, const ftkGeometryT* _o, const flatbuffers::rehasher_function_t *_rehasher) { + return CreateftkGeometry(_fbb, _o, _rehasher); +} + +inline flatbuffers::Offset CreateftkGeometry(flatbuffers::FlatBufferBuilder &_fbb, const ftkGeometryT *_o, const flatbuffers::rehasher_function_t *_rehasher) { + (void)_rehasher; + (void)_o; + struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const ftkGeometryT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; + auto _name = _o->name.empty() ? 0 : _fbb.CreateString(_o->name); + auto _geometryID = _o->geometryID; + auto _version = _o->version; + auto _positions = _o->positions.size() ? _fbb.CreateVectorOfStructs(_o->positions) : 0; + return grl::flatbuffer::CreateftkGeometry( + _fbb, + _name, + _geometryID, + _version, + _positions); +} + +inline ftkMarkerT *ftkMarker::UnPack(const flatbuffers::resolver_function_t *_resolver) const { + auto _o = new ftkMarkerT(); + UnPackTo(_o, _resolver); + return _o; +} + +inline void ftkMarker::UnPackTo(ftkMarkerT *_o, const flatbuffers::resolver_function_t *_resolver) const { + (void)_o; + (void)_resolver; + { auto _e = name(); if (_e) _o->name = _e->str(); }; + { auto _e = ID(); _o->ID = _e; }; + { auto _e = geometryID(); _o->geometryID = _e; }; + { auto _e = geometryPresenceMask(); if (_e) { _o->geometryPresenceMask.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->geometryPresenceMask[_i] = _e->Get(_i); } } }; + { auto _e = transform(); if (_e) _o->transform = std::unique_ptr(new Pose(*_e)); }; +} + +inline flatbuffers::Offset ftkMarker::Pack(flatbuffers::FlatBufferBuilder &_fbb, const ftkMarkerT* _o, const flatbuffers::rehasher_function_t *_rehasher) { + return CreateftkMarker(_fbb, _o, _rehasher); +} + +inline flatbuffers::Offset CreateftkMarker(flatbuffers::FlatBufferBuilder &_fbb, const ftkMarkerT *_o, const flatbuffers::rehasher_function_t *_rehasher) { + (void)_rehasher; + (void)_o; + struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const ftkMarkerT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; + auto _name = _o->name.empty() ? 0 : _fbb.CreateString(_o->name); + auto _ID = _o->ID; + auto _geometryID = _o->geometryID; + auto _geometryPresenceMask = _o->geometryPresenceMask.size() ? _fbb.CreateVector(_o->geometryPresenceMask) : 0; + auto _transform = _o->transform ? _o->transform.get() : 0; + return grl::flatbuffer::CreateftkMarker( + _fbb, + _name, + _ID, + _geometryID, + _geometryPresenceMask, + _transform); +} + +inline ftk3DFiducialT *ftk3DFiducial::UnPack(const flatbuffers::resolver_function_t *_resolver) const { + auto _o = new ftk3DFiducialT(); + UnPackTo(_o, _resolver); + return _o; +} + +inline void ftk3DFiducial::UnPackTo(ftk3DFiducialT *_o, const flatbuffers::resolver_function_t *_resolver) const { + (void)_o; + (void)_resolver; + { auto _e = markerID(); _o->markerID = _e; }; + { auto _e = leftIndex(); _o->leftIndex = _e; }; + { auto _e = rightIndex(); _o->rightIndex = _e; }; + { auto _e = position(); if (_e) _o->position = std::unique_ptr(new Vector3d(*_e)); }; + { auto _e = epipolarErrorPixels(); _o->epipolarErrorPixels = _e; }; + { auto _e = triangulationError(); _o->triangulationError = _e; }; + { auto _e = probability(); _o->probability = _e; }; +} + +inline flatbuffers::Offset ftk3DFiducial::Pack(flatbuffers::FlatBufferBuilder &_fbb, const ftk3DFiducialT* _o, const flatbuffers::rehasher_function_t *_rehasher) { + return Createftk3DFiducial(_fbb, _o, _rehasher); +} + +inline flatbuffers::Offset Createftk3DFiducial(flatbuffers::FlatBufferBuilder &_fbb, const ftk3DFiducialT *_o, const flatbuffers::rehasher_function_t *_rehasher) { + (void)_rehasher; + (void)_o; + struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const ftk3DFiducialT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; + auto _markerID = _o->markerID; + auto _leftIndex = _o->leftIndex; + auto _rightIndex = _o->rightIndex; + auto _position = _o->position ? _o->position.get() : 0; + auto _epipolarErrorPixels = _o->epipolarErrorPixels; + auto _triangulationError = _o->triangulationError; + auto _probability = _o->probability; + return grl::flatbuffer::Createftk3DFiducial( + _fbb, + _markerID, + _leftIndex, + _rightIndex, + _position, + _epipolarErrorPixels, + _triangulationError, + _probability); +} + +inline ftkRegionOfInterestT *ftkRegionOfInterest::UnPack(const flatbuffers::resolver_function_t *_resolver) const { + auto _o = new ftkRegionOfInterestT(); + UnPackTo(_o, _resolver); + return _o; +} + +inline void ftkRegionOfInterest::UnPackTo(ftkRegionOfInterestT *_o, const flatbuffers::resolver_function_t *_resolver) const { + (void)_o; + (void)_resolver; + { auto _e = centerXPixels(); _o->centerXPixels = _e; }; + { auto _e = centerYPixels(); _o->centerYPixels = _e; }; + { auto _e = RightEdge(); _o->RightEdge = _e; }; + { auto _e = BottomEdge(); _o->BottomEdge = _e; }; + { auto _e = LeftEdge(); _o->LeftEdge = _e; }; + { auto _e = TopEdge(); _o->TopEdge = _e; }; + { auto _e = pixelsCount(); _o->pixelsCount = _e; }; + { auto _e = probability(); _o->probability = _e; }; +} + +inline flatbuffers::Offset ftkRegionOfInterest::Pack(flatbuffers::FlatBufferBuilder &_fbb, const ftkRegionOfInterestT* _o, const flatbuffers::rehasher_function_t *_rehasher) { + return CreateftkRegionOfInterest(_fbb, _o, _rehasher); +} + +inline flatbuffers::Offset CreateftkRegionOfInterest(flatbuffers::FlatBufferBuilder &_fbb, const ftkRegionOfInterestT *_o, const flatbuffers::rehasher_function_t *_rehasher) { + (void)_rehasher; + (void)_o; + struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const ftkRegionOfInterestT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; + auto _centerXPixels = _o->centerXPixels; + auto _centerYPixels = _o->centerYPixels; + auto _RightEdge = _o->RightEdge; + auto _BottomEdge = _o->BottomEdge; + auto _LeftEdge = _o->LeftEdge; + auto _TopEdge = _o->TopEdge; + auto _pixelsCount = _o->pixelsCount; + auto _probability = _o->probability; + return grl::flatbuffer::CreateftkRegionOfInterest( + _fbb, + _centerXPixels, + _centerYPixels, + _RightEdge, + _BottomEdge, + _LeftEdge, + _TopEdge, + _pixelsCount, + _probability); +} + +inline FusionTrackFrameT *FusionTrackFrame::UnPack(const flatbuffers::resolver_function_t *_resolver) const { + auto _o = new FusionTrackFrameT(); + UnPackTo(_o, _resolver); + return _o; +} + +inline void FusionTrackFrame::UnPackTo(FusionTrackFrameT *_o, const flatbuffers::resolver_function_t *_resolver) const { + (void)_o; + (void)_resolver; + { auto _e = timestamp(); _o->timestamp = _e; }; + { auto _e = serialNumber(); _o->serialNumber = _e; }; + { auto _e = hardwareTimestampUS(); _o->hardwareTimestampUS = _e; }; + { auto _e = desynchroUS(); _o->desynchroUS = _e; }; + { auto _e = counter(); _o->counter = _e; }; + { auto _e = format(); _o->format = _e; }; + { auto _e = width(); _o->width = _e; }; + { auto _e = height(); _o->height = _e; }; + { auto _e = imageStrideInBytes(); _o->imageStrideInBytes = _e; }; + { auto _e = imageHeaderVersion(); _o->imageHeaderVersion = _e; }; + { auto _e = imageHeaderStatus(); _o->imageHeaderStatus = _e; }; + { auto _e = imageLeftPixels(); if (_e) _o->imageLeftPixels = _e->str(); }; + { auto _e = imageLeftPixelsVersion(); _o->imageLeftPixelsVersion = _e; }; + { auto _e = imageLeftStatus(); _o->imageLeftStatus = _e; }; + { auto _e = imageRightPixels(); if (_e) _o->imageRightPixels = _e->str(); }; + { auto _e = imageRightPixelsVersion(); _o->imageRightPixelsVersion = _e; }; + { auto _e = imageRightStatus(); _o->imageRightStatus = _e; }; + { auto _e = regionsOfInterestLeft(); if (_e) { _o->regionsOfInterestLeft.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->regionsOfInterestLeft[_i] = std::unique_ptr(_e->Get(_i)->UnPack(_resolver)); } } }; + { auto _e = regionsOfInterestLeftVersion(); _o->regionsOfInterestLeftVersion = _e; }; + { auto _e = regionsOfInterestLeftStatus(); _o->regionsOfInterestLeftStatus = _e; }; + { auto _e = regionsOfInterestRight(); if (_e) { _o->regionsOfInterestRight.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->regionsOfInterestRight[_i] = std::unique_ptr(_e->Get(_i)->UnPack(_resolver)); } } }; + { auto _e = regionsOfInterestRightVersion(); _o->regionsOfInterestRightVersion = _e; }; + { auto _e = regionsOfInterestRightStatus(); _o->regionsOfInterestRightStatus = _e; }; + { auto _e = threeDFiducials(); if (_e) { _o->threeDFiducials.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->threeDFiducials[_i] = std::unique_ptr(_e->Get(_i)->UnPack(_resolver)); } } }; + { auto _e = threeDFiducialsVersion(); _o->threeDFiducialsVersion = _e; }; + { auto _e = threeDFiducialsStatus(); _o->threeDFiducialsStatus = _e; }; + { auto _e = markers(); if (_e) { _o->markers.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->markers[_i] = std::unique_ptr(_e->Get(_i)->UnPack(_resolver)); } } }; + { auto _e = markersVersion(); _o->markersVersion = _e; }; + { auto _e = markersStatus(); _o->markersStatus = _e; }; + { auto _e = deviceType(); _o->deviceType = _e; }; + { auto _e = ftkError(); _o->ftkError = _e; }; +} + +inline flatbuffers::Offset FusionTrackFrame::Pack(flatbuffers::FlatBufferBuilder &_fbb, const FusionTrackFrameT* _o, const flatbuffers::rehasher_function_t *_rehasher) { + return CreateFusionTrackFrame(_fbb, _o, _rehasher); +} + +inline flatbuffers::Offset CreateFusionTrackFrame(flatbuffers::FlatBufferBuilder &_fbb, const FusionTrackFrameT *_o, const flatbuffers::rehasher_function_t *_rehasher) { + (void)_rehasher; + (void)_o; + struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const FusionTrackFrameT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; + auto _timestamp = _o->timestamp; + auto _serialNumber = _o->serialNumber; + auto _hardwareTimestampUS = _o->hardwareTimestampUS; + auto _desynchroUS = _o->desynchroUS; + auto _counter = _o->counter; + auto _format = _o->format; + auto _width = _o->width; + auto _height = _o->height; + auto _imageStrideInBytes = _o->imageStrideInBytes; + auto _imageHeaderVersion = _o->imageHeaderVersion; + auto _imageHeaderStatus = _o->imageHeaderStatus; + auto _imageLeftPixels = _o->imageLeftPixels.empty() ? 0 : _fbb.CreateString(_o->imageLeftPixels); + auto _imageLeftPixelsVersion = _o->imageLeftPixelsVersion; + auto _imageLeftStatus = _o->imageLeftStatus; + auto _imageRightPixels = _o->imageRightPixels.empty() ? 0 : _fbb.CreateString(_o->imageRightPixels); + auto _imageRightPixelsVersion = _o->imageRightPixelsVersion; + auto _imageRightStatus = _o->imageRightStatus; + auto _regionsOfInterestLeft = _o->regionsOfInterestLeft.size() ? _fbb.CreateVector> (_o->regionsOfInterestLeft.size(), [](size_t i, _VectorArgs *__va) { return CreateftkRegionOfInterest(*__va->__fbb, __va->__o->regionsOfInterestLeft[i].get(), __va->__rehasher); }, &_va ) : 0; + auto _regionsOfInterestLeftVersion = _o->regionsOfInterestLeftVersion; + auto _regionsOfInterestLeftStatus = _o->regionsOfInterestLeftStatus; + auto _regionsOfInterestRight = _o->regionsOfInterestRight.size() ? _fbb.CreateVector> (_o->regionsOfInterestRight.size(), [](size_t i, _VectorArgs *__va) { return CreateftkRegionOfInterest(*__va->__fbb, __va->__o->regionsOfInterestRight[i].get(), __va->__rehasher); }, &_va ) : 0; + auto _regionsOfInterestRightVersion = _o->regionsOfInterestRightVersion; + auto _regionsOfInterestRightStatus = _o->regionsOfInterestRightStatus; + auto _threeDFiducials = _o->threeDFiducials.size() ? _fbb.CreateVector> (_o->threeDFiducials.size(), [](size_t i, _VectorArgs *__va) { return Createftk3DFiducial(*__va->__fbb, __va->__o->threeDFiducials[i].get(), __va->__rehasher); }, &_va ) : 0; + auto _threeDFiducialsVersion = _o->threeDFiducialsVersion; + auto _threeDFiducialsStatus = _o->threeDFiducialsStatus; + auto _markers = _o->markers.size() ? _fbb.CreateVector> (_o->markers.size(), [](size_t i, _VectorArgs *__va) { return CreateftkMarker(*__va->__fbb, __va->__o->markers[i].get(), __va->__rehasher); }, &_va ) : 0; + auto _markersVersion = _o->markersVersion; + auto _markersStatus = _o->markersStatus; + auto _deviceType = _o->deviceType; + auto _ftkError = _o->ftkError; + return grl::flatbuffer::CreateFusionTrackFrame( + _fbb, + _timestamp, + _serialNumber, + _hardwareTimestampUS, + _desynchroUS, + _counter, + _format, + _width, + _height, + _imageStrideInBytes, + _imageHeaderVersion, + _imageHeaderStatus, + _imageLeftPixels, + _imageLeftPixelsVersion, + _imageLeftStatus, + _imageRightPixels, + _imageRightPixelsVersion, + _imageRightStatus, + _regionsOfInterestLeft, + _regionsOfInterestLeftVersion, + _regionsOfInterestLeftStatus, + _regionsOfInterestRight, + _regionsOfInterestRightVersion, + _regionsOfInterestRightStatus, + _threeDFiducials, + _threeDFiducialsVersion, + _threeDFiducialsStatus, + _markers, + _markersVersion, + _markersStatus, + _deviceType, + _ftkError); +} + +inline FusionTrackParametersT *FusionTrackParameters::UnPack(const flatbuffers::resolver_function_t *_resolver) const { + auto _o = new FusionTrackParametersT(); + UnPackTo(_o, _resolver); + return _o; +} + +inline void FusionTrackParameters::UnPackTo(FusionTrackParametersT *_o, const flatbuffers::resolver_function_t *_resolver) const { + (void)_o; + (void)_resolver; + { auto _e = name(); if (_e) _o->name = _e->str(); }; + { auto _e = deviceClockID(); if (_e) _o->deviceClockID = _e->str(); }; + { auto _e = localClockID(); if (_e) _o->localClockID = _e->str(); }; + { auto _e = geometries(); if (_e) { _o->geometries.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->geometries[_i] = std::unique_ptr(_e->Get(_i)->UnPack(_resolver)); } } }; + { auto _e = geometryFilenames(); if (_e) { _o->geometryFilenames.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->geometryFilenames[_i] = _e->Get(_i)->str(); } } }; + { auto _e = geometryDir(); if (_e) _o->geometryDir = _e->str(); }; + { auto _e = TrackerDeviceIDs(); if (_e) { _o->TrackerDeviceIDs.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->TrackerDeviceIDs[_i] = _e->Get(_i); } } }; + { auto _e = markerIDs(); if (_e) { _o->markerIDs.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->markerIDs[_i] = _e->Get(_i); } } }; + { auto _e = markerNames(); if (_e) { _o->markerNames.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->markerNames[_i] = _e->Get(_i)->str(); } } }; + { auto _e = m_deviceSerialNumbers(); if (_e) { _o->m_deviceSerialNumbers.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->m_deviceSerialNumbers[_i] = _e->Get(_i); } } }; + { auto _e = m_device_types(); if (_e) { _o->m_device_types.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->m_device_types[_i] = _e->Get(_i); } } }; +} + +inline flatbuffers::Offset FusionTrackParameters::Pack(flatbuffers::FlatBufferBuilder &_fbb, const FusionTrackParametersT* _o, const flatbuffers::rehasher_function_t *_rehasher) { + return CreateFusionTrackParameters(_fbb, _o, _rehasher); +} + +inline flatbuffers::Offset CreateFusionTrackParameters(flatbuffers::FlatBufferBuilder &_fbb, const FusionTrackParametersT *_o, const flatbuffers::rehasher_function_t *_rehasher) { + (void)_rehasher; + (void)_o; + struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const FusionTrackParametersT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; + auto _name = _o->name.empty() ? 0 : _fbb.CreateString(_o->name); + auto _deviceClockID = _o->deviceClockID.empty() ? 0 : _fbb.CreateString(_o->deviceClockID); + auto _localClockID = _o->localClockID.empty() ? 0 : _fbb.CreateString(_o->localClockID); + auto _geometries = _o->geometries.size() ? _fbb.CreateVector> (_o->geometries.size(), [](size_t i, _VectorArgs *__va) { return CreateftkGeometry(*__va->__fbb, __va->__o->geometries[i].get(), __va->__rehasher); }, &_va ) : 0; + auto _geometryFilenames = _o->geometryFilenames.size() ? _fbb.CreateVectorOfStrings(_o->geometryFilenames) : 0; + auto _geometryDir = _o->geometryDir.empty() ? 0 : _fbb.CreateString(_o->geometryDir); + auto _TrackerDeviceIDs = _o->TrackerDeviceIDs.size() ? _fbb.CreateVector(_o->TrackerDeviceIDs) : 0; + auto _markerIDs = _o->markerIDs.size() ? _fbb.CreateVector(_o->markerIDs) : 0; + auto _markerNames = _o->markerNames.size() ? _fbb.CreateVectorOfStrings(_o->markerNames) : 0; + auto _m_deviceSerialNumbers = _o->m_deviceSerialNumbers.size() ? _fbb.CreateVector(_o->m_deviceSerialNumbers) : 0; + auto _m_device_types = _o->m_device_types.size() ? _fbb.CreateVector(_o->m_device_types) : 0; + return grl::flatbuffer::CreateFusionTrackParameters( + _fbb, + _name, + _deviceClockID, + _localClockID, + _geometries, + _geometryFilenames, + _geometryDir, + _TrackerDeviceIDs, + _markerIDs, + _markerNames, + _m_deviceSerialNumbers, + _m_device_types); +} + +inline FusionTrackMessageT *FusionTrackMessage::UnPack(const flatbuffers::resolver_function_t *_resolver) const { + auto _o = new FusionTrackMessageT(); + UnPackTo(_o, _resolver); + return _o; +} + +inline void FusionTrackMessage::UnPackTo(FusionTrackMessageT *_o, const flatbuffers::resolver_function_t *_resolver) const { + (void)_o; + (void)_resolver; + { auto _e = timestamp(); _o->timestamp = _e; }; + { auto _e = parameters(); if (_e) _o->parameters = std::unique_ptr(_e->UnPack(_resolver)); }; + { auto _e = timeEvent(); if (_e) _o->timeEvent = std::unique_ptr(_e->UnPack(_resolver)); }; + { auto _e = frame(); if (_e) _o->frame = std::unique_ptr(_e->UnPack(_resolver)); }; +} + +inline flatbuffers::Offset FusionTrackMessage::Pack(flatbuffers::FlatBufferBuilder &_fbb, const FusionTrackMessageT* _o, const flatbuffers::rehasher_function_t *_rehasher) { + return CreateFusionTrackMessage(_fbb, _o, _rehasher); +} + +inline flatbuffers::Offset CreateFusionTrackMessage(flatbuffers::FlatBufferBuilder &_fbb, const FusionTrackMessageT *_o, const flatbuffers::rehasher_function_t *_rehasher) { + (void)_rehasher; + (void)_o; + struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const FusionTrackMessageT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; + auto _timestamp = _o->timestamp; + auto _parameters = _o->parameters ? CreateFusionTrackParameters(_fbb, _o->parameters.get(), _rehasher) : 0; + auto _timeEvent = _o->timeEvent ? CreateTimeEvent(_fbb, _o->timeEvent.get(), _rehasher) : 0; + auto _frame = _o->frame ? CreateFusionTrackFrame(_fbb, _o->frame.get(), _rehasher) : 0; + return grl::flatbuffer::CreateFusionTrackMessage( + _fbb, + _timestamp, + _parameters, + _timeEvent, + _frame); +} + +} // namespace flatbuffer +} // namespace grl + +#endif // FLATBUFFERS_GENERATED_FUSIONTRACK_GRL_FLATBUFFER_H_ diff --git a/include/grl/flatbuffer/Geometry_generated.h b/include/grl/flatbuffer/Geometry_generated.h new file mode 100644 index 0000000..ebf7d04 --- /dev/null +++ b/include/grl/flatbuffer/Geometry_generated.h @@ -0,0 +1,200 @@ +// automatically generated by the FlatBuffers compiler, do not modify + + +#ifndef FLATBUFFERS_GENERATED_GEOMETRY_GRL_FLATBUFFER_H_ +#define FLATBUFFERS_GENERATED_GEOMETRY_GRL_FLATBUFFER_H_ + +#include "flatbuffers/flatbuffers.h" + +namespace grl { +namespace flatbuffer { + +struct Vector3d; + +struct Quaternion; + +struct Pose; + +struct Wrench; + +struct Inertia; + +FLATBUFFERS_MANUALLY_ALIGNED_STRUCT(8) Vector3d FLATBUFFERS_FINAL_CLASS { + private: + double x_; + double y_; + double z_; + + public: + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.Vector3d"; + } + Vector3d() { + memset(this, 0, sizeof(Vector3d)); + } + Vector3d(double _x, double _y, double _z) + : x_(flatbuffers::EndianScalar(_x)), + y_(flatbuffers::EndianScalar(_y)), + z_(flatbuffers::EndianScalar(_z)) { + } + double x() const { + return flatbuffers::EndianScalar(x_); + } + double y() const { + return flatbuffers::EndianScalar(y_); + } + double z() const { + return flatbuffers::EndianScalar(z_); + } +}; +FLATBUFFERS_STRUCT_END(Vector3d, 24); + +FLATBUFFERS_MANUALLY_ALIGNED_STRUCT(8) Quaternion FLATBUFFERS_FINAL_CLASS { + private: + double x_; + double y_; + double z_; + double w_; + + public: + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.Quaternion"; + } + Quaternion() { + memset(this, 0, sizeof(Quaternion)); + } + Quaternion(double _x, double _y, double _z, double _w) + : x_(flatbuffers::EndianScalar(_x)), + y_(flatbuffers::EndianScalar(_y)), + z_(flatbuffers::EndianScalar(_z)), + w_(flatbuffers::EndianScalar(_w)) { + } + double x() const { + return flatbuffers::EndianScalar(x_); + } + double y() const { + return flatbuffers::EndianScalar(y_); + } + double z() const { + return flatbuffers::EndianScalar(z_); + } + double w() const { + return flatbuffers::EndianScalar(w_); + } +}; +FLATBUFFERS_STRUCT_END(Quaternion, 32); + +FLATBUFFERS_MANUALLY_ALIGNED_STRUCT(8) Pose FLATBUFFERS_FINAL_CLASS { + private: + Vector3d position_; + Quaternion orientation_; + + public: + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.Pose"; + } + Pose() { + memset(this, 0, sizeof(Pose)); + } + Pose(const Vector3d &_position, const Quaternion &_orientation) + : position_(_position), + orientation_(_orientation) { + } + const Vector3d &position() const { + return position_; + } + const Quaternion &orientation() const { + return orientation_; + } +}; +FLATBUFFERS_STRUCT_END(Pose, 56); + +FLATBUFFERS_MANUALLY_ALIGNED_STRUCT(8) Wrench FLATBUFFERS_FINAL_CLASS { + private: + Vector3d force_; + Vector3d torque_; + Vector3d force_offset_; + + public: + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.Wrench"; + } + Wrench() { + memset(this, 0, sizeof(Wrench)); + } + Wrench(const Vector3d &_force, const Vector3d &_torque, const Vector3d &_force_offset) + : force_(_force), + torque_(_torque), + force_offset_(_force_offset) { + } + const Vector3d &force() const { + return force_; + } + const Vector3d &torque() const { + return torque_; + } + const Vector3d &force_offset() const { + return force_offset_; + } +}; +FLATBUFFERS_STRUCT_END(Wrench, 72); + +FLATBUFFERS_MANUALLY_ALIGNED_STRUCT(8) Inertia FLATBUFFERS_FINAL_CLASS { + private: + double mass_; + Pose pose_; + double ixx_; + double ixy_; + double ixz_; + double iyy_; + double iyz_; + double izz_; + + public: + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.Inertia"; + } + Inertia() { + memset(this, 0, sizeof(Inertia)); + } + Inertia(double _mass, const Pose &_pose, double _ixx, double _ixy, double _ixz, double _iyy, double _iyz, double _izz) + : mass_(flatbuffers::EndianScalar(_mass)), + pose_(_pose), + ixx_(flatbuffers::EndianScalar(_ixx)), + ixy_(flatbuffers::EndianScalar(_ixy)), + ixz_(flatbuffers::EndianScalar(_ixz)), + iyy_(flatbuffers::EndianScalar(_iyy)), + iyz_(flatbuffers::EndianScalar(_iyz)), + izz_(flatbuffers::EndianScalar(_izz)) { + } + double mass() const { + return flatbuffers::EndianScalar(mass_); + } + const Pose &pose() const { + return pose_; + } + double ixx() const { + return flatbuffers::EndianScalar(ixx_); + } + double ixy() const { + return flatbuffers::EndianScalar(ixy_); + } + double ixz() const { + return flatbuffers::EndianScalar(ixz_); + } + double iyy() const { + return flatbuffers::EndianScalar(iyy_); + } + double iyz() const { + return flatbuffers::EndianScalar(iyz_); + } + double izz() const { + return flatbuffers::EndianScalar(izz_); + } +}; +FLATBUFFERS_STRUCT_END(Inertia, 112); + +} // namespace flatbuffer +} // namespace grl + +#endif // FLATBUFFERS_GENERATED_GEOMETRY_GRL_FLATBUFFER_H_ diff --git a/include/grl/flatbuffer/JointState_generated.h b/include/grl/flatbuffer/JointState_generated.h new file mode 100644 index 0000000..fb78786 --- /dev/null +++ b/include/grl/flatbuffer/JointState_generated.h @@ -0,0 +1,163 @@ +// automatically generated by the FlatBuffers compiler, do not modify + + +#ifndef FLATBUFFERS_GENERATED_JOINTSTATE_GRL_FLATBUFFER_H_ +#define FLATBUFFERS_GENERATED_JOINTSTATE_GRL_FLATBUFFER_H_ + +#include "flatbuffers/flatbuffers.h" + +namespace grl { +namespace flatbuffer { + +struct JointState; +struct JointStateT; + +struct JointStateT : public flatbuffers::NativeTable { + typedef JointState TableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.JointStateT"; + } + std::vector position; + std::vector velocity; + std::vector acceleration; + std::vector torque; + JointStateT() { + } +}; + +struct JointState FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { + typedef JointStateT NativeTableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.JointState"; + } + enum { + VT_POSITION = 4, + VT_VELOCITY = 6, + VT_ACCELERATION = 8, + VT_TORQUE = 10 + }; + const flatbuffers::Vector *position() const { + return GetPointer *>(VT_POSITION); + } + const flatbuffers::Vector *velocity() const { + return GetPointer *>(VT_VELOCITY); + } + const flatbuffers::Vector *acceleration() const { + return GetPointer *>(VT_ACCELERATION); + } + const flatbuffers::Vector *torque() const { + return GetPointer *>(VT_TORQUE); + } + bool Verify(flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + VerifyOffset(verifier, VT_POSITION) && + verifier.Verify(position()) && + VerifyOffset(verifier, VT_VELOCITY) && + verifier.Verify(velocity()) && + VerifyOffset(verifier, VT_ACCELERATION) && + verifier.Verify(acceleration()) && + VerifyOffset(verifier, VT_TORQUE) && + verifier.Verify(torque()) && + verifier.EndTable(); + } + JointStateT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; + void UnPackTo(JointStateT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; + static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const JointStateT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); +}; + +struct JointStateBuilder { + flatbuffers::FlatBufferBuilder &fbb_; + flatbuffers::uoffset_t start_; + void add_position(flatbuffers::Offset> position) { + fbb_.AddOffset(JointState::VT_POSITION, position); + } + void add_velocity(flatbuffers::Offset> velocity) { + fbb_.AddOffset(JointState::VT_VELOCITY, velocity); + } + void add_acceleration(flatbuffers::Offset> acceleration) { + fbb_.AddOffset(JointState::VT_ACCELERATION, acceleration); + } + void add_torque(flatbuffers::Offset> torque) { + fbb_.AddOffset(JointState::VT_TORQUE, torque); + } + explicit JointStateBuilder(flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + JointStateBuilder &operator=(const JointStateBuilder &); + flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = flatbuffers::Offset(end); + return o; + } +}; + +inline flatbuffers::Offset CreateJointState( + flatbuffers::FlatBufferBuilder &_fbb, + flatbuffers::Offset> position = 0, + flatbuffers::Offset> velocity = 0, + flatbuffers::Offset> acceleration = 0, + flatbuffers::Offset> torque = 0) { + JointStateBuilder builder_(_fbb); + builder_.add_torque(torque); + builder_.add_acceleration(acceleration); + builder_.add_velocity(velocity); + builder_.add_position(position); + return builder_.Finish(); +} + +inline flatbuffers::Offset CreateJointStateDirect( + flatbuffers::FlatBufferBuilder &_fbb, + const std::vector *position = nullptr, + const std::vector *velocity = nullptr, + const std::vector *acceleration = nullptr, + const std::vector *torque = nullptr) { + return grl::flatbuffer::CreateJointState( + _fbb, + position ? _fbb.CreateVector(*position) : 0, + velocity ? _fbb.CreateVector(*velocity) : 0, + acceleration ? _fbb.CreateVector(*acceleration) : 0, + torque ? _fbb.CreateVector(*torque) : 0); +} + +flatbuffers::Offset CreateJointState(flatbuffers::FlatBufferBuilder &_fbb, const JointStateT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); + +inline JointStateT *JointState::UnPack(const flatbuffers::resolver_function_t *_resolver) const { + auto _o = new JointStateT(); + UnPackTo(_o, _resolver); + return _o; +} + +inline void JointState::UnPackTo(JointStateT *_o, const flatbuffers::resolver_function_t *_resolver) const { + (void)_o; + (void)_resolver; + { auto _e = position(); if (_e) { _o->position.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->position[_i] = _e->Get(_i); } } }; + { auto _e = velocity(); if (_e) { _o->velocity.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->velocity[_i] = _e->Get(_i); } } }; + { auto _e = acceleration(); if (_e) { _o->acceleration.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->acceleration[_i] = _e->Get(_i); } } }; + { auto _e = torque(); if (_e) { _o->torque.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->torque[_i] = _e->Get(_i); } } }; +} + +inline flatbuffers::Offset JointState::Pack(flatbuffers::FlatBufferBuilder &_fbb, const JointStateT* _o, const flatbuffers::rehasher_function_t *_rehasher) { + return CreateJointState(_fbb, _o, _rehasher); +} + +inline flatbuffers::Offset CreateJointState(flatbuffers::FlatBufferBuilder &_fbb, const JointStateT *_o, const flatbuffers::rehasher_function_t *_rehasher) { + (void)_rehasher; + (void)_o; + struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const JointStateT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; + auto _position = _o->position.size() ? _fbb.CreateVector(_o->position) : 0; + auto _velocity = _o->velocity.size() ? _fbb.CreateVector(_o->velocity) : 0; + auto _acceleration = _o->acceleration.size() ? _fbb.CreateVector(_o->acceleration) : 0; + auto _torque = _o->torque.size() ? _fbb.CreateVector(_o->torque) : 0; + return grl::flatbuffer::CreateJointState( + _fbb, + _position, + _velocity, + _acceleration, + _torque); +} + +} // namespace flatbuffer +} // namespace grl + +#endif // FLATBUFFERS_GENERATED_JOINTSTATE_GRL_FLATBUFFER_H_ diff --git a/include/grl/flatbuffer/KUKAiiwa_generated.h b/include/grl/flatbuffer/KUKAiiwa_generated.h new file mode 100644 index 0000000..41cde57 --- /dev/null +++ b/include/grl/flatbuffer/KUKAiiwa_generated.h @@ -0,0 +1,2885 @@ +// automatically generated by the FlatBuffers compiler, do not modify + + +#ifndef FLATBUFFERS_GENERATED_KUKAIIWA_GRL_FLATBUFFER_H_ +#define FLATBUFFERS_GENERATED_KUKAIIWA_GRL_FLATBUFFER_H_ + +#include "flatbuffers/flatbuffers.h" + +#include "ArmControlState_generated.h" +#include "Euler_generated.h" +#include "Geometry_generated.h" +#include "JointState_generated.h" +#include "LinkObject_generated.h" +#include "Time_generated.h" + +namespace grl { +namespace flatbuffer { + +struct CartesianImpedenceControlMode; +struct CartesianImpedenceControlModeT; + +struct JointImpedenceControlMode; +struct JointImpedenceControlModeT; + +struct Disabled; +struct DisabledT; + +struct FRI; +struct FRIT; + +struct SmartServo; +struct SmartServoT; + +struct ProcessData; +struct ProcessDataT; + +struct KUKAiiwaArmConfiguration; +struct KUKAiiwaArmConfigurationT; + +struct KUKAiiwaMonitorConfiguration; +struct KUKAiiwaMonitorConfigurationT; + +struct KUKAiiwaMonitorState; +struct KUKAiiwaMonitorStateT; + +struct FRITimeStamp; +struct FRITimeStampT; + +struct FRIMessageLog; +struct FRIMessageLogT; + +struct KUKAiiwaState; +struct KUKAiiwaStateT; + +struct KUKAiiwaStates; +struct KUKAiiwaStatesT; + +enum class KUKAiiwaInterface : int8_t { + Disabled = 0, + SmartServo = 1, + DirectServo = 2, + FRI = 3, + MIN = Disabled, + MAX = FRI +}; + +inline const KUKAiiwaInterface (&EnumValuesKUKAiiwaInterface())[4] { + static const KUKAiiwaInterface values[] = { + KUKAiiwaInterface::Disabled, + KUKAiiwaInterface::SmartServo, + KUKAiiwaInterface::DirectServo, + KUKAiiwaInterface::FRI + }; + return values; +} + +inline const char * const *EnumNamesKUKAiiwaInterface() { + static const char * const names[] = { + "Disabled", + "SmartServo", + "DirectServo", + "FRI", + nullptr + }; + return names; +} + +inline const char *EnumNameKUKAiiwaInterface(KUKAiiwaInterface e) { + const size_t index = static_cast(e); + return EnumNamesKUKAiiwaInterface()[index]; +} + +enum class ESessionState : int8_t { + /// No session + IDLE = 0 /// Monitoring mode, receive state but connection too inconsistent to command +, + MONITORING_WAIT = 1 /// Monitoring mode +, + MONITORING_READY = 2 /// About to command (Overlay created in Java interface) +, + COMMANDING_WAIT = 3 /// Actively commanding the arm with FRI +, + COMMANDING_ACTIVE = 4, + MIN = IDLE, + MAX = COMMANDING_ACTIVE +}; + +inline const ESessionState (&EnumValuesESessionState())[5] { + static const ESessionState values[] = { + ESessionState::IDLE, + ESessionState::MONITORING_WAIT, + ESessionState::MONITORING_READY, + ESessionState::COMMANDING_WAIT, + ESessionState::COMMANDING_ACTIVE + }; + return values; +} + +inline const char * const *EnumNamesESessionState() { + static const char * const names[] = { + "IDLE", + "MONITORING_WAIT", + "MONITORING_READY", + "COMMANDING_WAIT", + "COMMANDING_ACTIVE", + nullptr + }; + return names; +} + +inline const char *EnumNameESessionState(ESessionState e) { + const size_t index = static_cast(e); + return EnumNamesESessionState()[index]; +} + +enum class EConnectionQuality : int8_t { + POOR = 0, + FAIR = 1, + GOOD = 2, + EXCELLENT = 3, + MIN = POOR, + MAX = EXCELLENT +}; + +inline const EConnectionQuality (&EnumValuesEConnectionQuality())[4] { + static const EConnectionQuality values[] = { + EConnectionQuality::POOR, + EConnectionQuality::FAIR, + EConnectionQuality::GOOD, + EConnectionQuality::EXCELLENT + }; + return values; +} + +inline const char * const *EnumNamesEConnectionQuality() { + static const char * const names[] = { + "POOR", + "FAIR", + "GOOD", + "EXCELLENT", + nullptr + }; + return names; +} + +inline const char *EnumNameEConnectionQuality(EConnectionQuality e) { + const size_t index = static_cast(e); + return EnumNamesEConnectionQuality()[index]; +} + +enum class ESafetyState : int8_t { + NORMAL_OPERATION = 0, + SAFETY_STOP_LEVEL_0 = 1, + SAFETY_STOP_LEVEL_1 = 2, + SAFETY_STOP_LEVEL_2 = 3, + MIN = NORMAL_OPERATION, + MAX = SAFETY_STOP_LEVEL_2 +}; + +inline const ESafetyState (&EnumValuesESafetyState())[4] { + static const ESafetyState values[] = { + ESafetyState::NORMAL_OPERATION, + ESafetyState::SAFETY_STOP_LEVEL_0, + ESafetyState::SAFETY_STOP_LEVEL_1, + ESafetyState::SAFETY_STOP_LEVEL_2 + }; + return values; +} + +inline const char * const *EnumNamesESafetyState() { + static const char * const names[] = { + "NORMAL_OPERATION", + "SAFETY_STOP_LEVEL_0", + "SAFETY_STOP_LEVEL_1", + "SAFETY_STOP_LEVEL_2", + nullptr + }; + return names; +} + +inline const char *EnumNameESafetyState(ESafetyState e) { + const size_t index = static_cast(e); + return EnumNamesESafetyState()[index]; +} + +enum class EOperationMode : int8_t { + TEST_MODE_1 = 0, + TEST_MODE_2 = 1, + AUTOMATIC_MODE = 2, + MIN = TEST_MODE_1, + MAX = AUTOMATIC_MODE +}; + +inline const EOperationMode (&EnumValuesEOperationMode())[3] { + static const EOperationMode values[] = { + EOperationMode::TEST_MODE_1, + EOperationMode::TEST_MODE_2, + EOperationMode::AUTOMATIC_MODE + }; + return values; +} + +inline const char * const *EnumNamesEOperationMode() { + static const char * const names[] = { + "TEST_MODE_1", + "TEST_MODE_2", + "AUTOMATIC_MODE", + nullptr + }; + return names; +} + +inline const char *EnumNameEOperationMode(EOperationMode e) { + const size_t index = static_cast(e); + return EnumNamesEOperationMode()[index]; +} + +enum class EDriveState : int8_t { + /// Driving mode currently unused + OFF = 1 /// About to drive +, + TRANSITIONING = 2 /// Actively commanding arm +, + ACTIVE = 3, + MIN = OFF, + MAX = ACTIVE +}; + +inline const EDriveState (&EnumValuesEDriveState())[3] { + static const EDriveState values[] = { + EDriveState::OFF, + EDriveState::TRANSITIONING, + EDriveState::ACTIVE + }; + return values; +} + +inline const char * const *EnumNamesEDriveState() { + static const char * const names[] = { + "OFF", + "TRANSITIONING", + "ACTIVE", + nullptr + }; + return names; +} + +inline const char *EnumNameEDriveState(EDriveState e) { + const size_t index = static_cast(e) - static_cast(EDriveState::OFF); + return EnumNamesEDriveState()[index]; +} + +enum class EControlMode : int8_t { + POSITION_CONTROL_MODE = 0, + CART_IMP_CONTROL_MODE = 1, + JOINT_IMP_CONTROL_MODE = 2, + NO_CONTROL = 3, + MIN = POSITION_CONTROL_MODE, + MAX = NO_CONTROL +}; + +inline const EControlMode (&EnumValuesEControlMode())[4] { + static const EControlMode values[] = { + EControlMode::POSITION_CONTROL_MODE, + EControlMode::CART_IMP_CONTROL_MODE, + EControlMode::JOINT_IMP_CONTROL_MODE, + EControlMode::NO_CONTROL + }; + return values; +} + +inline const char * const *EnumNamesEControlMode() { + static const char * const names[] = { + "POSITION_CONTROL_MODE", + "CART_IMP_CONTROL_MODE", + "JOINT_IMP_CONTROL_MODE", + "NO_CONTROL", + nullptr + }; + return names; +} + +inline const char *EnumNameEControlMode(EControlMode e) { + const size_t index = static_cast(e); + return EnumNamesEControlMode()[index]; +} + +/// Type of command being sent to the arm (Dimensonal units) +enum class EClientCommandMode : int8_t { + NO_COMMAND_MODE = 0, + POSITION = 1, + WRENCH = 2, + TORQUE = 3, + MIN = NO_COMMAND_MODE, + MAX = TORQUE +}; + +inline const EClientCommandMode (&EnumValuesEClientCommandMode())[4] { + static const EClientCommandMode values[] = { + EClientCommandMode::NO_COMMAND_MODE, + EClientCommandMode::POSITION, + EClientCommandMode::WRENCH, + EClientCommandMode::TORQUE + }; + return values; +} + +inline const char * const *EnumNamesEClientCommandMode() { + static const char * const names[] = { + "NO_COMMAND_MODE", + "POSITION", + "WRENCH", + "TORQUE", + nullptr + }; + return names; +} + +inline const char *EnumNameEClientCommandMode(EClientCommandMode e) { + const size_t index = static_cast(e); + return EnumNamesEClientCommandMode()[index]; +} + +enum class EOverlayType : int8_t { + NO_OVERLAY = 0, + JOINT = 1, + CARTESIAN = 2, + MIN = NO_OVERLAY, + MAX = CARTESIAN +}; + +inline const EOverlayType (&EnumValuesEOverlayType())[3] { + static const EOverlayType values[] = { + EOverlayType::NO_OVERLAY, + EOverlayType::JOINT, + EOverlayType::CARTESIAN + }; + return values; +} + +inline const char * const *EnumNamesEOverlayType() { + static const char * const names[] = { + "NO_OVERLAY", + "JOINT", + "CARTESIAN", + nullptr + }; + return names; +} + +inline const char *EnumNameEOverlayType(EOverlayType e) { + const size_t index = static_cast(e); + return EnumNamesEOverlayType()[index]; +} + +struct CartesianImpedenceControlModeT : public flatbuffers::NativeTable { + typedef CartesianImpedenceControlMode TableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.CartesianImpedenceControlModeT"; + } + std::unique_ptr stiffness; + std::unique_ptr damping; + double nullspaceStiffness; + double nullspaceDamping; + std::unique_ptr maxPathDeviation; + std::unique_ptr maxCartesianVelocity; + std::unique_ptr maxControlForce; + bool maxControlForceExceededStop; + CartesianImpedenceControlModeT() + : nullspaceStiffness(0.0), + nullspaceDamping(0.0), + maxControlForceExceededStop(false) { + } +}; + +struct CartesianImpedenceControlMode FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { + typedef CartesianImpedenceControlModeT NativeTableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.CartesianImpedenceControlMode"; + } + enum { + VT_STIFFNESS = 4, + VT_DAMPING = 6, + VT_NULLSPACESTIFFNESS = 8, + VT_NULLSPACEDAMPING = 10, + VT_MAXPATHDEVIATION = 12, + VT_MAXCARTESIANVELOCITY = 14, + VT_MAXCONTROLFORCE = 16, + VT_MAXCONTROLFORCEEXCEEDEDSTOP = 18 + }; + /// actual stiffness to set rot:[nm/rad] + const EulerPose *stiffness() const { + return GetStruct(VT_STIFFNESS); + } + /// actual damping to set + const EulerPose *damping() const { + return GetStruct(VT_DAMPING); + } + /// [Nm/rad] must be => 0.0 + double nullspaceStiffness() const { + return GetField(VT_NULLSPACESTIFFNESS, 0.0); + } + /// must be between 0.3-1.0 suggested is 0.7 + double nullspaceDamping() const { + return GetField(VT_NULLSPACEDAMPING, 0.0); + } + /// maximum deviation from set goal in mm and radians + const EulerPose *maxPathDeviation() const { + return GetStruct(VT_MAXPATHDEVIATION); + } + /// trans: [mm/s] rot: [rad/s] + const EulerPose *maxCartesianVelocity() const { + return GetStruct(VT_MAXCARTESIANVELOCITY); + } + /// xyz: Newtons rpy:Nm (all >=0) + const EulerPose *maxControlForce() const { + return GetStruct(VT_MAXCONTROLFORCE); + } + /// stop if max control force is exceeded + bool maxControlForceExceededStop() const { + return GetField(VT_MAXCONTROLFORCEEXCEEDEDSTOP, 0) != 0; + } + bool Verify(flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + VerifyField(verifier, VT_STIFFNESS) && + VerifyField(verifier, VT_DAMPING) && + VerifyField(verifier, VT_NULLSPACESTIFFNESS) && + VerifyField(verifier, VT_NULLSPACEDAMPING) && + VerifyField(verifier, VT_MAXPATHDEVIATION) && + VerifyField(verifier, VT_MAXCARTESIANVELOCITY) && + VerifyField(verifier, VT_MAXCONTROLFORCE) && + VerifyField(verifier, VT_MAXCONTROLFORCEEXCEEDEDSTOP) && + verifier.EndTable(); + } + CartesianImpedenceControlModeT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; + void UnPackTo(CartesianImpedenceControlModeT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; + static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const CartesianImpedenceControlModeT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); +}; + +struct CartesianImpedenceControlModeBuilder { + flatbuffers::FlatBufferBuilder &fbb_; + flatbuffers::uoffset_t start_; + void add_stiffness(const EulerPose *stiffness) { + fbb_.AddStruct(CartesianImpedenceControlMode::VT_STIFFNESS, stiffness); + } + void add_damping(const EulerPose *damping) { + fbb_.AddStruct(CartesianImpedenceControlMode::VT_DAMPING, damping); + } + void add_nullspaceStiffness(double nullspaceStiffness) { + fbb_.AddElement(CartesianImpedenceControlMode::VT_NULLSPACESTIFFNESS, nullspaceStiffness, 0.0); + } + void add_nullspaceDamping(double nullspaceDamping) { + fbb_.AddElement(CartesianImpedenceControlMode::VT_NULLSPACEDAMPING, nullspaceDamping, 0.0); + } + void add_maxPathDeviation(const EulerPose *maxPathDeviation) { + fbb_.AddStruct(CartesianImpedenceControlMode::VT_MAXPATHDEVIATION, maxPathDeviation); + } + void add_maxCartesianVelocity(const EulerPose *maxCartesianVelocity) { + fbb_.AddStruct(CartesianImpedenceControlMode::VT_MAXCARTESIANVELOCITY, maxCartesianVelocity); + } + void add_maxControlForce(const EulerPose *maxControlForce) { + fbb_.AddStruct(CartesianImpedenceControlMode::VT_MAXCONTROLFORCE, maxControlForce); + } + void add_maxControlForceExceededStop(bool maxControlForceExceededStop) { + fbb_.AddElement(CartesianImpedenceControlMode::VT_MAXCONTROLFORCEEXCEEDEDSTOP, static_cast(maxControlForceExceededStop), 0); + } + explicit CartesianImpedenceControlModeBuilder(flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + CartesianImpedenceControlModeBuilder &operator=(const CartesianImpedenceControlModeBuilder &); + flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = flatbuffers::Offset(end); + return o; + } +}; + +inline flatbuffers::Offset CreateCartesianImpedenceControlMode( + flatbuffers::FlatBufferBuilder &_fbb, + const EulerPose *stiffness = 0, + const EulerPose *damping = 0, + double nullspaceStiffness = 0.0, + double nullspaceDamping = 0.0, + const EulerPose *maxPathDeviation = 0, + const EulerPose *maxCartesianVelocity = 0, + const EulerPose *maxControlForce = 0, + bool maxControlForceExceededStop = false) { + CartesianImpedenceControlModeBuilder builder_(_fbb); + builder_.add_nullspaceDamping(nullspaceDamping); + builder_.add_nullspaceStiffness(nullspaceStiffness); + builder_.add_maxControlForce(maxControlForce); + builder_.add_maxCartesianVelocity(maxCartesianVelocity); + builder_.add_maxPathDeviation(maxPathDeviation); + builder_.add_damping(damping); + builder_.add_stiffness(stiffness); + builder_.add_maxControlForceExceededStop(maxControlForceExceededStop); + return builder_.Finish(); +} + +flatbuffers::Offset CreateCartesianImpedenceControlMode(flatbuffers::FlatBufferBuilder &_fbb, const CartesianImpedenceControlModeT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); + +struct JointImpedenceControlModeT : public flatbuffers::NativeTable { + typedef JointImpedenceControlMode TableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.JointImpedenceControlModeT"; + } + std::vector stiffness; + std::vector damping; + JointImpedenceControlModeT() { + } +}; + +struct JointImpedenceControlMode FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { + typedef JointImpedenceControlModeT NativeTableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.JointImpedenceControlMode"; + } + enum { + VT_STIFFNESS = 4, + VT_DAMPING = 6 + }; + const flatbuffers::Vector *stiffness() const { + return GetPointer *>(VT_STIFFNESS); + } + const flatbuffers::Vector *damping() const { + return GetPointer *>(VT_DAMPING); + } + bool Verify(flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + VerifyOffset(verifier, VT_STIFFNESS) && + verifier.Verify(stiffness()) && + VerifyOffset(verifier, VT_DAMPING) && + verifier.Verify(damping()) && + verifier.EndTable(); + } + JointImpedenceControlModeT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; + void UnPackTo(JointImpedenceControlModeT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; + static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const JointImpedenceControlModeT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); +}; + +struct JointImpedenceControlModeBuilder { + flatbuffers::FlatBufferBuilder &fbb_; + flatbuffers::uoffset_t start_; + void add_stiffness(flatbuffers::Offset> stiffness) { + fbb_.AddOffset(JointImpedenceControlMode::VT_STIFFNESS, stiffness); + } + void add_damping(flatbuffers::Offset> damping) { + fbb_.AddOffset(JointImpedenceControlMode::VT_DAMPING, damping); + } + explicit JointImpedenceControlModeBuilder(flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + JointImpedenceControlModeBuilder &operator=(const JointImpedenceControlModeBuilder &); + flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = flatbuffers::Offset(end); + return o; + } +}; + +inline flatbuffers::Offset CreateJointImpedenceControlMode( + flatbuffers::FlatBufferBuilder &_fbb, + flatbuffers::Offset> stiffness = 0, + flatbuffers::Offset> damping = 0) { + JointImpedenceControlModeBuilder builder_(_fbb); + builder_.add_damping(damping); + builder_.add_stiffness(stiffness); + return builder_.Finish(); +} + +inline flatbuffers::Offset CreateJointImpedenceControlModeDirect( + flatbuffers::FlatBufferBuilder &_fbb, + const std::vector *stiffness = nullptr, + const std::vector *damping = nullptr) { + return grl::flatbuffer::CreateJointImpedenceControlMode( + _fbb, + stiffness ? _fbb.CreateVector(*stiffness) : 0, + damping ? _fbb.CreateVector(*damping) : 0); +} + +flatbuffers::Offset CreateJointImpedenceControlMode(flatbuffers::FlatBufferBuilder &_fbb, const JointImpedenceControlModeT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); + +struct DisabledT : public flatbuffers::NativeTable { + typedef Disabled TableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.DisabledT"; + } + DisabledT() { + } +}; + +struct Disabled FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { + typedef DisabledT NativeTableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.Disabled"; + } + bool Verify(flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + verifier.EndTable(); + } + DisabledT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; + void UnPackTo(DisabledT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; + static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const DisabledT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); +}; + +struct DisabledBuilder { + flatbuffers::FlatBufferBuilder &fbb_; + flatbuffers::uoffset_t start_; + explicit DisabledBuilder(flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + DisabledBuilder &operator=(const DisabledBuilder &); + flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = flatbuffers::Offset(end); + return o; + } +}; + +inline flatbuffers::Offset CreateDisabled( + flatbuffers::FlatBufferBuilder &_fbb) { + DisabledBuilder builder_(_fbb); + return builder_.Finish(); +} + +flatbuffers::Offset CreateDisabled(flatbuffers::FlatBufferBuilder &_fbb, const DisabledT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); + +struct FRIT : public flatbuffers::NativeTable { + typedef FRI TableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.FRIT"; + } + EOverlayType overlayType; + int32_t sendPeriodMillisec; + int32_t setReceiveMultiplier; + bool updatePortOnRemote; + int16_t portOnRemote; + bool updatePortOnController; + int16_t portOnController; + FRIT() + : overlayType(EOverlayType::JOINT), + sendPeriodMillisec(4), + setReceiveMultiplier(5), + updatePortOnRemote(false), + portOnRemote(0), + updatePortOnController(false), + portOnController(0) { + } +}; + +struct FRI FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { + typedef FRIT NativeTableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.FRI"; + } + enum { + VT_OVERLAYTYPE = 4, + VT_SENDPERIODMILLISEC = 6, + VT_SETRECEIVEMULTIPLIER = 8, + VT_UPDATEPORTONREMOTE = 10, + VT_PORTONREMOTE = 12, + VT_UPDATEPORTONCONTROLLER = 14, + VT_PORTONCONTROLLER = 16 + }; + EOverlayType overlayType() const { + return static_cast(GetField(VT_OVERLAYTYPE, 1)); + } + /// Set the value for the send period of the connection from the KUKA controller to the remote side in [ms]. + /// This means, the KUKA controller will send cyclic FRI messages every sendPeriod milliseconds to the remote side. + /// + /// + /// Parameters: + /// sendPeriod - the send period in milliseconds, 1 <= sendPeriod <= 100. + /// Note: The recommended value for good performance should be between 1-5 milliseconds. + int32_t sendPeriodMillisec() const { + return GetField(VT_SENDPERIODMILLISEC, 4); + } + /// Set the receive multiplier of the cycle time from the remote side to the KUKA controller. + /// This multiplier defines the value of the receivePeriod which is calculated: + /// receivePeriod = receiveMultiplier * sendPeriod + /// + /// The KUKA controller will expect a FRI response message every receivePeriod milliseconds from the remote side. + /// + /// The receivePeriod has to be within the range of: + /// 1 <= receivePeriod <= 100. + int32_t setReceiveMultiplier() const { + return GetField(VT_SETRECEIVEMULTIPLIER, 5); + } + bool updatePortOnRemote() const { + return GetField(VT_UPDATEPORTONREMOTE, 0) != 0; + } + /// Set the port ID of the socket at the controller side. + /// Note: Do not change this port ID, unless your application requires different port IDs on both ends of the FRI channel. + /// For changing the FRI port ID on both sides, it is sufficient to call setPortOnRemote(int). + /// Values of controllerPortID: + /// "-1" - The configuration of setPortOnRemote(int) is used. This is the default. + /// recommended range of port IDs: 30200 <= controllerPortID < 30210 + int16_t portOnRemote() const { + return GetField(VT_PORTONREMOTE, 0); + } + bool updatePortOnController() const { + return GetField(VT_UPDATEPORTONCONTROLLER, 0) != 0; + } + /// Set the port ID of the FRI channel at the remote side. + /// By default, this port ID is used on both sides of the FRI channel, unless specified otherwise by setPortOnController(int). + /// + /// Values of portID: + /// + /// default port ID: 30200 + /// recommended range of port IDs: 30200 <= portID < 30210 + /// Since the FRI channel utilizes UDP as connection layer, make sure, that your network topology (firewall, network services) are chosen accordingly. + /// + /// Parameters: + /// portID - the port ID > 0 (also known as UDP port number) + int16_t portOnController() const { + return GetField(VT_PORTONCONTROLLER, 0); + } + bool Verify(flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + VerifyField(verifier, VT_OVERLAYTYPE) && + VerifyField(verifier, VT_SENDPERIODMILLISEC) && + VerifyField(verifier, VT_SETRECEIVEMULTIPLIER) && + VerifyField(verifier, VT_UPDATEPORTONREMOTE) && + VerifyField(verifier, VT_PORTONREMOTE) && + VerifyField(verifier, VT_UPDATEPORTONCONTROLLER) && + VerifyField(verifier, VT_PORTONCONTROLLER) && + verifier.EndTable(); + } + FRIT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; + void UnPackTo(FRIT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; + static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const FRIT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); +}; + +struct FRIBuilder { + flatbuffers::FlatBufferBuilder &fbb_; + flatbuffers::uoffset_t start_; + void add_overlayType(EOverlayType overlayType) { + fbb_.AddElement(FRI::VT_OVERLAYTYPE, static_cast(overlayType), 1); + } + void add_sendPeriodMillisec(int32_t sendPeriodMillisec) { + fbb_.AddElement(FRI::VT_SENDPERIODMILLISEC, sendPeriodMillisec, 4); + } + void add_setReceiveMultiplier(int32_t setReceiveMultiplier) { + fbb_.AddElement(FRI::VT_SETRECEIVEMULTIPLIER, setReceiveMultiplier, 5); + } + void add_updatePortOnRemote(bool updatePortOnRemote) { + fbb_.AddElement(FRI::VT_UPDATEPORTONREMOTE, static_cast(updatePortOnRemote), 0); + } + void add_portOnRemote(int16_t portOnRemote) { + fbb_.AddElement(FRI::VT_PORTONREMOTE, portOnRemote, 0); + } + void add_updatePortOnController(bool updatePortOnController) { + fbb_.AddElement(FRI::VT_UPDATEPORTONCONTROLLER, static_cast(updatePortOnController), 0); + } + void add_portOnController(int16_t portOnController) { + fbb_.AddElement(FRI::VT_PORTONCONTROLLER, portOnController, 0); + } + explicit FRIBuilder(flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + FRIBuilder &operator=(const FRIBuilder &); + flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = flatbuffers::Offset(end); + return o; + } +}; + +inline flatbuffers::Offset CreateFRI( + flatbuffers::FlatBufferBuilder &_fbb, + EOverlayType overlayType = EOverlayType::JOINT, + int32_t sendPeriodMillisec = 4, + int32_t setReceiveMultiplier = 5, + bool updatePortOnRemote = false, + int16_t portOnRemote = 0, + bool updatePortOnController = false, + int16_t portOnController = 0) { + FRIBuilder builder_(_fbb); + builder_.add_setReceiveMultiplier(setReceiveMultiplier); + builder_.add_sendPeriodMillisec(sendPeriodMillisec); + builder_.add_portOnController(portOnController); + builder_.add_portOnRemote(portOnRemote); + builder_.add_updatePortOnController(updatePortOnController); + builder_.add_updatePortOnRemote(updatePortOnRemote); + builder_.add_overlayType(overlayType); + return builder_.Finish(); +} + +flatbuffers::Offset CreateFRI(flatbuffers::FlatBufferBuilder &_fbb, const FRIT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); + +struct SmartServoT : public flatbuffers::NativeTable { + typedef SmartServo TableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.SmartServoT"; + } + std::vector jointAccelerationRel; + std::vector jointVelocityRel; + bool updateMinimumTrajectoryExecutionTime; + double minimumTrajectoryExecutionTime; + SmartServoT() + : updateMinimumTrajectoryExecutionTime(false), + minimumTrajectoryExecutionTime(0.0) { + } +}; + +struct SmartServo FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { + typedef SmartServoT NativeTableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.SmartServo"; + } + enum { + VT_JOINTACCELERATIONREL = 4, + VT_JOINTVELOCITYREL = 6, + VT_UPDATEMINIMUMTRAJECTORYEXECUTIONTIME = 8, + VT_MINIMUMTRAJECTORYEXECUTIONTIME = 10 + }; + /// normalized joint accelerations from 0 to 1 relative to system capabilities + const flatbuffers::Vector *jointAccelerationRel() const { + return GetPointer *>(VT_JOINTACCELERATIONREL); + } + /// normalized joint velocity from 0 to 1 relative to system capabilities + const flatbuffers::Vector *jointVelocityRel() const { + return GetPointer *>(VT_JOINTVELOCITYREL); + } + bool updateMinimumTrajectoryExecutionTime() const { + return GetField(VT_UPDATEMINIMUMTRAJECTORYEXECUTIONTIME, 0) != 0; + } + double minimumTrajectoryExecutionTime() const { + return GetField(VT_MINIMUMTRAJECTORYEXECUTIONTIME, 0.0); + } + bool Verify(flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + VerifyOffset(verifier, VT_JOINTACCELERATIONREL) && + verifier.Verify(jointAccelerationRel()) && + VerifyOffset(verifier, VT_JOINTVELOCITYREL) && + verifier.Verify(jointVelocityRel()) && + VerifyField(verifier, VT_UPDATEMINIMUMTRAJECTORYEXECUTIONTIME) && + VerifyField(verifier, VT_MINIMUMTRAJECTORYEXECUTIONTIME) && + verifier.EndTable(); + } + SmartServoT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; + void UnPackTo(SmartServoT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; + static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const SmartServoT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); +}; + +struct SmartServoBuilder { + flatbuffers::FlatBufferBuilder &fbb_; + flatbuffers::uoffset_t start_; + void add_jointAccelerationRel(flatbuffers::Offset> jointAccelerationRel) { + fbb_.AddOffset(SmartServo::VT_JOINTACCELERATIONREL, jointAccelerationRel); + } + void add_jointVelocityRel(flatbuffers::Offset> jointVelocityRel) { + fbb_.AddOffset(SmartServo::VT_JOINTVELOCITYREL, jointVelocityRel); + } + void add_updateMinimumTrajectoryExecutionTime(bool updateMinimumTrajectoryExecutionTime) { + fbb_.AddElement(SmartServo::VT_UPDATEMINIMUMTRAJECTORYEXECUTIONTIME, static_cast(updateMinimumTrajectoryExecutionTime), 0); + } + void add_minimumTrajectoryExecutionTime(double minimumTrajectoryExecutionTime) { + fbb_.AddElement(SmartServo::VT_MINIMUMTRAJECTORYEXECUTIONTIME, minimumTrajectoryExecutionTime, 0.0); + } + explicit SmartServoBuilder(flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + SmartServoBuilder &operator=(const SmartServoBuilder &); + flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = flatbuffers::Offset(end); + return o; + } +}; + +inline flatbuffers::Offset CreateSmartServo( + flatbuffers::FlatBufferBuilder &_fbb, + flatbuffers::Offset> jointAccelerationRel = 0, + flatbuffers::Offset> jointVelocityRel = 0, + bool updateMinimumTrajectoryExecutionTime = false, + double minimumTrajectoryExecutionTime = 0.0) { + SmartServoBuilder builder_(_fbb); + builder_.add_minimumTrajectoryExecutionTime(minimumTrajectoryExecutionTime); + builder_.add_jointVelocityRel(jointVelocityRel); + builder_.add_jointAccelerationRel(jointAccelerationRel); + builder_.add_updateMinimumTrajectoryExecutionTime(updateMinimumTrajectoryExecutionTime); + return builder_.Finish(); +} + +inline flatbuffers::Offset CreateSmartServoDirect( + flatbuffers::FlatBufferBuilder &_fbb, + const std::vector *jointAccelerationRel = nullptr, + const std::vector *jointVelocityRel = nullptr, + bool updateMinimumTrajectoryExecutionTime = false, + double minimumTrajectoryExecutionTime = 0.0) { + return grl::flatbuffer::CreateSmartServo( + _fbb, + jointAccelerationRel ? _fbb.CreateVector(*jointAccelerationRel) : 0, + jointVelocityRel ? _fbb.CreateVector(*jointVelocityRel) : 0, + updateMinimumTrajectoryExecutionTime, + minimumTrajectoryExecutionTime); +} + +flatbuffers::Offset CreateSmartServo(flatbuffers::FlatBufferBuilder &_fbb, const SmartServoT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); + +struct ProcessDataT : public flatbuffers::NativeTable { + typedef ProcessData TableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.ProcessDataT"; + } + std::string dataType; + std::string defaultValue; + std::string displayName; + std::string id; + std::string min; + std::string max; + std::string unit; + std::string value; + bool shouldRemove; + bool shouldUpdate; + ProcessDataT() + : shouldRemove(false), + shouldUpdate(false) { + } +}; + +/// "ProcessData" is a field that appears +/// on your physical kuka tablet. +/// This message allows you to update these +/// fields on the tablet yourself. +struct ProcessData FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { + typedef ProcessDataT NativeTableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.ProcessData"; + } + enum { + VT_DATATYPE = 4, + VT_DEFAULTVALUE = 6, + VT_DISPLAYNAME = 8, + VT_ID = 10, + VT_MIN = 12, + VT_MAX = 14, + VT_UNIT = 16, + VT_VALUE = 18, + VT_SHOULDREMOVE = 20, + VT_SHOULDUPDATE = 22 + }; + const flatbuffers::String *dataType() const { + return GetPointer(VT_DATATYPE); + } + const flatbuffers::String *defaultValue() const { + return GetPointer(VT_DEFAULTVALUE); + } + const flatbuffers::String *displayName() const { + return GetPointer(VT_DISPLAYNAME); + } + const flatbuffers::String *id() const { + return GetPointer(VT_ID); + } + const flatbuffers::String *min() const { + return GetPointer(VT_MIN); + } + const flatbuffers::String *max() const { + return GetPointer(VT_MAX); + } + const flatbuffers::String *unit() const { + return GetPointer(VT_UNIT); + } + const flatbuffers::String *value() const { + return GetPointer(VT_VALUE); + } + /// should the data be removed completely? + bool shouldRemove() const { + return GetField(VT_SHOULDREMOVE, 0) != 0; + } + /// should the data be updated to these values? + bool shouldUpdate() const { + return GetField(VT_SHOULDUPDATE, 0) != 0; + } + bool Verify(flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + VerifyOffset(verifier, VT_DATATYPE) && + verifier.Verify(dataType()) && + VerifyOffset(verifier, VT_DEFAULTVALUE) && + verifier.Verify(defaultValue()) && + VerifyOffset(verifier, VT_DISPLAYNAME) && + verifier.Verify(displayName()) && + VerifyOffset(verifier, VT_ID) && + verifier.Verify(id()) && + VerifyOffset(verifier, VT_MIN) && + verifier.Verify(min()) && + VerifyOffset(verifier, VT_MAX) && + verifier.Verify(max()) && + VerifyOffset(verifier, VT_UNIT) && + verifier.Verify(unit()) && + VerifyOffset(verifier, VT_VALUE) && + verifier.Verify(value()) && + VerifyField(verifier, VT_SHOULDREMOVE) && + VerifyField(verifier, VT_SHOULDUPDATE) && + verifier.EndTable(); + } + ProcessDataT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; + void UnPackTo(ProcessDataT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; + static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const ProcessDataT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); +}; + +struct ProcessDataBuilder { + flatbuffers::FlatBufferBuilder &fbb_; + flatbuffers::uoffset_t start_; + void add_dataType(flatbuffers::Offset dataType) { + fbb_.AddOffset(ProcessData::VT_DATATYPE, dataType); + } + void add_defaultValue(flatbuffers::Offset defaultValue) { + fbb_.AddOffset(ProcessData::VT_DEFAULTVALUE, defaultValue); + } + void add_displayName(flatbuffers::Offset displayName) { + fbb_.AddOffset(ProcessData::VT_DISPLAYNAME, displayName); + } + void add_id(flatbuffers::Offset id) { + fbb_.AddOffset(ProcessData::VT_ID, id); + } + void add_min(flatbuffers::Offset min) { + fbb_.AddOffset(ProcessData::VT_MIN, min); + } + void add_max(flatbuffers::Offset max) { + fbb_.AddOffset(ProcessData::VT_MAX, max); + } + void add_unit(flatbuffers::Offset unit) { + fbb_.AddOffset(ProcessData::VT_UNIT, unit); + } + void add_value(flatbuffers::Offset value) { + fbb_.AddOffset(ProcessData::VT_VALUE, value); + } + void add_shouldRemove(bool shouldRemove) { + fbb_.AddElement(ProcessData::VT_SHOULDREMOVE, static_cast(shouldRemove), 0); + } + void add_shouldUpdate(bool shouldUpdate) { + fbb_.AddElement(ProcessData::VT_SHOULDUPDATE, static_cast(shouldUpdate), 0); + } + explicit ProcessDataBuilder(flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + ProcessDataBuilder &operator=(const ProcessDataBuilder &); + flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = flatbuffers::Offset(end); + return o; + } +}; + +inline flatbuffers::Offset CreateProcessData( + flatbuffers::FlatBufferBuilder &_fbb, + flatbuffers::Offset dataType = 0, + flatbuffers::Offset defaultValue = 0, + flatbuffers::Offset displayName = 0, + flatbuffers::Offset id = 0, + flatbuffers::Offset min = 0, + flatbuffers::Offset max = 0, + flatbuffers::Offset unit = 0, + flatbuffers::Offset value = 0, + bool shouldRemove = false, + bool shouldUpdate = false) { + ProcessDataBuilder builder_(_fbb); + builder_.add_value(value); + builder_.add_unit(unit); + builder_.add_max(max); + builder_.add_min(min); + builder_.add_id(id); + builder_.add_displayName(displayName); + builder_.add_defaultValue(defaultValue); + builder_.add_dataType(dataType); + builder_.add_shouldUpdate(shouldUpdate); + builder_.add_shouldRemove(shouldRemove); + return builder_.Finish(); +} + +inline flatbuffers::Offset CreateProcessDataDirect( + flatbuffers::FlatBufferBuilder &_fbb, + const char *dataType = nullptr, + const char *defaultValue = nullptr, + const char *displayName = nullptr, + const char *id = nullptr, + const char *min = nullptr, + const char *max = nullptr, + const char *unit = nullptr, + const char *value = nullptr, + bool shouldRemove = false, + bool shouldUpdate = false) { + return grl::flatbuffer::CreateProcessData( + _fbb, + dataType ? _fbb.CreateString(dataType) : 0, + defaultValue ? _fbb.CreateString(defaultValue) : 0, + displayName ? _fbb.CreateString(displayName) : 0, + id ? _fbb.CreateString(id) : 0, + min ? _fbb.CreateString(min) : 0, + max ? _fbb.CreateString(max) : 0, + unit ? _fbb.CreateString(unit) : 0, + value ? _fbb.CreateString(value) : 0, + shouldRemove, + shouldUpdate); +} + +flatbuffers::Offset CreateProcessData(flatbuffers::FlatBufferBuilder &_fbb, const ProcessDataT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); + +struct KUKAiiwaArmConfigurationT : public flatbuffers::NativeTable { + typedef KUKAiiwaArmConfiguration TableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.KUKAiiwaArmConfigurationT"; + } + std::string name; + KUKAiiwaInterface commandInterface; + KUKAiiwaInterface monitorInterface; + EClientCommandMode clientCommandMode; + EOverlayType overlayType; + EControlMode controlMode; + std::unique_ptr setCartImpedance; + std::unique_ptr setJointImpedance; + std::unique_ptr smartServoConfig; + std::unique_ptr FRIConfig; + std::vector> tools; + std::vector> processData; + std::string currentMotionCenter; + bool requestMonitorProcessData; + KUKAiiwaArmConfigurationT() + : commandInterface(KUKAiiwaInterface::Disabled), + monitorInterface(KUKAiiwaInterface::Disabled), + clientCommandMode(EClientCommandMode::NO_COMMAND_MODE), + overlayType(EOverlayType::NO_OVERLAY), + controlMode(EControlMode::POSITION_CONTROL_MODE), + requestMonitorProcessData(false) { + } +}; + +struct KUKAiiwaArmConfiguration FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { + typedef KUKAiiwaArmConfigurationT NativeTableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.KUKAiiwaArmConfiguration"; + } + enum { + VT_NAME = 4, + VT_COMMANDINTERFACE = 6, + VT_MONITORINTERFACE = 8, + VT_CLIENTCOMMANDMODE = 10, + VT_OVERLAYTYPE = 12, + VT_CONTROLMODE = 14, + VT_SETCARTIMPEDANCE = 16, + VT_SETJOINTIMPEDANCE = 18, + VT_SMARTSERVOCONFIG = 20, + VT_FRICONFIG = 22, + VT_TOOLS = 24, + VT_PROCESSDATA = 26, + VT_CURRENTMOTIONCENTER = 28, + VT_REQUESTMONITORPROCESSDATA = 30 + }; + const flatbuffers::String *name() const { + return GetPointer(VT_NAME); + } + /// how commands will be sent to robot + KUKAiiwaInterface commandInterface() const { + return static_cast(GetField(VT_COMMANDINTERFACE, 0)); + } + /// how robot state will be sent to driver + KUKAiiwaInterface monitorInterface() const { + return static_cast(GetField(VT_MONITORINTERFACE, 0)); + } + /// motion command mode: cartesian, wrench, torque commands + EClientCommandMode clientCommandMode() const { + return static_cast(GetField(VT_CLIENTCOMMANDMODE, 0)); + } + /// The type of commands FRI will use: cartesian, joint + EOverlayType overlayType() const { + return static_cast(GetField(VT_OVERLAYTYPE, 0)); + } + /// position, cartesian impedence, or joint impedence low level controller adjustments + EControlMode controlMode() const { + return static_cast(GetField(VT_CONTROLMODE, 0)); + } + const CartesianImpedenceControlMode *setCartImpedance() const { + return GetPointer(VT_SETCARTIMPEDANCE); + } + const JointImpedenceControlMode *setJointImpedance() const { + return GetPointer(VT_SETJOINTIMPEDANCE); + } + const SmartServo *smartServoConfig() const { + return GetPointer(VT_SMARTSERVOCONFIG); + } + const FRI *FRIConfig() const { + return GetPointer(VT_FRICONFIG); + } + const flatbuffers::Vector> *tools() const { + return GetPointer> *>(VT_TOOLS); + } + /// set kuka tablet "processData" panel UI config strings + const flatbuffers::Vector> *processData() const { + return GetPointer> *>(VT_PROCESSDATA); + } + const flatbuffers::String *currentMotionCenter() const { + return GetPointer(VT_CURRENTMOTIONCENTER); + } + bool requestMonitorProcessData() const { + return GetField(VT_REQUESTMONITORPROCESSDATA, 0) != 0; + } + bool Verify(flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + VerifyOffset(verifier, VT_NAME) && + verifier.Verify(name()) && + VerifyField(verifier, VT_COMMANDINTERFACE) && + VerifyField(verifier, VT_MONITORINTERFACE) && + VerifyField(verifier, VT_CLIENTCOMMANDMODE) && + VerifyField(verifier, VT_OVERLAYTYPE) && + VerifyField(verifier, VT_CONTROLMODE) && + VerifyOffset(verifier, VT_SETCARTIMPEDANCE) && + verifier.VerifyTable(setCartImpedance()) && + VerifyOffset(verifier, VT_SETJOINTIMPEDANCE) && + verifier.VerifyTable(setJointImpedance()) && + VerifyOffset(verifier, VT_SMARTSERVOCONFIG) && + verifier.VerifyTable(smartServoConfig()) && + VerifyOffset(verifier, VT_FRICONFIG) && + verifier.VerifyTable(FRIConfig()) && + VerifyOffset(verifier, VT_TOOLS) && + verifier.Verify(tools()) && + verifier.VerifyVectorOfTables(tools()) && + VerifyOffset(verifier, VT_PROCESSDATA) && + verifier.Verify(processData()) && + verifier.VerifyVectorOfTables(processData()) && + VerifyOffset(verifier, VT_CURRENTMOTIONCENTER) && + verifier.Verify(currentMotionCenter()) && + VerifyField(verifier, VT_REQUESTMONITORPROCESSDATA) && + verifier.EndTable(); + } + KUKAiiwaArmConfigurationT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; + void UnPackTo(KUKAiiwaArmConfigurationT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; + static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaArmConfigurationT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); +}; + +struct KUKAiiwaArmConfigurationBuilder { + flatbuffers::FlatBufferBuilder &fbb_; + flatbuffers::uoffset_t start_; + void add_name(flatbuffers::Offset name) { + fbb_.AddOffset(KUKAiiwaArmConfiguration::VT_NAME, name); + } + void add_commandInterface(KUKAiiwaInterface commandInterface) { + fbb_.AddElement(KUKAiiwaArmConfiguration::VT_COMMANDINTERFACE, static_cast(commandInterface), 0); + } + void add_monitorInterface(KUKAiiwaInterface monitorInterface) { + fbb_.AddElement(KUKAiiwaArmConfiguration::VT_MONITORINTERFACE, static_cast(monitorInterface), 0); + } + void add_clientCommandMode(EClientCommandMode clientCommandMode) { + fbb_.AddElement(KUKAiiwaArmConfiguration::VT_CLIENTCOMMANDMODE, static_cast(clientCommandMode), 0); + } + void add_overlayType(EOverlayType overlayType) { + fbb_.AddElement(KUKAiiwaArmConfiguration::VT_OVERLAYTYPE, static_cast(overlayType), 0); + } + void add_controlMode(EControlMode controlMode) { + fbb_.AddElement(KUKAiiwaArmConfiguration::VT_CONTROLMODE, static_cast(controlMode), 0); + } + void add_setCartImpedance(flatbuffers::Offset setCartImpedance) { + fbb_.AddOffset(KUKAiiwaArmConfiguration::VT_SETCARTIMPEDANCE, setCartImpedance); + } + void add_setJointImpedance(flatbuffers::Offset setJointImpedance) { + fbb_.AddOffset(KUKAiiwaArmConfiguration::VT_SETJOINTIMPEDANCE, setJointImpedance); + } + void add_smartServoConfig(flatbuffers::Offset smartServoConfig) { + fbb_.AddOffset(KUKAiiwaArmConfiguration::VT_SMARTSERVOCONFIG, smartServoConfig); + } + void add_FRIConfig(flatbuffers::Offset FRIConfig) { + fbb_.AddOffset(KUKAiiwaArmConfiguration::VT_FRICONFIG, FRIConfig); + } + void add_tools(flatbuffers::Offset>> tools) { + fbb_.AddOffset(KUKAiiwaArmConfiguration::VT_TOOLS, tools); + } + void add_processData(flatbuffers::Offset>> processData) { + fbb_.AddOffset(KUKAiiwaArmConfiguration::VT_PROCESSDATA, processData); + } + void add_currentMotionCenter(flatbuffers::Offset currentMotionCenter) { + fbb_.AddOffset(KUKAiiwaArmConfiguration::VT_CURRENTMOTIONCENTER, currentMotionCenter); + } + void add_requestMonitorProcessData(bool requestMonitorProcessData) { + fbb_.AddElement(KUKAiiwaArmConfiguration::VT_REQUESTMONITORPROCESSDATA, static_cast(requestMonitorProcessData), 0); + } + explicit KUKAiiwaArmConfigurationBuilder(flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + KUKAiiwaArmConfigurationBuilder &operator=(const KUKAiiwaArmConfigurationBuilder &); + flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = flatbuffers::Offset(end); + return o; + } +}; + +inline flatbuffers::Offset CreateKUKAiiwaArmConfiguration( + flatbuffers::FlatBufferBuilder &_fbb, + flatbuffers::Offset name = 0, + KUKAiiwaInterface commandInterface = KUKAiiwaInterface::Disabled, + KUKAiiwaInterface monitorInterface = KUKAiiwaInterface::Disabled, + EClientCommandMode clientCommandMode = EClientCommandMode::NO_COMMAND_MODE, + EOverlayType overlayType = EOverlayType::NO_OVERLAY, + EControlMode controlMode = EControlMode::POSITION_CONTROL_MODE, + flatbuffers::Offset setCartImpedance = 0, + flatbuffers::Offset setJointImpedance = 0, + flatbuffers::Offset smartServoConfig = 0, + flatbuffers::Offset FRIConfig = 0, + flatbuffers::Offset>> tools = 0, + flatbuffers::Offset>> processData = 0, + flatbuffers::Offset currentMotionCenter = 0, + bool requestMonitorProcessData = false) { + KUKAiiwaArmConfigurationBuilder builder_(_fbb); + builder_.add_currentMotionCenter(currentMotionCenter); + builder_.add_processData(processData); + builder_.add_tools(tools); + builder_.add_FRIConfig(FRIConfig); + builder_.add_smartServoConfig(smartServoConfig); + builder_.add_setJointImpedance(setJointImpedance); + builder_.add_setCartImpedance(setCartImpedance); + builder_.add_name(name); + builder_.add_requestMonitorProcessData(requestMonitorProcessData); + builder_.add_controlMode(controlMode); + builder_.add_overlayType(overlayType); + builder_.add_clientCommandMode(clientCommandMode); + builder_.add_monitorInterface(monitorInterface); + builder_.add_commandInterface(commandInterface); + return builder_.Finish(); +} + +inline flatbuffers::Offset CreateKUKAiiwaArmConfigurationDirect( + flatbuffers::FlatBufferBuilder &_fbb, + const char *name = nullptr, + KUKAiiwaInterface commandInterface = KUKAiiwaInterface::Disabled, + KUKAiiwaInterface monitorInterface = KUKAiiwaInterface::Disabled, + EClientCommandMode clientCommandMode = EClientCommandMode::NO_COMMAND_MODE, + EOverlayType overlayType = EOverlayType::NO_OVERLAY, + EControlMode controlMode = EControlMode::POSITION_CONTROL_MODE, + flatbuffers::Offset setCartImpedance = 0, + flatbuffers::Offset setJointImpedance = 0, + flatbuffers::Offset smartServoConfig = 0, + flatbuffers::Offset FRIConfig = 0, + const std::vector> *tools = nullptr, + const std::vector> *processData = nullptr, + const char *currentMotionCenter = nullptr, + bool requestMonitorProcessData = false) { + return grl::flatbuffer::CreateKUKAiiwaArmConfiguration( + _fbb, + name ? _fbb.CreateString(name) : 0, + commandInterface, + monitorInterface, + clientCommandMode, + overlayType, + controlMode, + setCartImpedance, + setJointImpedance, + smartServoConfig, + FRIConfig, + tools ? _fbb.CreateVector>(*tools) : 0, + processData ? _fbb.CreateVector>(*processData) : 0, + currentMotionCenter ? _fbb.CreateString(currentMotionCenter) : 0, + requestMonitorProcessData); +} + +flatbuffers::Offset CreateKUKAiiwaArmConfiguration(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaArmConfigurationT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); + +struct KUKAiiwaMonitorConfigurationT : public flatbuffers::NativeTable { + typedef KUKAiiwaMonitorConfiguration TableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.KUKAiiwaMonitorConfigurationT"; + } + std::string hardwareVersion; + std::vector torqueSensorLimits; + bool isReadyToMove; + bool isMastered; + std::vector> processData; + KUKAiiwaMonitorConfigurationT() + : isReadyToMove(false), + isMastered(false) { + } +}; + +struct KUKAiiwaMonitorConfiguration FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { + typedef KUKAiiwaMonitorConfigurationT NativeTableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.KUKAiiwaMonitorConfiguration"; + } + enum { + VT_HARDWAREVERSION = 4, + VT_TORQUESENSORLIMITS = 6, + VT_ISREADYTOMOVE = 8, + VT_ISMASTERED = 10, + VT_PROCESSDATA = 12 + }; + const flatbuffers::String *hardwareVersion() const { + return GetPointer(VT_HARDWAREVERSION); + } + const flatbuffers::Vector *torqueSensorLimits() const { + return GetPointer *>(VT_TORQUESENSORLIMITS); + } + bool isReadyToMove() const { + return GetField(VT_ISREADYTOMOVE, 0) != 0; + } + bool isMastered() const { + return GetField(VT_ISMASTERED, 0) != 0; + } + /// set kuka tablet "processData" panel UI config strings + const flatbuffers::Vector> *processData() const { + return GetPointer> *>(VT_PROCESSDATA); + } + bool Verify(flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + VerifyOffset(verifier, VT_HARDWAREVERSION) && + verifier.Verify(hardwareVersion()) && + VerifyOffset(verifier, VT_TORQUESENSORLIMITS) && + verifier.Verify(torqueSensorLimits()) && + VerifyField(verifier, VT_ISREADYTOMOVE) && + VerifyField(verifier, VT_ISMASTERED) && + VerifyOffset(verifier, VT_PROCESSDATA) && + verifier.Verify(processData()) && + verifier.VerifyVectorOfTables(processData()) && + verifier.EndTable(); + } + KUKAiiwaMonitorConfigurationT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; + void UnPackTo(KUKAiiwaMonitorConfigurationT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; + static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaMonitorConfigurationT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); +}; + +struct KUKAiiwaMonitorConfigurationBuilder { + flatbuffers::FlatBufferBuilder &fbb_; + flatbuffers::uoffset_t start_; + void add_hardwareVersion(flatbuffers::Offset hardwareVersion) { + fbb_.AddOffset(KUKAiiwaMonitorConfiguration::VT_HARDWAREVERSION, hardwareVersion); + } + void add_torqueSensorLimits(flatbuffers::Offset> torqueSensorLimits) { + fbb_.AddOffset(KUKAiiwaMonitorConfiguration::VT_TORQUESENSORLIMITS, torqueSensorLimits); + } + void add_isReadyToMove(bool isReadyToMove) { + fbb_.AddElement(KUKAiiwaMonitorConfiguration::VT_ISREADYTOMOVE, static_cast(isReadyToMove), 0); + } + void add_isMastered(bool isMastered) { + fbb_.AddElement(KUKAiiwaMonitorConfiguration::VT_ISMASTERED, static_cast(isMastered), 0); + } + void add_processData(flatbuffers::Offset>> processData) { + fbb_.AddOffset(KUKAiiwaMonitorConfiguration::VT_PROCESSDATA, processData); + } + explicit KUKAiiwaMonitorConfigurationBuilder(flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + KUKAiiwaMonitorConfigurationBuilder &operator=(const KUKAiiwaMonitorConfigurationBuilder &); + flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = flatbuffers::Offset(end); + return o; + } +}; + +inline flatbuffers::Offset CreateKUKAiiwaMonitorConfiguration( + flatbuffers::FlatBufferBuilder &_fbb, + flatbuffers::Offset hardwareVersion = 0, + flatbuffers::Offset> torqueSensorLimits = 0, + bool isReadyToMove = false, + bool isMastered = false, + flatbuffers::Offset>> processData = 0) { + KUKAiiwaMonitorConfigurationBuilder builder_(_fbb); + builder_.add_processData(processData); + builder_.add_torqueSensorLimits(torqueSensorLimits); + builder_.add_hardwareVersion(hardwareVersion); + builder_.add_isMastered(isMastered); + builder_.add_isReadyToMove(isReadyToMove); + return builder_.Finish(); +} + +inline flatbuffers::Offset CreateKUKAiiwaMonitorConfigurationDirect( + flatbuffers::FlatBufferBuilder &_fbb, + const char *hardwareVersion = nullptr, + const std::vector *torqueSensorLimits = nullptr, + bool isReadyToMove = false, + bool isMastered = false, + const std::vector> *processData = nullptr) { + return grl::flatbuffer::CreateKUKAiiwaMonitorConfiguration( + _fbb, + hardwareVersion ? _fbb.CreateString(hardwareVersion) : 0, + torqueSensorLimits ? _fbb.CreateVector(*torqueSensorLimits) : 0, + isReadyToMove, + isMastered, + processData ? _fbb.CreateVector>(*processData) : 0); +} + +flatbuffers::Offset CreateKUKAiiwaMonitorConfiguration(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaMonitorConfigurationT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); + +struct KUKAiiwaMonitorStateT : public flatbuffers::NativeTable { + typedef KUKAiiwaMonitorState TableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.KUKAiiwaMonitorStateT"; + } + std::unique_ptr measuredState; + std::unique_ptr cartesianFlangePose; + std::unique_ptr jointStateReal; + std::unique_ptr jointStateInterpolated; + std::unique_ptr externalState; + EOperationMode operationMode; + ESessionState sessionState; + std::unique_ptr CartesianWrench; + KUKAiiwaMonitorStateT() + : operationMode(EOperationMode::TEST_MODE_1), + sessionState(ESessionState::IDLE) { + } +}; + +struct KUKAiiwaMonitorState FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { + typedef KUKAiiwaMonitorStateT NativeTableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.KUKAiiwaMonitorState"; + } + enum { + VT_MEASUREDSTATE = 4, + VT_CARTESIANFLANGEPOSE = 6, + VT_JOINTSTATEREAL = 8, + VT_JOINTSTATEINTERPOLATED = 10, + VT_EXTERNALSTATE = 12, + VT_OPERATIONMODE = 14, + VT_SESSIONSTATE = 16, + VT_CARTESIANWRENCH = 18 + }; + const JointState *measuredState() const { + return GetPointer(VT_MEASUREDSTATE); + } + const Pose *cartesianFlangePose() const { + return GetStruct(VT_CARTESIANFLANGEPOSE); + } + const JointState *jointStateReal() const { + return GetPointer(VT_JOINTSTATEREAL); + } + const JointState *jointStateInterpolated() const { + return GetPointer(VT_JOINTSTATEINTERPOLATED); + } + /// The state of the arm as calculated by kuka after + /// subtracting the known weights of the arm + /// and any attachments configured to be present. + /// + /// Most likely only contains torque. + /// KukaState::ExternalTorque goes here + const JointState *externalState() const { + return GetPointer(VT_EXTERNALSTATE); + } + /// KUKA::FRI::EOperationMode + EOperationMode operationMode() const { + return static_cast(GetField(VT_OPERATIONMODE, 0)); + } + ESessionState sessionState() const { + return static_cast(GetField(VT_SESSIONSTATE, 0)); + } + const Wrench *CartesianWrench() const { + return GetStruct(VT_CARTESIANWRENCH); + } + bool Verify(flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + VerifyOffset(verifier, VT_MEASUREDSTATE) && + verifier.VerifyTable(measuredState()) && + VerifyField(verifier, VT_CARTESIANFLANGEPOSE) && + VerifyOffset(verifier, VT_JOINTSTATEREAL) && + verifier.VerifyTable(jointStateReal()) && + VerifyOffset(verifier, VT_JOINTSTATEINTERPOLATED) && + verifier.VerifyTable(jointStateInterpolated()) && + VerifyOffset(verifier, VT_EXTERNALSTATE) && + verifier.VerifyTable(externalState()) && + VerifyField(verifier, VT_OPERATIONMODE) && + VerifyField(verifier, VT_SESSIONSTATE) && + VerifyField(verifier, VT_CARTESIANWRENCH) && + verifier.EndTable(); + } + KUKAiiwaMonitorStateT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; + void UnPackTo(KUKAiiwaMonitorStateT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; + static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaMonitorStateT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); +}; + +struct KUKAiiwaMonitorStateBuilder { + flatbuffers::FlatBufferBuilder &fbb_; + flatbuffers::uoffset_t start_; + void add_measuredState(flatbuffers::Offset measuredState) { + fbb_.AddOffset(KUKAiiwaMonitorState::VT_MEASUREDSTATE, measuredState); + } + void add_cartesianFlangePose(const Pose *cartesianFlangePose) { + fbb_.AddStruct(KUKAiiwaMonitorState::VT_CARTESIANFLANGEPOSE, cartesianFlangePose); + } + void add_jointStateReal(flatbuffers::Offset jointStateReal) { + fbb_.AddOffset(KUKAiiwaMonitorState::VT_JOINTSTATEREAL, jointStateReal); + } + void add_jointStateInterpolated(flatbuffers::Offset jointStateInterpolated) { + fbb_.AddOffset(KUKAiiwaMonitorState::VT_JOINTSTATEINTERPOLATED, jointStateInterpolated); + } + void add_externalState(flatbuffers::Offset externalState) { + fbb_.AddOffset(KUKAiiwaMonitorState::VT_EXTERNALSTATE, externalState); + } + void add_operationMode(EOperationMode operationMode) { + fbb_.AddElement(KUKAiiwaMonitorState::VT_OPERATIONMODE, static_cast(operationMode), 0); + } + void add_sessionState(ESessionState sessionState) { + fbb_.AddElement(KUKAiiwaMonitorState::VT_SESSIONSTATE, static_cast(sessionState), 0); + } + void add_CartesianWrench(const Wrench *CartesianWrench) { + fbb_.AddStruct(KUKAiiwaMonitorState::VT_CARTESIANWRENCH, CartesianWrench); + } + explicit KUKAiiwaMonitorStateBuilder(flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + KUKAiiwaMonitorStateBuilder &operator=(const KUKAiiwaMonitorStateBuilder &); + flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = flatbuffers::Offset(end); + return o; + } +}; + +inline flatbuffers::Offset CreateKUKAiiwaMonitorState( + flatbuffers::FlatBufferBuilder &_fbb, + flatbuffers::Offset measuredState = 0, + const Pose *cartesianFlangePose = 0, + flatbuffers::Offset jointStateReal = 0, + flatbuffers::Offset jointStateInterpolated = 0, + flatbuffers::Offset externalState = 0, + EOperationMode operationMode = EOperationMode::TEST_MODE_1, + ESessionState sessionState = ESessionState::IDLE, + const Wrench *CartesianWrench = 0) { + KUKAiiwaMonitorStateBuilder builder_(_fbb); + builder_.add_CartesianWrench(CartesianWrench); + builder_.add_externalState(externalState); + builder_.add_jointStateInterpolated(jointStateInterpolated); + builder_.add_jointStateReal(jointStateReal); + builder_.add_cartesianFlangePose(cartesianFlangePose); + builder_.add_measuredState(measuredState); + builder_.add_sessionState(sessionState); + builder_.add_operationMode(operationMode); + return builder_.Finish(); +} + +flatbuffers::Offset CreateKUKAiiwaMonitorState(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaMonitorStateT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); + +struct FRITimeStampT : public flatbuffers::NativeTable { + typedef FRITimeStamp TableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.FRITimeStampT"; + } + int32_t sec; + int32_t nanosec; + FRITimeStampT() + : sec(0), + nanosec(0) { + } +}; + +struct FRITimeStamp FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { + typedef FRITimeStampT NativeTableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.FRITimeStamp"; + } + enum { + VT_SEC = 4, + VT_NANOSEC = 6 + }; + int32_t sec() const { + return GetField(VT_SEC, 0); + } + int32_t nanosec() const { + return GetField(VT_NANOSEC, 0); + } + bool Verify(flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + VerifyField(verifier, VT_SEC) && + VerifyField(verifier, VT_NANOSEC) && + verifier.EndTable(); + } + FRITimeStampT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; + void UnPackTo(FRITimeStampT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; + static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const FRITimeStampT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); +}; + +struct FRITimeStampBuilder { + flatbuffers::FlatBufferBuilder &fbb_; + flatbuffers::uoffset_t start_; + void add_sec(int32_t sec) { + fbb_.AddElement(FRITimeStamp::VT_SEC, sec, 0); + } + void add_nanosec(int32_t nanosec) { + fbb_.AddElement(FRITimeStamp::VT_NANOSEC, nanosec, 0); + } + explicit FRITimeStampBuilder(flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + FRITimeStampBuilder &operator=(const FRITimeStampBuilder &); + flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = flatbuffers::Offset(end); + return o; + } +}; + +inline flatbuffers::Offset CreateFRITimeStamp( + flatbuffers::FlatBufferBuilder &_fbb, + int32_t sec = 0, + int32_t nanosec = 0) { + FRITimeStampBuilder builder_(_fbb); + builder_.add_nanosec(nanosec); + builder_.add_sec(sec); + return builder_.Finish(); +} + +flatbuffers::Offset CreateFRITimeStamp(flatbuffers::FlatBufferBuilder &_fbb, const FRITimeStampT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); + +struct FRIMessageLogT : public flatbuffers::NativeTable { + typedef FRIMessageLog TableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.FRIMessageLogT"; + } + ESessionState sessionState; + EConnectionQuality connectionQuality; + EControlMode controlMode; + int32_t messageIdentifier; + int32_t sequenceCounter; + int32_t reflectedSequenceCounter; + std::vector measuredJointPosition; + std::vector measuredTorque; + std::vector commandedJointPosition; + std::vector commandedTorque; + std::vector externalTorque; + std::vector jointStateInterpolated; + std::unique_ptr timeStamp; + FRIMessageLogT() + : sessionState(ESessionState::IDLE), + connectionQuality(EConnectionQuality::POOR), + controlMode(EControlMode::POSITION_CONTROL_MODE), + messageIdentifier(0), + sequenceCounter(0), + reflectedSequenceCounter(0) { + } +}; + +struct FRIMessageLog FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { + typedef FRIMessageLogT NativeTableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.FRIMessageLog"; + } + enum { + VT_SESSIONSTATE = 4, + VT_CONNECTIONQUALITY = 6, + VT_CONTROLMODE = 8, + VT_MESSAGEIDENTIFIER = 10, + VT_SEQUENCECOUNTER = 12, + VT_REFLECTEDSEQUENCECOUNTER = 14, + VT_MEASUREDJOINTPOSITION = 16, + VT_MEASUREDTORQUE = 18, + VT_COMMANDEDJOINTPOSITION = 20, + VT_COMMANDEDTORQUE = 22, + VT_EXTERNALTORQUE = 24, + VT_JOINTSTATEINTERPOLATED = 26, + VT_TIMESTAMP = 28 + }; + ESessionState sessionState() const { + return static_cast(GetField(VT_SESSIONSTATE, 0)); + } + EConnectionQuality connectionQuality() const { + return static_cast(GetField(VT_CONNECTIONQUALITY, 0)); + } + EControlMode controlMode() const { + return static_cast(GetField(VT_CONTROLMODE, 0)); + } + int32_t messageIdentifier() const { + return GetField(VT_MESSAGEIDENTIFIER, 0); + } + int32_t sequenceCounter() const { + return GetField(VT_SEQUENCECOUNTER, 0); + } + int32_t reflectedSequenceCounter() const { + return GetField(VT_REFLECTEDSEQUENCECOUNTER, 0); + } + const flatbuffers::Vector *measuredJointPosition() const { + return GetPointer *>(VT_MEASUREDJOINTPOSITION); + } + const flatbuffers::Vector *measuredTorque() const { + return GetPointer *>(VT_MEASUREDTORQUE); + } + const flatbuffers::Vector *commandedJointPosition() const { + return GetPointer *>(VT_COMMANDEDJOINTPOSITION); + } + const flatbuffers::Vector *commandedTorque() const { + return GetPointer *>(VT_COMMANDEDTORQUE); + } + const flatbuffers::Vector *externalTorque() const { + return GetPointer *>(VT_EXTERNALTORQUE); + } + const flatbuffers::Vector *jointStateInterpolated() const { + return GetPointer *>(VT_JOINTSTATEINTERPOLATED); + } + const TimeEvent *timeStamp() const { + return GetPointer(VT_TIMESTAMP); + } + bool Verify(flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + VerifyField(verifier, VT_SESSIONSTATE) && + VerifyField(verifier, VT_CONNECTIONQUALITY) && + VerifyField(verifier, VT_CONTROLMODE) && + VerifyField(verifier, VT_MESSAGEIDENTIFIER) && + VerifyField(verifier, VT_SEQUENCECOUNTER) && + VerifyField(verifier, VT_REFLECTEDSEQUENCECOUNTER) && + VerifyOffset(verifier, VT_MEASUREDJOINTPOSITION) && + verifier.Verify(measuredJointPosition()) && + VerifyOffset(verifier, VT_MEASUREDTORQUE) && + verifier.Verify(measuredTorque()) && + VerifyOffset(verifier, VT_COMMANDEDJOINTPOSITION) && + verifier.Verify(commandedJointPosition()) && + VerifyOffset(verifier, VT_COMMANDEDTORQUE) && + verifier.Verify(commandedTorque()) && + VerifyOffset(verifier, VT_EXTERNALTORQUE) && + verifier.Verify(externalTorque()) && + VerifyOffset(verifier, VT_JOINTSTATEINTERPOLATED) && + verifier.Verify(jointStateInterpolated()) && + VerifyOffset(verifier, VT_TIMESTAMP) && + verifier.VerifyTable(timeStamp()) && + verifier.EndTable(); + } + FRIMessageLogT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; + void UnPackTo(FRIMessageLogT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; + static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const FRIMessageLogT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); +}; + +struct FRIMessageLogBuilder { + flatbuffers::FlatBufferBuilder &fbb_; + flatbuffers::uoffset_t start_; + void add_sessionState(ESessionState sessionState) { + fbb_.AddElement(FRIMessageLog::VT_SESSIONSTATE, static_cast(sessionState), 0); + } + void add_connectionQuality(EConnectionQuality connectionQuality) { + fbb_.AddElement(FRIMessageLog::VT_CONNECTIONQUALITY, static_cast(connectionQuality), 0); + } + void add_controlMode(EControlMode controlMode) { + fbb_.AddElement(FRIMessageLog::VT_CONTROLMODE, static_cast(controlMode), 0); + } + void add_messageIdentifier(int32_t messageIdentifier) { + fbb_.AddElement(FRIMessageLog::VT_MESSAGEIDENTIFIER, messageIdentifier, 0); + } + void add_sequenceCounter(int32_t sequenceCounter) { + fbb_.AddElement(FRIMessageLog::VT_SEQUENCECOUNTER, sequenceCounter, 0); + } + void add_reflectedSequenceCounter(int32_t reflectedSequenceCounter) { + fbb_.AddElement(FRIMessageLog::VT_REFLECTEDSEQUENCECOUNTER, reflectedSequenceCounter, 0); + } + void add_measuredJointPosition(flatbuffers::Offset> measuredJointPosition) { + fbb_.AddOffset(FRIMessageLog::VT_MEASUREDJOINTPOSITION, measuredJointPosition); + } + void add_measuredTorque(flatbuffers::Offset> measuredTorque) { + fbb_.AddOffset(FRIMessageLog::VT_MEASUREDTORQUE, measuredTorque); + } + void add_commandedJointPosition(flatbuffers::Offset> commandedJointPosition) { + fbb_.AddOffset(FRIMessageLog::VT_COMMANDEDJOINTPOSITION, commandedJointPosition); + } + void add_commandedTorque(flatbuffers::Offset> commandedTorque) { + fbb_.AddOffset(FRIMessageLog::VT_COMMANDEDTORQUE, commandedTorque); + } + void add_externalTorque(flatbuffers::Offset> externalTorque) { + fbb_.AddOffset(FRIMessageLog::VT_EXTERNALTORQUE, externalTorque); + } + void add_jointStateInterpolated(flatbuffers::Offset> jointStateInterpolated) { + fbb_.AddOffset(FRIMessageLog::VT_JOINTSTATEINTERPOLATED, jointStateInterpolated); + } + void add_timeStamp(flatbuffers::Offset timeStamp) { + fbb_.AddOffset(FRIMessageLog::VT_TIMESTAMP, timeStamp); + } + explicit FRIMessageLogBuilder(flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + FRIMessageLogBuilder &operator=(const FRIMessageLogBuilder &); + flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = flatbuffers::Offset(end); + return o; + } +}; + +inline flatbuffers::Offset CreateFRIMessageLog( + flatbuffers::FlatBufferBuilder &_fbb, + ESessionState sessionState = ESessionState::IDLE, + EConnectionQuality connectionQuality = EConnectionQuality::POOR, + EControlMode controlMode = EControlMode::POSITION_CONTROL_MODE, + int32_t messageIdentifier = 0, + int32_t sequenceCounter = 0, + int32_t reflectedSequenceCounter = 0, + flatbuffers::Offset> measuredJointPosition = 0, + flatbuffers::Offset> measuredTorque = 0, + flatbuffers::Offset> commandedJointPosition = 0, + flatbuffers::Offset> commandedTorque = 0, + flatbuffers::Offset> externalTorque = 0, + flatbuffers::Offset> jointStateInterpolated = 0, + flatbuffers::Offset timeStamp = 0) { + FRIMessageLogBuilder builder_(_fbb); + builder_.add_timeStamp(timeStamp); + builder_.add_jointStateInterpolated(jointStateInterpolated); + builder_.add_externalTorque(externalTorque); + builder_.add_commandedTorque(commandedTorque); + builder_.add_commandedJointPosition(commandedJointPosition); + builder_.add_measuredTorque(measuredTorque); + builder_.add_measuredJointPosition(measuredJointPosition); + builder_.add_reflectedSequenceCounter(reflectedSequenceCounter); + builder_.add_sequenceCounter(sequenceCounter); + builder_.add_messageIdentifier(messageIdentifier); + builder_.add_controlMode(controlMode); + builder_.add_connectionQuality(connectionQuality); + builder_.add_sessionState(sessionState); + return builder_.Finish(); +} + +inline flatbuffers::Offset CreateFRIMessageLogDirect( + flatbuffers::FlatBufferBuilder &_fbb, + ESessionState sessionState = ESessionState::IDLE, + EConnectionQuality connectionQuality = EConnectionQuality::POOR, + EControlMode controlMode = EControlMode::POSITION_CONTROL_MODE, + int32_t messageIdentifier = 0, + int32_t sequenceCounter = 0, + int32_t reflectedSequenceCounter = 0, + const std::vector *measuredJointPosition = nullptr, + const std::vector *measuredTorque = nullptr, + const std::vector *commandedJointPosition = nullptr, + const std::vector *commandedTorque = nullptr, + const std::vector *externalTorque = nullptr, + const std::vector *jointStateInterpolated = nullptr, + flatbuffers::Offset timeStamp = 0) { + return grl::flatbuffer::CreateFRIMessageLog( + _fbb, + sessionState, + connectionQuality, + controlMode, + messageIdentifier, + sequenceCounter, + reflectedSequenceCounter, + measuredJointPosition ? _fbb.CreateVector(*measuredJointPosition) : 0, + measuredTorque ? _fbb.CreateVector(*measuredTorque) : 0, + commandedJointPosition ? _fbb.CreateVector(*commandedJointPosition) : 0, + commandedTorque ? _fbb.CreateVector(*commandedTorque) : 0, + externalTorque ? _fbb.CreateVector(*externalTorque) : 0, + jointStateInterpolated ? _fbb.CreateVector(*jointStateInterpolated) : 0, + timeStamp); +} + +flatbuffers::Offset CreateFRIMessageLog(flatbuffers::FlatBufferBuilder &_fbb, const FRIMessageLogT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); + +struct KUKAiiwaStateT : public flatbuffers::NativeTable { + typedef KUKAiiwaState TableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.KUKAiiwaStateT"; + } + std::string name; + std::string destination; + std::string source; + std::unique_ptr timeStamp; + bool setArmControlState; + std::unique_ptr armControlState; + bool setArmConfiguration; + std::unique_ptr armConfiguration; + bool hasMonitorState; + std::unique_ptr monitorState; + bool hasMonitorConfig; + std::unique_ptr monitorConfig; + std::unique_ptr FRIMessage; + KUKAiiwaStateT() + : setArmControlState(false), + setArmConfiguration(false), + hasMonitorState(false), + hasMonitorConfig(false) { + } +}; + +struct KUKAiiwaState FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { + typedef KUKAiiwaStateT NativeTableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.KUKAiiwaState"; + } + enum { + VT_NAME = 4, + VT_DESTINATION = 6, + VT_SOURCE = 8, + VT_TIMESTAMP = 10, + VT_SETARMCONTROLSTATE = 12, + VT_ARMCONTROLSTATE = 14, + VT_SETARMCONFIGURATION = 16, + VT_ARMCONFIGURATION = 18, + VT_HASMONITORSTATE = 20, + VT_MONITORSTATE = 22, + VT_HASMONITORCONFIG = 24, + VT_MONITORCONFIG = 26, + VT_FRIMESSAGE = 28 + }; + const flatbuffers::String *name() const { + return GetPointer(VT_NAME); + } + const flatbuffers::String *destination() const { + return GetPointer(VT_DESTINATION); + } + const flatbuffers::String *source() const { + return GetPointer(VT_SOURCE); + } + const TimeEvent *timeStamp() const { + return GetPointer(VT_TIMESTAMP); + } + bool setArmControlState() const { + return GetField(VT_SETARMCONTROLSTATE, 0) != 0; + } + const ArmControlState *armControlState() const { + return GetPointer(VT_ARMCONTROLSTATE); + } + bool setArmConfiguration() const { + return GetField(VT_SETARMCONFIGURATION, 0) != 0; + } + const KUKAiiwaArmConfiguration *armConfiguration() const { + return GetPointer(VT_ARMCONFIGURATION); + } + bool hasMonitorState() const { + return GetField(VT_HASMONITORSTATE, 0) != 0; + } + const KUKAiiwaMonitorState *monitorState() const { + return GetPointer(VT_MONITORSTATE); + } + bool hasMonitorConfig() const { + return GetField(VT_HASMONITORCONFIG, 0) != 0; + } + const KUKAiiwaMonitorConfiguration *monitorConfig() const { + return GetPointer(VT_MONITORCONFIG); + } + const FRIMessageLog *FRIMessage() const { + return GetPointer(VT_FRIMESSAGE); + } + bool Verify(flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + VerifyOffset(verifier, VT_NAME) && + verifier.Verify(name()) && + VerifyOffset(verifier, VT_DESTINATION) && + verifier.Verify(destination()) && + VerifyOffset(verifier, VT_SOURCE) && + verifier.Verify(source()) && + VerifyOffset(verifier, VT_TIMESTAMP) && + verifier.VerifyTable(timeStamp()) && + VerifyField(verifier, VT_SETARMCONTROLSTATE) && + VerifyOffset(verifier, VT_ARMCONTROLSTATE) && + verifier.VerifyTable(armControlState()) && + VerifyField(verifier, VT_SETARMCONFIGURATION) && + VerifyOffset(verifier, VT_ARMCONFIGURATION) && + verifier.VerifyTable(armConfiguration()) && + VerifyField(verifier, VT_HASMONITORSTATE) && + VerifyOffset(verifier, VT_MONITORSTATE) && + verifier.VerifyTable(monitorState()) && + VerifyField(verifier, VT_HASMONITORCONFIG) && + VerifyOffset(verifier, VT_MONITORCONFIG) && + verifier.VerifyTable(monitorConfig()) && + VerifyOffset(verifier, VT_FRIMESSAGE) && + verifier.VerifyTable(FRIMessage()) && + verifier.EndTable(); + } + KUKAiiwaStateT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; + void UnPackTo(KUKAiiwaStateT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; + static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaStateT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); +}; + +struct KUKAiiwaStateBuilder { + flatbuffers::FlatBufferBuilder &fbb_; + flatbuffers::uoffset_t start_; + void add_name(flatbuffers::Offset name) { + fbb_.AddOffset(KUKAiiwaState::VT_NAME, name); + } + void add_destination(flatbuffers::Offset destination) { + fbb_.AddOffset(KUKAiiwaState::VT_DESTINATION, destination); + } + void add_source(flatbuffers::Offset source) { + fbb_.AddOffset(KUKAiiwaState::VT_SOURCE, source); + } + void add_timeStamp(flatbuffers::Offset timeStamp) { + fbb_.AddOffset(KUKAiiwaState::VT_TIMESTAMP, timeStamp); + } + void add_setArmControlState(bool setArmControlState) { + fbb_.AddElement(KUKAiiwaState::VT_SETARMCONTROLSTATE, static_cast(setArmControlState), 0); + } + void add_armControlState(flatbuffers::Offset armControlState) { + fbb_.AddOffset(KUKAiiwaState::VT_ARMCONTROLSTATE, armControlState); + } + void add_setArmConfiguration(bool setArmConfiguration) { + fbb_.AddElement(KUKAiiwaState::VT_SETARMCONFIGURATION, static_cast(setArmConfiguration), 0); + } + void add_armConfiguration(flatbuffers::Offset armConfiguration) { + fbb_.AddOffset(KUKAiiwaState::VT_ARMCONFIGURATION, armConfiguration); + } + void add_hasMonitorState(bool hasMonitorState) { + fbb_.AddElement(KUKAiiwaState::VT_HASMONITORSTATE, static_cast(hasMonitorState), 0); + } + void add_monitorState(flatbuffers::Offset monitorState) { + fbb_.AddOffset(KUKAiiwaState::VT_MONITORSTATE, monitorState); + } + void add_hasMonitorConfig(bool hasMonitorConfig) { + fbb_.AddElement(KUKAiiwaState::VT_HASMONITORCONFIG, static_cast(hasMonitorConfig), 0); + } + void add_monitorConfig(flatbuffers::Offset monitorConfig) { + fbb_.AddOffset(KUKAiiwaState::VT_MONITORCONFIG, monitorConfig); + } + void add_FRIMessage(flatbuffers::Offset FRIMessage) { + fbb_.AddOffset(KUKAiiwaState::VT_FRIMESSAGE, FRIMessage); + } + explicit KUKAiiwaStateBuilder(flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + KUKAiiwaStateBuilder &operator=(const KUKAiiwaStateBuilder &); + flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = flatbuffers::Offset(end); + return o; + } +}; + +inline flatbuffers::Offset CreateKUKAiiwaState( + flatbuffers::FlatBufferBuilder &_fbb, + flatbuffers::Offset name = 0, + flatbuffers::Offset destination = 0, + flatbuffers::Offset source = 0, + flatbuffers::Offset timeStamp = 0, + bool setArmControlState = false, + flatbuffers::Offset armControlState = 0, + bool setArmConfiguration = false, + flatbuffers::Offset armConfiguration = 0, + bool hasMonitorState = false, + flatbuffers::Offset monitorState = 0, + bool hasMonitorConfig = false, + flatbuffers::Offset monitorConfig = 0, + flatbuffers::Offset FRIMessage = 0) { + KUKAiiwaStateBuilder builder_(_fbb); + builder_.add_FRIMessage(FRIMessage); + builder_.add_monitorConfig(monitorConfig); + builder_.add_monitorState(monitorState); + builder_.add_armConfiguration(armConfiguration); + builder_.add_armControlState(armControlState); + builder_.add_timeStamp(timeStamp); + builder_.add_source(source); + builder_.add_destination(destination); + builder_.add_name(name); + builder_.add_hasMonitorConfig(hasMonitorConfig); + builder_.add_hasMonitorState(hasMonitorState); + builder_.add_setArmConfiguration(setArmConfiguration); + builder_.add_setArmControlState(setArmControlState); + return builder_.Finish(); +} + +inline flatbuffers::Offset CreateKUKAiiwaStateDirect( + flatbuffers::FlatBufferBuilder &_fbb, + const char *name = nullptr, + const char *destination = nullptr, + const char *source = nullptr, + flatbuffers::Offset timeStamp = 0, + bool setArmControlState = false, + flatbuffers::Offset armControlState = 0, + bool setArmConfiguration = false, + flatbuffers::Offset armConfiguration = 0, + bool hasMonitorState = false, + flatbuffers::Offset monitorState = 0, + bool hasMonitorConfig = false, + flatbuffers::Offset monitorConfig = 0, + flatbuffers::Offset FRIMessage = 0) { + return grl::flatbuffer::CreateKUKAiiwaState( + _fbb, + name ? _fbb.CreateString(name) : 0, + destination ? _fbb.CreateString(destination) : 0, + source ? _fbb.CreateString(source) : 0, + timeStamp, + setArmControlState, + armControlState, + setArmConfiguration, + armConfiguration, + hasMonitorState, + monitorState, + hasMonitorConfig, + monitorConfig, + FRIMessage); +} + +flatbuffers::Offset CreateKUKAiiwaState(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaStateT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); + +struct KUKAiiwaStatesT : public flatbuffers::NativeTable { + typedef KUKAiiwaStates TableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.KUKAiiwaStatesT"; + } + std::vector> states; + KUKAiiwaStatesT() { + } +}; + +struct KUKAiiwaStates FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { + typedef KUKAiiwaStatesT NativeTableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.KUKAiiwaStates"; + } + enum { + VT_STATES = 4 + }; + const flatbuffers::Vector> *states() const { + return GetPointer> *>(VT_STATES); + } + bool Verify(flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + VerifyOffset(verifier, VT_STATES) && + verifier.Verify(states()) && + verifier.VerifyVectorOfTables(states()) && + verifier.EndTable(); + } + KUKAiiwaStatesT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; + void UnPackTo(KUKAiiwaStatesT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; + static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaStatesT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); +}; + +struct KUKAiiwaStatesBuilder { + flatbuffers::FlatBufferBuilder &fbb_; + flatbuffers::uoffset_t start_; + void add_states(flatbuffers::Offset>> states) { + fbb_.AddOffset(KUKAiiwaStates::VT_STATES, states); + } + explicit KUKAiiwaStatesBuilder(flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + KUKAiiwaStatesBuilder &operator=(const KUKAiiwaStatesBuilder &); + flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = flatbuffers::Offset(end); + return o; + } +}; + +inline flatbuffers::Offset CreateKUKAiiwaStates( + flatbuffers::FlatBufferBuilder &_fbb, + flatbuffers::Offset>> states = 0) { + KUKAiiwaStatesBuilder builder_(_fbb); + builder_.add_states(states); + return builder_.Finish(); +} + +inline flatbuffers::Offset CreateKUKAiiwaStatesDirect( + flatbuffers::FlatBufferBuilder &_fbb, + const std::vector> *states = nullptr) { + return grl::flatbuffer::CreateKUKAiiwaStates( + _fbb, + states ? _fbb.CreateVector>(*states) : 0); +} + +flatbuffers::Offset CreateKUKAiiwaStates(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaStatesT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); + +inline CartesianImpedenceControlModeT *CartesianImpedenceControlMode::UnPack(const flatbuffers::resolver_function_t *_resolver) const { + auto _o = new CartesianImpedenceControlModeT(); + UnPackTo(_o, _resolver); + return _o; +} + +inline void CartesianImpedenceControlMode::UnPackTo(CartesianImpedenceControlModeT *_o, const flatbuffers::resolver_function_t *_resolver) const { + (void)_o; + (void)_resolver; + { auto _e = stiffness(); if (_e) _o->stiffness = std::unique_ptr(new EulerPose(*_e)); }; + { auto _e = damping(); if (_e) _o->damping = std::unique_ptr(new EulerPose(*_e)); }; + { auto _e = nullspaceStiffness(); _o->nullspaceStiffness = _e; }; + { auto _e = nullspaceDamping(); _o->nullspaceDamping = _e; }; + { auto _e = maxPathDeviation(); if (_e) _o->maxPathDeviation = std::unique_ptr(new EulerPose(*_e)); }; + { auto _e = maxCartesianVelocity(); if (_e) _o->maxCartesianVelocity = std::unique_ptr(new EulerPose(*_e)); }; + { auto _e = maxControlForce(); if (_e) _o->maxControlForce = std::unique_ptr(new EulerPose(*_e)); }; + { auto _e = maxControlForceExceededStop(); _o->maxControlForceExceededStop = _e; }; +} + +inline flatbuffers::Offset CartesianImpedenceControlMode::Pack(flatbuffers::FlatBufferBuilder &_fbb, const CartesianImpedenceControlModeT* _o, const flatbuffers::rehasher_function_t *_rehasher) { + return CreateCartesianImpedenceControlMode(_fbb, _o, _rehasher); +} + +inline flatbuffers::Offset CreateCartesianImpedenceControlMode(flatbuffers::FlatBufferBuilder &_fbb, const CartesianImpedenceControlModeT *_o, const flatbuffers::rehasher_function_t *_rehasher) { + (void)_rehasher; + (void)_o; + struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const CartesianImpedenceControlModeT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; + auto _stiffness = _o->stiffness ? _o->stiffness.get() : 0; + auto _damping = _o->damping ? _o->damping.get() : 0; + auto _nullspaceStiffness = _o->nullspaceStiffness; + auto _nullspaceDamping = _o->nullspaceDamping; + auto _maxPathDeviation = _o->maxPathDeviation ? _o->maxPathDeviation.get() : 0; + auto _maxCartesianVelocity = _o->maxCartesianVelocity ? _o->maxCartesianVelocity.get() : 0; + auto _maxControlForce = _o->maxControlForce ? _o->maxControlForce.get() : 0; + auto _maxControlForceExceededStop = _o->maxControlForceExceededStop; + return grl::flatbuffer::CreateCartesianImpedenceControlMode( + _fbb, + _stiffness, + _damping, + _nullspaceStiffness, + _nullspaceDamping, + _maxPathDeviation, + _maxCartesianVelocity, + _maxControlForce, + _maxControlForceExceededStop); +} + +inline JointImpedenceControlModeT *JointImpedenceControlMode::UnPack(const flatbuffers::resolver_function_t *_resolver) const { + auto _o = new JointImpedenceControlModeT(); + UnPackTo(_o, _resolver); + return _o; +} + +inline void JointImpedenceControlMode::UnPackTo(JointImpedenceControlModeT *_o, const flatbuffers::resolver_function_t *_resolver) const { + (void)_o; + (void)_resolver; + { auto _e = stiffness(); if (_e) { _o->stiffness.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->stiffness[_i] = _e->Get(_i); } } }; + { auto _e = damping(); if (_e) { _o->damping.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->damping[_i] = _e->Get(_i); } } }; +} + +inline flatbuffers::Offset JointImpedenceControlMode::Pack(flatbuffers::FlatBufferBuilder &_fbb, const JointImpedenceControlModeT* _o, const flatbuffers::rehasher_function_t *_rehasher) { + return CreateJointImpedenceControlMode(_fbb, _o, _rehasher); +} + +inline flatbuffers::Offset CreateJointImpedenceControlMode(flatbuffers::FlatBufferBuilder &_fbb, const JointImpedenceControlModeT *_o, const flatbuffers::rehasher_function_t *_rehasher) { + (void)_rehasher; + (void)_o; + struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const JointImpedenceControlModeT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; + auto _stiffness = _o->stiffness.size() ? _fbb.CreateVector(_o->stiffness) : 0; + auto _damping = _o->damping.size() ? _fbb.CreateVector(_o->damping) : 0; + return grl::flatbuffer::CreateJointImpedenceControlMode( + _fbb, + _stiffness, + _damping); +} + +inline DisabledT *Disabled::UnPack(const flatbuffers::resolver_function_t *_resolver) const { + auto _o = new DisabledT(); + UnPackTo(_o, _resolver); + return _o; +} + +inline void Disabled::UnPackTo(DisabledT *_o, const flatbuffers::resolver_function_t *_resolver) const { + (void)_o; + (void)_resolver; +} + +inline flatbuffers::Offset Disabled::Pack(flatbuffers::FlatBufferBuilder &_fbb, const DisabledT* _o, const flatbuffers::rehasher_function_t *_rehasher) { + return CreateDisabled(_fbb, _o, _rehasher); +} + +inline flatbuffers::Offset CreateDisabled(flatbuffers::FlatBufferBuilder &_fbb, const DisabledT *_o, const flatbuffers::rehasher_function_t *_rehasher) { + (void)_rehasher; + (void)_o; + struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const DisabledT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; + return grl::flatbuffer::CreateDisabled( + _fbb); +} + +inline FRIT *FRI::UnPack(const flatbuffers::resolver_function_t *_resolver) const { + auto _o = new FRIT(); + UnPackTo(_o, _resolver); + return _o; +} + +inline void FRI::UnPackTo(FRIT *_o, const flatbuffers::resolver_function_t *_resolver) const { + (void)_o; + (void)_resolver; + { auto _e = overlayType(); _o->overlayType = _e; }; + { auto _e = sendPeriodMillisec(); _o->sendPeriodMillisec = _e; }; + { auto _e = setReceiveMultiplier(); _o->setReceiveMultiplier = _e; }; + { auto _e = updatePortOnRemote(); _o->updatePortOnRemote = _e; }; + { auto _e = portOnRemote(); _o->portOnRemote = _e; }; + { auto _e = updatePortOnController(); _o->updatePortOnController = _e; }; + { auto _e = portOnController(); _o->portOnController = _e; }; +} + +inline flatbuffers::Offset FRI::Pack(flatbuffers::FlatBufferBuilder &_fbb, const FRIT* _o, const flatbuffers::rehasher_function_t *_rehasher) { + return CreateFRI(_fbb, _o, _rehasher); +} + +inline flatbuffers::Offset CreateFRI(flatbuffers::FlatBufferBuilder &_fbb, const FRIT *_o, const flatbuffers::rehasher_function_t *_rehasher) { + (void)_rehasher; + (void)_o; + struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const FRIT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; + auto _overlayType = _o->overlayType; + auto _sendPeriodMillisec = _o->sendPeriodMillisec; + auto _setReceiveMultiplier = _o->setReceiveMultiplier; + auto _updatePortOnRemote = _o->updatePortOnRemote; + auto _portOnRemote = _o->portOnRemote; + auto _updatePortOnController = _o->updatePortOnController; + auto _portOnController = _o->portOnController; + return grl::flatbuffer::CreateFRI( + _fbb, + _overlayType, + _sendPeriodMillisec, + _setReceiveMultiplier, + _updatePortOnRemote, + _portOnRemote, + _updatePortOnController, + _portOnController); +} + +inline SmartServoT *SmartServo::UnPack(const flatbuffers::resolver_function_t *_resolver) const { + auto _o = new SmartServoT(); + UnPackTo(_o, _resolver); + return _o; +} + +inline void SmartServo::UnPackTo(SmartServoT *_o, const flatbuffers::resolver_function_t *_resolver) const { + (void)_o; + (void)_resolver; + { auto _e = jointAccelerationRel(); if (_e) { _o->jointAccelerationRel.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->jointAccelerationRel[_i] = _e->Get(_i); } } }; + { auto _e = jointVelocityRel(); if (_e) { _o->jointVelocityRel.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->jointVelocityRel[_i] = _e->Get(_i); } } }; + { auto _e = updateMinimumTrajectoryExecutionTime(); _o->updateMinimumTrajectoryExecutionTime = _e; }; + { auto _e = minimumTrajectoryExecutionTime(); _o->minimumTrajectoryExecutionTime = _e; }; +} + +inline flatbuffers::Offset SmartServo::Pack(flatbuffers::FlatBufferBuilder &_fbb, const SmartServoT* _o, const flatbuffers::rehasher_function_t *_rehasher) { + return CreateSmartServo(_fbb, _o, _rehasher); +} + +inline flatbuffers::Offset CreateSmartServo(flatbuffers::FlatBufferBuilder &_fbb, const SmartServoT *_o, const flatbuffers::rehasher_function_t *_rehasher) { + (void)_rehasher; + (void)_o; + struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const SmartServoT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; + auto _jointAccelerationRel = _o->jointAccelerationRel.size() ? _fbb.CreateVector(_o->jointAccelerationRel) : 0; + auto _jointVelocityRel = _o->jointVelocityRel.size() ? _fbb.CreateVector(_o->jointVelocityRel) : 0; + auto _updateMinimumTrajectoryExecutionTime = _o->updateMinimumTrajectoryExecutionTime; + auto _minimumTrajectoryExecutionTime = _o->minimumTrajectoryExecutionTime; + return grl::flatbuffer::CreateSmartServo( + _fbb, + _jointAccelerationRel, + _jointVelocityRel, + _updateMinimumTrajectoryExecutionTime, + _minimumTrajectoryExecutionTime); +} + +inline ProcessDataT *ProcessData::UnPack(const flatbuffers::resolver_function_t *_resolver) const { + auto _o = new ProcessDataT(); + UnPackTo(_o, _resolver); + return _o; +} + +inline void ProcessData::UnPackTo(ProcessDataT *_o, const flatbuffers::resolver_function_t *_resolver) const { + (void)_o; + (void)_resolver; + { auto _e = dataType(); if (_e) _o->dataType = _e->str(); }; + { auto _e = defaultValue(); if (_e) _o->defaultValue = _e->str(); }; + { auto _e = displayName(); if (_e) _o->displayName = _e->str(); }; + { auto _e = id(); if (_e) _o->id = _e->str(); }; + { auto _e = min(); if (_e) _o->min = _e->str(); }; + { auto _e = max(); if (_e) _o->max = _e->str(); }; + { auto _e = unit(); if (_e) _o->unit = _e->str(); }; + { auto _e = value(); if (_e) _o->value = _e->str(); }; + { auto _e = shouldRemove(); _o->shouldRemove = _e; }; + { auto _e = shouldUpdate(); _o->shouldUpdate = _e; }; +} + +inline flatbuffers::Offset ProcessData::Pack(flatbuffers::FlatBufferBuilder &_fbb, const ProcessDataT* _o, const flatbuffers::rehasher_function_t *_rehasher) { + return CreateProcessData(_fbb, _o, _rehasher); +} + +inline flatbuffers::Offset CreateProcessData(flatbuffers::FlatBufferBuilder &_fbb, const ProcessDataT *_o, const flatbuffers::rehasher_function_t *_rehasher) { + (void)_rehasher; + (void)_o; + struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const ProcessDataT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; + auto _dataType = _o->dataType.empty() ? 0 : _fbb.CreateString(_o->dataType); + auto _defaultValue = _o->defaultValue.empty() ? 0 : _fbb.CreateString(_o->defaultValue); + auto _displayName = _o->displayName.empty() ? 0 : _fbb.CreateString(_o->displayName); + auto _id = _o->id.empty() ? 0 : _fbb.CreateString(_o->id); + auto _min = _o->min.empty() ? 0 : _fbb.CreateString(_o->min); + auto _max = _o->max.empty() ? 0 : _fbb.CreateString(_o->max); + auto _unit = _o->unit.empty() ? 0 : _fbb.CreateString(_o->unit); + auto _value = _o->value.empty() ? 0 : _fbb.CreateString(_o->value); + auto _shouldRemove = _o->shouldRemove; + auto _shouldUpdate = _o->shouldUpdate; + return grl::flatbuffer::CreateProcessData( + _fbb, + _dataType, + _defaultValue, + _displayName, + _id, + _min, + _max, + _unit, + _value, + _shouldRemove, + _shouldUpdate); +} + +inline KUKAiiwaArmConfigurationT *KUKAiiwaArmConfiguration::UnPack(const flatbuffers::resolver_function_t *_resolver) const { + auto _o = new KUKAiiwaArmConfigurationT(); + UnPackTo(_o, _resolver); + return _o; +} + +inline void KUKAiiwaArmConfiguration::UnPackTo(KUKAiiwaArmConfigurationT *_o, const flatbuffers::resolver_function_t *_resolver) const { + (void)_o; + (void)_resolver; + { auto _e = name(); if (_e) _o->name = _e->str(); }; + { auto _e = commandInterface(); _o->commandInterface = _e; }; + { auto _e = monitorInterface(); _o->monitorInterface = _e; }; + { auto _e = clientCommandMode(); _o->clientCommandMode = _e; }; + { auto _e = overlayType(); _o->overlayType = _e; }; + { auto _e = controlMode(); _o->controlMode = _e; }; + { auto _e = setCartImpedance(); if (_e) _o->setCartImpedance = std::unique_ptr(_e->UnPack(_resolver)); }; + { auto _e = setJointImpedance(); if (_e) _o->setJointImpedance = std::unique_ptr(_e->UnPack(_resolver)); }; + { auto _e = smartServoConfig(); if (_e) _o->smartServoConfig = std::unique_ptr(_e->UnPack(_resolver)); }; + { auto _e = FRIConfig(); if (_e) _o->FRIConfig = std::unique_ptr(_e->UnPack(_resolver)); }; + { auto _e = tools(); if (_e) { _o->tools.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->tools[_i] = std::unique_ptr(_e->Get(_i)->UnPack(_resolver)); } } }; + { auto _e = processData(); if (_e) { _o->processData.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->processData[_i] = std::unique_ptr(_e->Get(_i)->UnPack(_resolver)); } } }; + { auto _e = currentMotionCenter(); if (_e) _o->currentMotionCenter = _e->str(); }; + { auto _e = requestMonitorProcessData(); _o->requestMonitorProcessData = _e; }; +} + +inline flatbuffers::Offset KUKAiiwaArmConfiguration::Pack(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaArmConfigurationT* _o, const flatbuffers::rehasher_function_t *_rehasher) { + return CreateKUKAiiwaArmConfiguration(_fbb, _o, _rehasher); +} + +inline flatbuffers::Offset CreateKUKAiiwaArmConfiguration(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaArmConfigurationT *_o, const flatbuffers::rehasher_function_t *_rehasher) { + (void)_rehasher; + (void)_o; + struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const KUKAiiwaArmConfigurationT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; + auto _name = _o->name.empty() ? 0 : _fbb.CreateString(_o->name); + auto _commandInterface = _o->commandInterface; + auto _monitorInterface = _o->monitorInterface; + auto _clientCommandMode = _o->clientCommandMode; + auto _overlayType = _o->overlayType; + auto _controlMode = _o->controlMode; + auto _setCartImpedance = _o->setCartImpedance ? CreateCartesianImpedenceControlMode(_fbb, _o->setCartImpedance.get(), _rehasher) : 0; + auto _setJointImpedance = _o->setJointImpedance ? CreateJointImpedenceControlMode(_fbb, _o->setJointImpedance.get(), _rehasher) : 0; + auto _smartServoConfig = _o->smartServoConfig ? CreateSmartServo(_fbb, _o->smartServoConfig.get(), _rehasher) : 0; + auto _FRIConfig = _o->FRIConfig ? CreateFRI(_fbb, _o->FRIConfig.get(), _rehasher) : 0; + auto _tools = _o->tools.size() ? _fbb.CreateVector> (_o->tools.size(), [](size_t i, _VectorArgs *__va) { return CreateLinkObject(*__va->__fbb, __va->__o->tools[i].get(), __va->__rehasher); }, &_va ) : 0; + auto _processData = _o->processData.size() ? _fbb.CreateVector> (_o->processData.size(), [](size_t i, _VectorArgs *__va) { return CreateProcessData(*__va->__fbb, __va->__o->processData[i].get(), __va->__rehasher); }, &_va ) : 0; + auto _currentMotionCenter = _o->currentMotionCenter.empty() ? 0 : _fbb.CreateString(_o->currentMotionCenter); + auto _requestMonitorProcessData = _o->requestMonitorProcessData; + return grl::flatbuffer::CreateKUKAiiwaArmConfiguration( + _fbb, + _name, + _commandInterface, + _monitorInterface, + _clientCommandMode, + _overlayType, + _controlMode, + _setCartImpedance, + _setJointImpedance, + _smartServoConfig, + _FRIConfig, + _tools, + _processData, + _currentMotionCenter, + _requestMonitorProcessData); +} + +inline KUKAiiwaMonitorConfigurationT *KUKAiiwaMonitorConfiguration::UnPack(const flatbuffers::resolver_function_t *_resolver) const { + auto _o = new KUKAiiwaMonitorConfigurationT(); + UnPackTo(_o, _resolver); + return _o; +} + +inline void KUKAiiwaMonitorConfiguration::UnPackTo(KUKAiiwaMonitorConfigurationT *_o, const flatbuffers::resolver_function_t *_resolver) const { + (void)_o; + (void)_resolver; + { auto _e = hardwareVersion(); if (_e) _o->hardwareVersion = _e->str(); }; + { auto _e = torqueSensorLimits(); if (_e) { _o->torqueSensorLimits.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->torqueSensorLimits[_i] = _e->Get(_i); } } }; + { auto _e = isReadyToMove(); _o->isReadyToMove = _e; }; + { auto _e = isMastered(); _o->isMastered = _e; }; + { auto _e = processData(); if (_e) { _o->processData.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->processData[_i] = std::unique_ptr(_e->Get(_i)->UnPack(_resolver)); } } }; +} + +inline flatbuffers::Offset KUKAiiwaMonitorConfiguration::Pack(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaMonitorConfigurationT* _o, const flatbuffers::rehasher_function_t *_rehasher) { + return CreateKUKAiiwaMonitorConfiguration(_fbb, _o, _rehasher); +} + +inline flatbuffers::Offset CreateKUKAiiwaMonitorConfiguration(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaMonitorConfigurationT *_o, const flatbuffers::rehasher_function_t *_rehasher) { + (void)_rehasher; + (void)_o; + struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const KUKAiiwaMonitorConfigurationT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; + auto _hardwareVersion = _o->hardwareVersion.empty() ? 0 : _fbb.CreateString(_o->hardwareVersion); + auto _torqueSensorLimits = _o->torqueSensorLimits.size() ? _fbb.CreateVector(_o->torqueSensorLimits) : 0; + auto _isReadyToMove = _o->isReadyToMove; + auto _isMastered = _o->isMastered; + auto _processData = _o->processData.size() ? _fbb.CreateVector> (_o->processData.size(), [](size_t i, _VectorArgs *__va) { return CreateProcessData(*__va->__fbb, __va->__o->processData[i].get(), __va->__rehasher); }, &_va ) : 0; + return grl::flatbuffer::CreateKUKAiiwaMonitorConfiguration( + _fbb, + _hardwareVersion, + _torqueSensorLimits, + _isReadyToMove, + _isMastered, + _processData); +} + +inline KUKAiiwaMonitorStateT *KUKAiiwaMonitorState::UnPack(const flatbuffers::resolver_function_t *_resolver) const { + auto _o = new KUKAiiwaMonitorStateT(); + UnPackTo(_o, _resolver); + return _o; +} + +inline void KUKAiiwaMonitorState::UnPackTo(KUKAiiwaMonitorStateT *_o, const flatbuffers::resolver_function_t *_resolver) const { + (void)_o; + (void)_resolver; + { auto _e = measuredState(); if (_e) _o->measuredState = std::unique_ptr(_e->UnPack(_resolver)); }; + { auto _e = cartesianFlangePose(); if (_e) _o->cartesianFlangePose = std::unique_ptr(new Pose(*_e)); }; + { auto _e = jointStateReal(); if (_e) _o->jointStateReal = std::unique_ptr(_e->UnPack(_resolver)); }; + { auto _e = jointStateInterpolated(); if (_e) _o->jointStateInterpolated = std::unique_ptr(_e->UnPack(_resolver)); }; + { auto _e = externalState(); if (_e) _o->externalState = std::unique_ptr(_e->UnPack(_resolver)); }; + { auto _e = operationMode(); _o->operationMode = _e; }; + { auto _e = sessionState(); _o->sessionState = _e; }; + { auto _e = CartesianWrench(); if (_e) _o->CartesianWrench = std::unique_ptr(new Wrench(*_e)); }; +} + +inline flatbuffers::Offset KUKAiiwaMonitorState::Pack(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaMonitorStateT* _o, const flatbuffers::rehasher_function_t *_rehasher) { + return CreateKUKAiiwaMonitorState(_fbb, _o, _rehasher); +} + +inline flatbuffers::Offset CreateKUKAiiwaMonitorState(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaMonitorStateT *_o, const flatbuffers::rehasher_function_t *_rehasher) { + (void)_rehasher; + (void)_o; + struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const KUKAiiwaMonitorStateT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; + auto _measuredState = _o->measuredState ? CreateJointState(_fbb, _o->measuredState.get(), _rehasher) : 0; + auto _cartesianFlangePose = _o->cartesianFlangePose ? _o->cartesianFlangePose.get() : 0; + auto _jointStateReal = _o->jointStateReal ? CreateJointState(_fbb, _o->jointStateReal.get(), _rehasher) : 0; + auto _jointStateInterpolated = _o->jointStateInterpolated ? CreateJointState(_fbb, _o->jointStateInterpolated.get(), _rehasher) : 0; + auto _externalState = _o->externalState ? CreateJointState(_fbb, _o->externalState.get(), _rehasher) : 0; + auto _operationMode = _o->operationMode; + auto _sessionState = _o->sessionState; + auto _CartesianWrench = _o->CartesianWrench ? _o->CartesianWrench.get() : 0; + return grl::flatbuffer::CreateKUKAiiwaMonitorState( + _fbb, + _measuredState, + _cartesianFlangePose, + _jointStateReal, + _jointStateInterpolated, + _externalState, + _operationMode, + _sessionState, + _CartesianWrench); +} + +inline FRITimeStampT *FRITimeStamp::UnPack(const flatbuffers::resolver_function_t *_resolver) const { + auto _o = new FRITimeStampT(); + UnPackTo(_o, _resolver); + return _o; +} + +inline void FRITimeStamp::UnPackTo(FRITimeStampT *_o, const flatbuffers::resolver_function_t *_resolver) const { + (void)_o; + (void)_resolver; + { auto _e = sec(); _o->sec = _e; }; + { auto _e = nanosec(); _o->nanosec = _e; }; +} + +inline flatbuffers::Offset FRITimeStamp::Pack(flatbuffers::FlatBufferBuilder &_fbb, const FRITimeStampT* _o, const flatbuffers::rehasher_function_t *_rehasher) { + return CreateFRITimeStamp(_fbb, _o, _rehasher); +} + +inline flatbuffers::Offset CreateFRITimeStamp(flatbuffers::FlatBufferBuilder &_fbb, const FRITimeStampT *_o, const flatbuffers::rehasher_function_t *_rehasher) { + (void)_rehasher; + (void)_o; + struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const FRITimeStampT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; + auto _sec = _o->sec; + auto _nanosec = _o->nanosec; + return grl::flatbuffer::CreateFRITimeStamp( + _fbb, + _sec, + _nanosec); +} + +inline FRIMessageLogT *FRIMessageLog::UnPack(const flatbuffers::resolver_function_t *_resolver) const { + auto _o = new FRIMessageLogT(); + UnPackTo(_o, _resolver); + return _o; +} + +inline void FRIMessageLog::UnPackTo(FRIMessageLogT *_o, const flatbuffers::resolver_function_t *_resolver) const { + (void)_o; + (void)_resolver; + { auto _e = sessionState(); _o->sessionState = _e; }; + { auto _e = connectionQuality(); _o->connectionQuality = _e; }; + { auto _e = controlMode(); _o->controlMode = _e; }; + { auto _e = messageIdentifier(); _o->messageIdentifier = _e; }; + { auto _e = sequenceCounter(); _o->sequenceCounter = _e; }; + { auto _e = reflectedSequenceCounter(); _o->reflectedSequenceCounter = _e; }; + { auto _e = measuredJointPosition(); if (_e) { _o->measuredJointPosition.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->measuredJointPosition[_i] = _e->Get(_i); } } }; + { auto _e = measuredTorque(); if (_e) { _o->measuredTorque.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->measuredTorque[_i] = _e->Get(_i); } } }; + { auto _e = commandedJointPosition(); if (_e) { _o->commandedJointPosition.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->commandedJointPosition[_i] = _e->Get(_i); } } }; + { auto _e = commandedTorque(); if (_e) { _o->commandedTorque.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->commandedTorque[_i] = _e->Get(_i); } } }; + { auto _e = externalTorque(); if (_e) { _o->externalTorque.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->externalTorque[_i] = _e->Get(_i); } } }; + { auto _e = jointStateInterpolated(); if (_e) { _o->jointStateInterpolated.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->jointStateInterpolated[_i] = _e->Get(_i); } } }; + { auto _e = timeStamp(); if (_e) _o->timeStamp = std::unique_ptr(_e->UnPack(_resolver)); }; +} + +inline flatbuffers::Offset FRIMessageLog::Pack(flatbuffers::FlatBufferBuilder &_fbb, const FRIMessageLogT* _o, const flatbuffers::rehasher_function_t *_rehasher) { + return CreateFRIMessageLog(_fbb, _o, _rehasher); +} + +inline flatbuffers::Offset CreateFRIMessageLog(flatbuffers::FlatBufferBuilder &_fbb, const FRIMessageLogT *_o, const flatbuffers::rehasher_function_t *_rehasher) { + (void)_rehasher; + (void)_o; + struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const FRIMessageLogT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; + auto _sessionState = _o->sessionState; + auto _connectionQuality = _o->connectionQuality; + auto _controlMode = _o->controlMode; + auto _messageIdentifier = _o->messageIdentifier; + auto _sequenceCounter = _o->sequenceCounter; + auto _reflectedSequenceCounter = _o->reflectedSequenceCounter; + auto _measuredJointPosition = _o->measuredJointPosition.size() ? _fbb.CreateVector(_o->measuredJointPosition) : 0; + auto _measuredTorque = _o->measuredTorque.size() ? _fbb.CreateVector(_o->measuredTorque) : 0; + auto _commandedJointPosition = _o->commandedJointPosition.size() ? _fbb.CreateVector(_o->commandedJointPosition) : 0; + auto _commandedTorque = _o->commandedTorque.size() ? _fbb.CreateVector(_o->commandedTorque) : 0; + auto _externalTorque = _o->externalTorque.size() ? _fbb.CreateVector(_o->externalTorque) : 0; + auto _jointStateInterpolated = _o->jointStateInterpolated.size() ? _fbb.CreateVector(_o->jointStateInterpolated) : 0; + auto _timeStamp = _o->timeStamp ? CreateTimeEvent(_fbb, _o->timeStamp.get(), _rehasher) : 0; + return grl::flatbuffer::CreateFRIMessageLog( + _fbb, + _sessionState, + _connectionQuality, + _controlMode, + _messageIdentifier, + _sequenceCounter, + _reflectedSequenceCounter, + _measuredJointPosition, + _measuredTorque, + _commandedJointPosition, + _commandedTorque, + _externalTorque, + _jointStateInterpolated, + _timeStamp); +} + +inline KUKAiiwaStateT *KUKAiiwaState::UnPack(const flatbuffers::resolver_function_t *_resolver) const { + auto _o = new KUKAiiwaStateT(); + UnPackTo(_o, _resolver); + return _o; +} + +inline void KUKAiiwaState::UnPackTo(KUKAiiwaStateT *_o, const flatbuffers::resolver_function_t *_resolver) const { + (void)_o; + (void)_resolver; + { auto _e = name(); if (_e) _o->name = _e->str(); }; + { auto _e = destination(); if (_e) _o->destination = _e->str(); }; + { auto _e = source(); if (_e) _o->source = _e->str(); }; + { auto _e = timeStamp(); if (_e) _o->timeStamp = std::unique_ptr(_e->UnPack(_resolver)); }; + { auto _e = setArmControlState(); _o->setArmControlState = _e; }; + { auto _e = armControlState(); if (_e) _o->armControlState = std::unique_ptr(_e->UnPack(_resolver)); }; + { auto _e = setArmConfiguration(); _o->setArmConfiguration = _e; }; + { auto _e = armConfiguration(); if (_e) _o->armConfiguration = std::unique_ptr(_e->UnPack(_resolver)); }; + { auto _e = hasMonitorState(); _o->hasMonitorState = _e; }; + { auto _e = monitorState(); if (_e) _o->monitorState = std::unique_ptr(_e->UnPack(_resolver)); }; + { auto _e = hasMonitorConfig(); _o->hasMonitorConfig = _e; }; + { auto _e = monitorConfig(); if (_e) _o->monitorConfig = std::unique_ptr(_e->UnPack(_resolver)); }; + { auto _e = FRIMessage(); if (_e) _o->FRIMessage = std::unique_ptr(_e->UnPack(_resolver)); }; +} + +inline flatbuffers::Offset KUKAiiwaState::Pack(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaStateT* _o, const flatbuffers::rehasher_function_t *_rehasher) { + return CreateKUKAiiwaState(_fbb, _o, _rehasher); +} + +inline flatbuffers::Offset CreateKUKAiiwaState(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaStateT *_o, const flatbuffers::rehasher_function_t *_rehasher) { + (void)_rehasher; + (void)_o; + struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const KUKAiiwaStateT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; + auto _name = _o->name.empty() ? 0 : _fbb.CreateString(_o->name); + auto _destination = _o->destination.empty() ? 0 : _fbb.CreateString(_o->destination); + auto _source = _o->source.empty() ? 0 : _fbb.CreateString(_o->source); + auto _timeStamp = _o->timeStamp ? CreateTimeEvent(_fbb, _o->timeStamp.get(), _rehasher) : 0; + auto _setArmControlState = _o->setArmControlState; + auto _armControlState = _o->armControlState ? CreateArmControlState(_fbb, _o->armControlState.get(), _rehasher) : 0; + auto _setArmConfiguration = _o->setArmConfiguration; + auto _armConfiguration = _o->armConfiguration ? CreateKUKAiiwaArmConfiguration(_fbb, _o->armConfiguration.get(), _rehasher) : 0; + auto _hasMonitorState = _o->hasMonitorState; + auto _monitorState = _o->monitorState ? CreateKUKAiiwaMonitorState(_fbb, _o->monitorState.get(), _rehasher) : 0; + auto _hasMonitorConfig = _o->hasMonitorConfig; + auto _monitorConfig = _o->monitorConfig ? CreateKUKAiiwaMonitorConfiguration(_fbb, _o->monitorConfig.get(), _rehasher) : 0; + auto _FRIMessage = _o->FRIMessage ? CreateFRIMessageLog(_fbb, _o->FRIMessage.get(), _rehasher) : 0; + return grl::flatbuffer::CreateKUKAiiwaState( + _fbb, + _name, + _destination, + _source, + _timeStamp, + _setArmControlState, + _armControlState, + _setArmConfiguration, + _armConfiguration, + _hasMonitorState, + _monitorState, + _hasMonitorConfig, + _monitorConfig, + _FRIMessage); +} + +inline KUKAiiwaStatesT *KUKAiiwaStates::UnPack(const flatbuffers::resolver_function_t *_resolver) const { + auto _o = new KUKAiiwaStatesT(); + UnPackTo(_o, _resolver); + return _o; +} + +inline void KUKAiiwaStates::UnPackTo(KUKAiiwaStatesT *_o, const flatbuffers::resolver_function_t *_resolver) const { + (void)_o; + (void)_resolver; + { auto _e = states(); if (_e) { _o->states.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->states[_i] = std::unique_ptr(_e->Get(_i)->UnPack(_resolver)); } } }; +} + +inline flatbuffers::Offset KUKAiiwaStates::Pack(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaStatesT* _o, const flatbuffers::rehasher_function_t *_rehasher) { + return CreateKUKAiiwaStates(_fbb, _o, _rehasher); +} + +inline flatbuffers::Offset CreateKUKAiiwaStates(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaStatesT *_o, const flatbuffers::rehasher_function_t *_rehasher) { + (void)_rehasher; + (void)_o; + struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const KUKAiiwaStatesT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; + auto _states = _o->states.size() ? _fbb.CreateVector> (_o->states.size(), [](size_t i, _VectorArgs *__va) { return CreateKUKAiiwaState(*__va->__fbb, __va->__o->states[i].get(), __va->__rehasher); }, &_va ) : 0; + return grl::flatbuffer::CreateKUKAiiwaStates( + _fbb, + _states); +} + +inline const grl::flatbuffer::KUKAiiwaStates *GetKUKAiiwaStates(const void *buf) { + return flatbuffers::GetRoot(buf); +} + +inline const grl::flatbuffer::KUKAiiwaStates *GetSizePrefixedKUKAiiwaStates(const void *buf) { + return flatbuffers::GetSizePrefixedRoot(buf); +} + +inline const char *KUKAiiwaStatesIdentifier() { + return "iiwa"; +} + +inline bool KUKAiiwaStatesBufferHasIdentifier(const void *buf) { + return flatbuffers::BufferHasIdentifier( + buf, KUKAiiwaStatesIdentifier()); +} + +inline bool VerifyKUKAiiwaStatesBuffer( + flatbuffers::Verifier &verifier) { + return verifier.VerifyBuffer(KUKAiiwaStatesIdentifier()); +} + +inline bool VerifySizePrefixedKUKAiiwaStatesBuffer( + flatbuffers::Verifier &verifier) { + return verifier.VerifySizePrefixedBuffer(KUKAiiwaStatesIdentifier()); +} + +inline const char *KUKAiiwaStatesExtension() { + return "iiwa"; +} + +inline void FinishKUKAiiwaStatesBuffer( + flatbuffers::FlatBufferBuilder &fbb, + flatbuffers::Offset root) { + fbb.Finish(root, KUKAiiwaStatesIdentifier()); +} + +inline void FinishSizePrefixedKUKAiiwaStatesBuffer( + flatbuffers::FlatBufferBuilder &fbb, + flatbuffers::Offset root) { + fbb.FinishSizePrefixed(root, KUKAiiwaStatesIdentifier()); +} + +inline std::unique_ptr UnPackKUKAiiwaStates( + const void *buf, + const flatbuffers::resolver_function_t *res = nullptr) { + return std::unique_ptr(GetKUKAiiwaStates(buf)->UnPack(res)); +} + +} // namespace flatbuffer +} // namespace grl + +#endif // FLATBUFFERS_GENERATED_KUKAIIWA_GRL_FLATBUFFER_H_ diff --git a/include/grl/flatbuffer/LinkObject_generated.h b/include/grl/flatbuffer/LinkObject_generated.h new file mode 100644 index 0000000..40e5312 --- /dev/null +++ b/include/grl/flatbuffer/LinkObject_generated.h @@ -0,0 +1,163 @@ +// automatically generated by the FlatBuffers compiler, do not modify + + +#ifndef FLATBUFFERS_GENERATED_LINKOBJECT_GRL_FLATBUFFER_H_ +#define FLATBUFFERS_GENERATED_LINKOBJECT_GRL_FLATBUFFER_H_ + +#include "flatbuffers/flatbuffers.h" + +#include "Geometry_generated.h" + +namespace grl { +namespace flatbuffer { + +struct LinkObject; +struct LinkObjectT; + +struct LinkObjectT : public flatbuffers::NativeTable { + typedef LinkObject TableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.LinkObjectT"; + } + std::string name; + std::string parent; + std::unique_ptr pose; + std::unique_ptr inertia; + LinkObjectT() { + } +}; + +struct LinkObject FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { + typedef LinkObjectT NativeTableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.LinkObject"; + } + enum { + VT_NAME = 4, + VT_PARENT = 6, + VT_POSE = 8, + VT_INERTIA = 10 + }; + const flatbuffers::String *name() const { + return GetPointer(VT_NAME); + } + const flatbuffers::String *parent() const { + return GetPointer(VT_PARENT); + } + const Pose *pose() const { + return GetStruct(VT_POSE); + } + const Inertia *inertia() const { + return GetStruct(VT_INERTIA); + } + bool Verify(flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + VerifyOffset(verifier, VT_NAME) && + verifier.Verify(name()) && + VerifyOffset(verifier, VT_PARENT) && + verifier.Verify(parent()) && + VerifyField(verifier, VT_POSE) && + VerifyField(verifier, VT_INERTIA) && + verifier.EndTable(); + } + LinkObjectT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; + void UnPackTo(LinkObjectT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; + static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const LinkObjectT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); +}; + +struct LinkObjectBuilder { + flatbuffers::FlatBufferBuilder &fbb_; + flatbuffers::uoffset_t start_; + void add_name(flatbuffers::Offset name) { + fbb_.AddOffset(LinkObject::VT_NAME, name); + } + void add_parent(flatbuffers::Offset parent) { + fbb_.AddOffset(LinkObject::VT_PARENT, parent); + } + void add_pose(const Pose *pose) { + fbb_.AddStruct(LinkObject::VT_POSE, pose); + } + void add_inertia(const Inertia *inertia) { + fbb_.AddStruct(LinkObject::VT_INERTIA, inertia); + } + explicit LinkObjectBuilder(flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + LinkObjectBuilder &operator=(const LinkObjectBuilder &); + flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = flatbuffers::Offset(end); + return o; + } +}; + +inline flatbuffers::Offset CreateLinkObject( + flatbuffers::FlatBufferBuilder &_fbb, + flatbuffers::Offset name = 0, + flatbuffers::Offset parent = 0, + const Pose *pose = 0, + const Inertia *inertia = 0) { + LinkObjectBuilder builder_(_fbb); + builder_.add_inertia(inertia); + builder_.add_pose(pose); + builder_.add_parent(parent); + builder_.add_name(name); + return builder_.Finish(); +} + +inline flatbuffers::Offset CreateLinkObjectDirect( + flatbuffers::FlatBufferBuilder &_fbb, + const char *name = nullptr, + const char *parent = nullptr, + const Pose *pose = 0, + const Inertia *inertia = 0) { + return grl::flatbuffer::CreateLinkObject( + _fbb, + name ? _fbb.CreateString(name) : 0, + parent ? _fbb.CreateString(parent) : 0, + pose, + inertia); +} + +flatbuffers::Offset CreateLinkObject(flatbuffers::FlatBufferBuilder &_fbb, const LinkObjectT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); + +inline LinkObjectT *LinkObject::UnPack(const flatbuffers::resolver_function_t *_resolver) const { + auto _o = new LinkObjectT(); + UnPackTo(_o, _resolver); + return _o; +} + +inline void LinkObject::UnPackTo(LinkObjectT *_o, const flatbuffers::resolver_function_t *_resolver) const { + (void)_o; + (void)_resolver; + { auto _e = name(); if (_e) _o->name = _e->str(); }; + { auto _e = parent(); if (_e) _o->parent = _e->str(); }; + { auto _e = pose(); if (_e) _o->pose = std::unique_ptr(new Pose(*_e)); }; + { auto _e = inertia(); if (_e) _o->inertia = std::unique_ptr(new Inertia(*_e)); }; +} + +inline flatbuffers::Offset LinkObject::Pack(flatbuffers::FlatBufferBuilder &_fbb, const LinkObjectT* _o, const flatbuffers::rehasher_function_t *_rehasher) { + return CreateLinkObject(_fbb, _o, _rehasher); +} + +inline flatbuffers::Offset CreateLinkObject(flatbuffers::FlatBufferBuilder &_fbb, const LinkObjectT *_o, const flatbuffers::rehasher_function_t *_rehasher) { + (void)_rehasher; + (void)_o; + struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const LinkObjectT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; + auto _name = _o->name.empty() ? 0 : _fbb.CreateString(_o->name); + auto _parent = _o->parent.empty() ? 0 : _fbb.CreateString(_o->parent); + auto _pose = _o->pose ? _o->pose.get() : 0; + auto _inertia = _o->inertia ? _o->inertia.get() : 0; + return grl::flatbuffer::CreateLinkObject( + _fbb, + _name, + _parent, + _pose, + _inertia); +} + +} // namespace flatbuffer +} // namespace grl + +#endif // FLATBUFFERS_GENERATED_LINKOBJECT_GRL_FLATBUFFER_H_ diff --git a/include/grl/flatbuffer/LogKUKAiiwaFusionTrack_generated.h b/include/grl/flatbuffer/LogKUKAiiwaFusionTrack_generated.h new file mode 100644 index 0000000..ab2cf92 --- /dev/null +++ b/include/grl/flatbuffer/LogKUKAiiwaFusionTrack_generated.h @@ -0,0 +1,504 @@ +// automatically generated by the FlatBuffers compiler, do not modify + + +#ifndef FLATBUFFERS_GENERATED_LOGKUKAIIWAFUSIONTRACK_GRL_FLATBUFFER_H_ +#define FLATBUFFERS_GENERATED_LOGKUKAIIWAFUSIONTRACK_GRL_FLATBUFFER_H_ + +#include "flatbuffers/flatbuffers.h" + +#include "ArmControlState_generated.h" +#include "Euler_generated.h" +#include "FusionTrack_generated.h" +#include "Geometry_generated.h" +#include "JointState_generated.h" +#include "KUKAiiwa_generated.h" +#include "LinkObject_generated.h" +#include "Time_generated.h" + +namespace grl { +namespace flatbuffer { + +struct KUKAiiwaFusionTrackMessage; +struct KUKAiiwaFusionTrackMessageT; + +struct LogKUKAiiwaFusionTrack; +struct LogKUKAiiwaFusionTrackT; + +enum class DeviceState : uint8_t { + NONE = 0, + KUKAiiwaState = 1, + FusionTrackMessage = 2, + MIN = NONE, + MAX = FusionTrackMessage +}; + +inline const DeviceState (&EnumValuesDeviceState())[3] { + static const DeviceState values[] = { + DeviceState::NONE, + DeviceState::KUKAiiwaState, + DeviceState::FusionTrackMessage + }; + return values; +} + +inline const char * const *EnumNamesDeviceState() { + static const char * const names[] = { + "NONE", + "KUKAiiwaState", + "FusionTrackMessage", + nullptr + }; + return names; +} + +inline const char *EnumNameDeviceState(DeviceState e) { + const size_t index = static_cast(e); + return EnumNamesDeviceState()[index]; +} + +template struct DeviceStateTraits { + static const DeviceState enum_value = DeviceState::NONE; +}; + +template<> struct DeviceStateTraits { + static const DeviceState enum_value = DeviceState::KUKAiiwaState; +}; + +template<> struct DeviceStateTraits { + static const DeviceState enum_value = DeviceState::FusionTrackMessage; +}; + +struct DeviceStateUnion { + DeviceState type; + void *value; + + DeviceStateUnion() : type(DeviceState::NONE), value(nullptr) {} + DeviceStateUnion(DeviceStateUnion&& u) FLATBUFFERS_NOEXCEPT : + type(DeviceState::NONE), value(nullptr) + { std::swap(type, u.type); std::swap(value, u.value); } + DeviceStateUnion(const DeviceStateUnion &) FLATBUFFERS_NOEXCEPT; + DeviceStateUnion &operator=(const DeviceStateUnion &u) FLATBUFFERS_NOEXCEPT + { DeviceStateUnion t(u); std::swap(type, t.type); std::swap(value, t.value); return *this; } + DeviceStateUnion &operator=(DeviceStateUnion &&u) FLATBUFFERS_NOEXCEPT + { std::swap(type, u.type); std::swap(value, u.value); return *this; } + ~DeviceStateUnion() { Reset(); } + + void Reset(); + +#ifndef FLATBUFFERS_CPP98_STL + template + void Set(T&& val) { + Reset(); + type = DeviceStateTraits::enum_value; + if (type != DeviceState::NONE) { + value = new T(std::forward(val)); + } + } +#endif // FLATBUFFERS_CPP98_STL + + static void *UnPack(const void *obj, DeviceState type, const flatbuffers::resolver_function_t *resolver); + flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const flatbuffers::rehasher_function_t *_rehasher = nullptr) const; + + KUKAiiwaStateT *AsKUKAiiwaState() { + return type == DeviceState::KUKAiiwaState ? + reinterpret_cast(value) : nullptr; + } + const KUKAiiwaStateT *AsKUKAiiwaState() const { + return type == DeviceState::KUKAiiwaState ? + reinterpret_cast(value) : nullptr; + } + FusionTrackMessageT *AsFusionTrackMessage() { + return type == DeviceState::FusionTrackMessage ? + reinterpret_cast(value) : nullptr; + } + const FusionTrackMessageT *AsFusionTrackMessage() const { + return type == DeviceState::FusionTrackMessage ? + reinterpret_cast(value) : nullptr; + } +}; + +bool VerifyDeviceState(flatbuffers::Verifier &verifier, const void *obj, DeviceState type); +bool VerifyDeviceStateVector(flatbuffers::Verifier &verifier, const flatbuffers::Vector> *values, const flatbuffers::Vector *types); + +struct KUKAiiwaFusionTrackMessageT : public flatbuffers::NativeTable { + typedef KUKAiiwaFusionTrackMessage TableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.KUKAiiwaFusionTrackMessageT"; + } + double timestamp; + std::unique_ptr timeEvent; + DeviceStateUnion deviceState; + KUKAiiwaFusionTrackMessageT() + : timestamp(0.0) { + } +}; + +struct KUKAiiwaFusionTrackMessage FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { + typedef KUKAiiwaFusionTrackMessageT NativeTableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.KUKAiiwaFusionTrackMessage"; + } + enum { + VT_TIMESTAMP = 4, + VT_TIMEEVENT = 6, + VT_DEVICESTATE_TYPE = 8, + VT_DEVICESTATE = 10 + }; + double timestamp() const { + return GetField(VT_TIMESTAMP, 0.0); + } + const TimeEvent *timeEvent() const { + return GetPointer(VT_TIMEEVENT); + } + DeviceState deviceState_type() const { + return static_cast(GetField(VT_DEVICESTATE_TYPE, 0)); + } + const void *deviceState() const { + return GetPointer(VT_DEVICESTATE); + } + template const T *deviceState_as() const; + const KUKAiiwaState *deviceState_as_KUKAiiwaState() const { + return deviceState_type() == DeviceState::KUKAiiwaState ? static_cast(deviceState()) : nullptr; + } + const FusionTrackMessage *deviceState_as_FusionTrackMessage() const { + return deviceState_type() == DeviceState::FusionTrackMessage ? static_cast(deviceState()) : nullptr; + } + bool Verify(flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + VerifyField(verifier, VT_TIMESTAMP) && + VerifyOffset(verifier, VT_TIMEEVENT) && + verifier.VerifyTable(timeEvent()) && + VerifyField(verifier, VT_DEVICESTATE_TYPE) && + VerifyOffset(verifier, VT_DEVICESTATE) && + VerifyDeviceState(verifier, deviceState(), deviceState_type()) && + verifier.EndTable(); + } + KUKAiiwaFusionTrackMessageT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; + void UnPackTo(KUKAiiwaFusionTrackMessageT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; + static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaFusionTrackMessageT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); +}; + +template<> inline const KUKAiiwaState *KUKAiiwaFusionTrackMessage::deviceState_as() const { + return deviceState_as_KUKAiiwaState(); +} + +template<> inline const FusionTrackMessage *KUKAiiwaFusionTrackMessage::deviceState_as() const { + return deviceState_as_FusionTrackMessage(); +} + +struct KUKAiiwaFusionTrackMessageBuilder { + flatbuffers::FlatBufferBuilder &fbb_; + flatbuffers::uoffset_t start_; + void add_timestamp(double timestamp) { + fbb_.AddElement(KUKAiiwaFusionTrackMessage::VT_TIMESTAMP, timestamp, 0.0); + } + void add_timeEvent(flatbuffers::Offset timeEvent) { + fbb_.AddOffset(KUKAiiwaFusionTrackMessage::VT_TIMEEVENT, timeEvent); + } + void add_deviceState_type(DeviceState deviceState_type) { + fbb_.AddElement(KUKAiiwaFusionTrackMessage::VT_DEVICESTATE_TYPE, static_cast(deviceState_type), 0); + } + void add_deviceState(flatbuffers::Offset deviceState) { + fbb_.AddOffset(KUKAiiwaFusionTrackMessage::VT_DEVICESTATE, deviceState); + } + explicit KUKAiiwaFusionTrackMessageBuilder(flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + KUKAiiwaFusionTrackMessageBuilder &operator=(const KUKAiiwaFusionTrackMessageBuilder &); + flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = flatbuffers::Offset(end); + return o; + } +}; + +inline flatbuffers::Offset CreateKUKAiiwaFusionTrackMessage( + flatbuffers::FlatBufferBuilder &_fbb, + double timestamp = 0.0, + flatbuffers::Offset timeEvent = 0, + DeviceState deviceState_type = DeviceState::NONE, + flatbuffers::Offset deviceState = 0) { + KUKAiiwaFusionTrackMessageBuilder builder_(_fbb); + builder_.add_timestamp(timestamp); + builder_.add_deviceState(deviceState); + builder_.add_timeEvent(timeEvent); + builder_.add_deviceState_type(deviceState_type); + return builder_.Finish(); +} + +flatbuffers::Offset CreateKUKAiiwaFusionTrackMessage(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaFusionTrackMessageT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); + +struct LogKUKAiiwaFusionTrackT : public flatbuffers::NativeTable { + typedef LogKUKAiiwaFusionTrack TableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.LogKUKAiiwaFusionTrackT"; + } + std::vector> states; + LogKUKAiiwaFusionTrackT() { + } +}; + +struct LogKUKAiiwaFusionTrack FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { + typedef LogKUKAiiwaFusionTrackT NativeTableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.LogKUKAiiwaFusionTrack"; + } + enum { + VT_STATES = 4 + }; + const flatbuffers::Vector> *states() const { + return GetPointer> *>(VT_STATES); + } + bool Verify(flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + VerifyOffset(verifier, VT_STATES) && + verifier.Verify(states()) && + verifier.VerifyVectorOfTables(states()) && + verifier.EndTable(); + } + LogKUKAiiwaFusionTrackT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; + void UnPackTo(LogKUKAiiwaFusionTrackT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; + static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const LogKUKAiiwaFusionTrackT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); +}; + +struct LogKUKAiiwaFusionTrackBuilder { + flatbuffers::FlatBufferBuilder &fbb_; + flatbuffers::uoffset_t start_; + void add_states(flatbuffers::Offset>> states) { + fbb_.AddOffset(LogKUKAiiwaFusionTrack::VT_STATES, states); + } + explicit LogKUKAiiwaFusionTrackBuilder(flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + LogKUKAiiwaFusionTrackBuilder &operator=(const LogKUKAiiwaFusionTrackBuilder &); + flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = flatbuffers::Offset(end); + return o; + } +}; + +inline flatbuffers::Offset CreateLogKUKAiiwaFusionTrack( + flatbuffers::FlatBufferBuilder &_fbb, + flatbuffers::Offset>> states = 0) { + LogKUKAiiwaFusionTrackBuilder builder_(_fbb); + builder_.add_states(states); + return builder_.Finish(); +} + +inline flatbuffers::Offset CreateLogKUKAiiwaFusionTrackDirect( + flatbuffers::FlatBufferBuilder &_fbb, + const std::vector> *states = nullptr) { + return grl::flatbuffer::CreateLogKUKAiiwaFusionTrack( + _fbb, + states ? _fbb.CreateVector>(*states) : 0); +} + +flatbuffers::Offset CreateLogKUKAiiwaFusionTrack(flatbuffers::FlatBufferBuilder &_fbb, const LogKUKAiiwaFusionTrackT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); + +inline KUKAiiwaFusionTrackMessageT *KUKAiiwaFusionTrackMessage::UnPack(const flatbuffers::resolver_function_t *_resolver) const { + auto _o = new KUKAiiwaFusionTrackMessageT(); + UnPackTo(_o, _resolver); + return _o; +} + +inline void KUKAiiwaFusionTrackMessage::UnPackTo(KUKAiiwaFusionTrackMessageT *_o, const flatbuffers::resolver_function_t *_resolver) const { + (void)_o; + (void)_resolver; + { auto _e = timestamp(); _o->timestamp = _e; }; + { auto _e = timeEvent(); if (_e) _o->timeEvent = std::unique_ptr(_e->UnPack(_resolver)); }; + { auto _e = deviceState_type(); _o->deviceState.type = _e; }; + { auto _e = deviceState(); if (_e) _o->deviceState.value = DeviceStateUnion::UnPack(_e, deviceState_type(), _resolver); }; +} + +inline flatbuffers::Offset KUKAiiwaFusionTrackMessage::Pack(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaFusionTrackMessageT* _o, const flatbuffers::rehasher_function_t *_rehasher) { + return CreateKUKAiiwaFusionTrackMessage(_fbb, _o, _rehasher); +} + +inline flatbuffers::Offset CreateKUKAiiwaFusionTrackMessage(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaFusionTrackMessageT *_o, const flatbuffers::rehasher_function_t *_rehasher) { + (void)_rehasher; + (void)_o; + struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const KUKAiiwaFusionTrackMessageT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; + auto _timestamp = _o->timestamp; + auto _timeEvent = _o->timeEvent ? CreateTimeEvent(_fbb, _o->timeEvent.get(), _rehasher) : 0; + auto _deviceState_type = _o->deviceState.type; + auto _deviceState = _o->deviceState.Pack(_fbb); + return grl::flatbuffer::CreateKUKAiiwaFusionTrackMessage( + _fbb, + _timestamp, + _timeEvent, + _deviceState_type, + _deviceState); +} + +inline LogKUKAiiwaFusionTrackT *LogKUKAiiwaFusionTrack::UnPack(const flatbuffers::resolver_function_t *_resolver) const { + auto _o = new LogKUKAiiwaFusionTrackT(); + UnPackTo(_o, _resolver); + return _o; +} + +inline void LogKUKAiiwaFusionTrack::UnPackTo(LogKUKAiiwaFusionTrackT *_o, const flatbuffers::resolver_function_t *_resolver) const { + (void)_o; + (void)_resolver; + { auto _e = states(); if (_e) { _o->states.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->states[_i] = std::unique_ptr(_e->Get(_i)->UnPack(_resolver)); } } }; +} + +inline flatbuffers::Offset LogKUKAiiwaFusionTrack::Pack(flatbuffers::FlatBufferBuilder &_fbb, const LogKUKAiiwaFusionTrackT* _o, const flatbuffers::rehasher_function_t *_rehasher) { + return CreateLogKUKAiiwaFusionTrack(_fbb, _o, _rehasher); +} + +inline flatbuffers::Offset CreateLogKUKAiiwaFusionTrack(flatbuffers::FlatBufferBuilder &_fbb, const LogKUKAiiwaFusionTrackT *_o, const flatbuffers::rehasher_function_t *_rehasher) { + (void)_rehasher; + (void)_o; + struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const LogKUKAiiwaFusionTrackT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; + auto _states = _o->states.size() ? _fbb.CreateVector> (_o->states.size(), [](size_t i, _VectorArgs *__va) { return CreateKUKAiiwaFusionTrackMessage(*__va->__fbb, __va->__o->states[i].get(), __va->__rehasher); }, &_va ) : 0; + return grl::flatbuffer::CreateLogKUKAiiwaFusionTrack( + _fbb, + _states); +} + +inline bool VerifyDeviceState(flatbuffers::Verifier &verifier, const void *obj, DeviceState type) { + switch (type) { + case DeviceState::NONE: { + return true; + } + case DeviceState::KUKAiiwaState: { + auto ptr = reinterpret_cast(obj); + return verifier.VerifyTable(ptr); + } + case DeviceState::FusionTrackMessage: { + auto ptr = reinterpret_cast(obj); + return verifier.VerifyTable(ptr); + } + default: return false; + } +} + +inline bool VerifyDeviceStateVector(flatbuffers::Verifier &verifier, const flatbuffers::Vector> *values, const flatbuffers::Vector *types) { + if (!values || !types) return !values && !types; + if (values->size() != types->size()) return false; + for (flatbuffers::uoffset_t i = 0; i < values->size(); ++i) { + if (!VerifyDeviceState( + verifier, values->Get(i), types->GetEnum(i))) { + return false; + } + } + return true; +} + +inline void *DeviceStateUnion::UnPack(const void *obj, DeviceState type, const flatbuffers::resolver_function_t *resolver) { + switch (type) { + case DeviceState::KUKAiiwaState: { + auto ptr = reinterpret_cast(obj); + return ptr->UnPack(resolver); + } + case DeviceState::FusionTrackMessage: { + auto ptr = reinterpret_cast(obj); + return ptr->UnPack(resolver); + } + default: return nullptr; + } +} + +inline flatbuffers::Offset DeviceStateUnion::Pack(flatbuffers::FlatBufferBuilder &_fbb, const flatbuffers::rehasher_function_t *_rehasher) const { + switch (type) { + case DeviceState::KUKAiiwaState: { + auto ptr = reinterpret_cast(value); + return CreateKUKAiiwaState(_fbb, ptr, _rehasher).Union(); + } + case DeviceState::FusionTrackMessage: { + auto ptr = reinterpret_cast(value); + return CreateFusionTrackMessage(_fbb, ptr, _rehasher).Union(); + } + default: return 0; + } +} + +inline DeviceStateUnion::DeviceStateUnion(const DeviceStateUnion &u) FLATBUFFERS_NOEXCEPT : type(u.type), value(nullptr) { + switch (type) { + case DeviceState::KUKAiiwaState: { + assert(false); // KUKAiiwaStateT not copyable. + break; + } + case DeviceState::FusionTrackMessage: { + assert(false); // FusionTrackMessageT not copyable. + break; + } + default: + break; + } +} + +inline void DeviceStateUnion::Reset() { + switch (type) { + case DeviceState::KUKAiiwaState: { + auto ptr = reinterpret_cast(value); + delete ptr; + break; + } + case DeviceState::FusionTrackMessage: { + auto ptr = reinterpret_cast(value); + delete ptr; + break; + } + default: break; + } + value = nullptr; + type = DeviceState::NONE; +} + +inline const grl::flatbuffer::LogKUKAiiwaFusionTrack *GetLogKUKAiiwaFusionTrack(const void *buf) { + return flatbuffers::GetRoot(buf); +} + +inline const grl::flatbuffer::LogKUKAiiwaFusionTrack *GetSizePrefixedLogKUKAiiwaFusionTrack(const void *buf) { + return flatbuffers::GetSizePrefixedRoot(buf); +} + +inline const char *LogKUKAiiwaFusionTrackIdentifier() { + return "flik"; +} + +inline bool LogKUKAiiwaFusionTrackBufferHasIdentifier(const void *buf) { + return flatbuffers::BufferHasIdentifier( + buf, LogKUKAiiwaFusionTrackIdentifier()); +} + +inline bool VerifyLogKUKAiiwaFusionTrackBuffer( + flatbuffers::Verifier &verifier) { + return verifier.VerifyBuffer(LogKUKAiiwaFusionTrackIdentifier()); +} + +inline bool VerifySizePrefixedLogKUKAiiwaFusionTrackBuffer( + flatbuffers::Verifier &verifier) { + return verifier.VerifySizePrefixedBuffer(LogKUKAiiwaFusionTrackIdentifier()); +} + +inline const char *LogKUKAiiwaFusionTrackExtension() { + return "flik"; +} + +inline void FinishLogKUKAiiwaFusionTrackBuffer( + flatbuffers::FlatBufferBuilder &fbb, + flatbuffers::Offset root) { + fbb.Finish(root, LogKUKAiiwaFusionTrackIdentifier()); +} + +inline void FinishSizePrefixedLogKUKAiiwaFusionTrackBuffer( + flatbuffers::FlatBufferBuilder &fbb, + flatbuffers::Offset root) { + fbb.FinishSizePrefixed(root, LogKUKAiiwaFusionTrackIdentifier()); +} + +inline std::unique_ptr UnPackLogKUKAiiwaFusionTrack( + const void *buf, + const flatbuffers::resolver_function_t *res = nullptr) { + return std::unique_ptr(GetLogKUKAiiwaFusionTrack(buf)->UnPack(res)); +} + +} // namespace flatbuffer +} // namespace grl + +#endif // FLATBUFFERS_GENERATED_LOGKUKAIIWAFUSIONTRACK_GRL_FLATBUFFER_H_ diff --git a/include/grl/flatbuffer/Time_generated.h b/include/grl/flatbuffer/Time_generated.h new file mode 100644 index 0000000..16fe1c8 --- /dev/null +++ b/include/grl/flatbuffer/Time_generated.h @@ -0,0 +1,291 @@ +// automatically generated by the FlatBuffers compiler, do not modify + + +#ifndef FLATBUFFERS_GENERATED_TIME_GRL_FLATBUFFER_H_ +#define FLATBUFFERS_GENERATED_TIME_GRL_FLATBUFFER_H_ + +#include "flatbuffers/flatbuffers.h" + +namespace grl { +namespace flatbuffer { + +struct Time; + +struct TimeEvent; +struct TimeEventT; + +FLATBUFFERS_MANUALLY_ALIGNED_STRUCT(8) Time FLATBUFFERS_FINAL_CLASS { + private: + double sec_; + + public: + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.Time"; + } + Time() { + memset(this, 0, sizeof(Time)); + } + Time(double _sec) + : sec_(flatbuffers::EndianScalar(_sec)) { + } + double sec() const { + return flatbuffers::EndianScalar(sec_); + } +}; +FLATBUFFERS_STRUCT_END(Time, 8); + +struct TimeEventT : public flatbuffers::NativeTable { + typedef TimeEvent TableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.TimeEventT"; + } + std::string event_name; + int64_t local_request_time; + std::string device_clock_id; + int64_t device_time; + std::string local_clock_id; + int64_t local_receive_time; + int64_t corrected_local_time; + int64_t clock_skew; + int64_t min_transport_delay; + TimeEventT() + : local_request_time(0), + device_time(0), + local_receive_time(0), + corrected_local_time(0), + clock_skew(0), + min_transport_delay(0) { + } +}; + +/// Note that all of these time entries are +/// longs with a minimum step of 100ns, +/// see google cartographer's cartographer::common::time +struct TimeEvent FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { + typedef TimeEventT NativeTableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.TimeEvent"; + } + enum { + VT_EVENT_NAME = 4, + VT_LOCAL_REQUEST_TIME = 6, + VT_DEVICE_CLOCK_ID = 8, + VT_DEVICE_TIME = 10, + VT_LOCAL_CLOCK_ID = 12, + VT_LOCAL_RECEIVE_TIME = 14, + VT_CORRECTED_LOCAL_TIME = 16, + VT_CLOCK_SKEW = 18, + VT_MIN_TRANSPORT_DELAY = 20 + }; + /// Identifying string for this time stamped data topic + /// something like "/opticaltracker/00000000/frame" where + /// 00000000 is the serial number of the optical tracker. + const flatbuffers::String *event_name() const { + return GetPointer(VT_EVENT_NAME); + } + /// The time just before a data update request is made + int64_t local_request_time() const { + return GetField(VT_LOCAL_REQUEST_TIME, 0); + } + /// Identifying string for the clock used to drive the device + /// something like "/opticaltracker/00000000/clock" + /// if it is the clock internal to a sensor like an optical tracker + const flatbuffers::String *device_clock_id() const { + return GetPointer(VT_DEVICE_CLOCK_ID); + } + /// The time provided by the device specified by device_clock_id + int64_t device_time() const { + return GetField(VT_DEVICE_TIME, 0); + } + /// Identifying string for the clock used to drive the device + /// or "/control_computer/clock/steady" if the device has no clock + /// and the time is the desktop computer + /// running the steady clock (vs clocks which might change time) + const flatbuffers::String *local_clock_id() const { + return GetPointer(VT_LOCAL_CLOCK_ID); + } + /// The time at which the data was received + int64_t local_receive_time() const { + return GetField(VT_LOCAL_RECEIVE_TIME, 0); + } + /// The corrected local time which represents when the sensor + /// data was actually captured. + int64_t corrected_local_time() const { + return GetField(VT_CORRECTED_LOCAL_TIME, 0); + } + /// Estimated duration of the skew between the device clock + /// and the local time clock + int64_t clock_skew() const { + return GetField(VT_CLOCK_SKEW, 0); + } + /// The minimum expected delay in transporting the data request + int64_t min_transport_delay() const { + return GetField(VT_MIN_TRANSPORT_DELAY, 0); + } + bool Verify(flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + VerifyOffset(verifier, VT_EVENT_NAME) && + verifier.Verify(event_name()) && + VerifyField(verifier, VT_LOCAL_REQUEST_TIME) && + VerifyOffset(verifier, VT_DEVICE_CLOCK_ID) && + verifier.Verify(device_clock_id()) && + VerifyField(verifier, VT_DEVICE_TIME) && + VerifyOffset(verifier, VT_LOCAL_CLOCK_ID) && + verifier.Verify(local_clock_id()) && + VerifyField(verifier, VT_LOCAL_RECEIVE_TIME) && + VerifyField(verifier, VT_CORRECTED_LOCAL_TIME) && + VerifyField(verifier, VT_CLOCK_SKEW) && + VerifyField(verifier, VT_MIN_TRANSPORT_DELAY) && + verifier.EndTable(); + } + TimeEventT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; + void UnPackTo(TimeEventT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; + static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const TimeEventT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); +}; + +struct TimeEventBuilder { + flatbuffers::FlatBufferBuilder &fbb_; + flatbuffers::uoffset_t start_; + void add_event_name(flatbuffers::Offset event_name) { + fbb_.AddOffset(TimeEvent::VT_EVENT_NAME, event_name); + } + void add_local_request_time(int64_t local_request_time) { + fbb_.AddElement(TimeEvent::VT_LOCAL_REQUEST_TIME, local_request_time, 0); + } + void add_device_clock_id(flatbuffers::Offset device_clock_id) { + fbb_.AddOffset(TimeEvent::VT_DEVICE_CLOCK_ID, device_clock_id); + } + void add_device_time(int64_t device_time) { + fbb_.AddElement(TimeEvent::VT_DEVICE_TIME, device_time, 0); + } + void add_local_clock_id(flatbuffers::Offset local_clock_id) { + fbb_.AddOffset(TimeEvent::VT_LOCAL_CLOCK_ID, local_clock_id); + } + void add_local_receive_time(int64_t local_receive_time) { + fbb_.AddElement(TimeEvent::VT_LOCAL_RECEIVE_TIME, local_receive_time, 0); + } + void add_corrected_local_time(int64_t corrected_local_time) { + fbb_.AddElement(TimeEvent::VT_CORRECTED_LOCAL_TIME, corrected_local_time, 0); + } + void add_clock_skew(int64_t clock_skew) { + fbb_.AddElement(TimeEvent::VT_CLOCK_SKEW, clock_skew, 0); + } + void add_min_transport_delay(int64_t min_transport_delay) { + fbb_.AddElement(TimeEvent::VT_MIN_TRANSPORT_DELAY, min_transport_delay, 0); + } + explicit TimeEventBuilder(flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + TimeEventBuilder &operator=(const TimeEventBuilder &); + flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = flatbuffers::Offset(end); + return o; + } +}; + +inline flatbuffers::Offset CreateTimeEvent( + flatbuffers::FlatBufferBuilder &_fbb, + flatbuffers::Offset event_name = 0, + int64_t local_request_time = 0, + flatbuffers::Offset device_clock_id = 0, + int64_t device_time = 0, + flatbuffers::Offset local_clock_id = 0, + int64_t local_receive_time = 0, + int64_t corrected_local_time = 0, + int64_t clock_skew = 0, + int64_t min_transport_delay = 0) { + TimeEventBuilder builder_(_fbb); + builder_.add_min_transport_delay(min_transport_delay); + builder_.add_clock_skew(clock_skew); + builder_.add_corrected_local_time(corrected_local_time); + builder_.add_local_receive_time(local_receive_time); + builder_.add_device_time(device_time); + builder_.add_local_request_time(local_request_time); + builder_.add_local_clock_id(local_clock_id); + builder_.add_device_clock_id(device_clock_id); + builder_.add_event_name(event_name); + return builder_.Finish(); +} + +inline flatbuffers::Offset CreateTimeEventDirect( + flatbuffers::FlatBufferBuilder &_fbb, + const char *event_name = nullptr, + int64_t local_request_time = 0, + const char *device_clock_id = nullptr, + int64_t device_time = 0, + const char *local_clock_id = nullptr, + int64_t local_receive_time = 0, + int64_t corrected_local_time = 0, + int64_t clock_skew = 0, + int64_t min_transport_delay = 0) { + return grl::flatbuffer::CreateTimeEvent( + _fbb, + event_name ? _fbb.CreateString(event_name) : 0, + local_request_time, + device_clock_id ? _fbb.CreateString(device_clock_id) : 0, + device_time, + local_clock_id ? _fbb.CreateString(local_clock_id) : 0, + local_receive_time, + corrected_local_time, + clock_skew, + min_transport_delay); +} + +flatbuffers::Offset CreateTimeEvent(flatbuffers::FlatBufferBuilder &_fbb, const TimeEventT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); + +inline TimeEventT *TimeEvent::UnPack(const flatbuffers::resolver_function_t *_resolver) const { + auto _o = new TimeEventT(); + UnPackTo(_o, _resolver); + return _o; +} + +inline void TimeEvent::UnPackTo(TimeEventT *_o, const flatbuffers::resolver_function_t *_resolver) const { + (void)_o; + (void)_resolver; + { auto _e = event_name(); if (_e) _o->event_name = _e->str(); }; + { auto _e = local_request_time(); _o->local_request_time = _e; }; + { auto _e = device_clock_id(); if (_e) _o->device_clock_id = _e->str(); }; + { auto _e = device_time(); _o->device_time = _e; }; + { auto _e = local_clock_id(); if (_e) _o->local_clock_id = _e->str(); }; + { auto _e = local_receive_time(); _o->local_receive_time = _e; }; + { auto _e = corrected_local_time(); _o->corrected_local_time = _e; }; + { auto _e = clock_skew(); _o->clock_skew = _e; }; + { auto _e = min_transport_delay(); _o->min_transport_delay = _e; }; +} + +inline flatbuffers::Offset TimeEvent::Pack(flatbuffers::FlatBufferBuilder &_fbb, const TimeEventT* _o, const flatbuffers::rehasher_function_t *_rehasher) { + return CreateTimeEvent(_fbb, _o, _rehasher); +} + +inline flatbuffers::Offset CreateTimeEvent(flatbuffers::FlatBufferBuilder &_fbb, const TimeEventT *_o, const flatbuffers::rehasher_function_t *_rehasher) { + (void)_rehasher; + (void)_o; + struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const TimeEventT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; + auto _event_name = _o->event_name.empty() ? 0 : _fbb.CreateString(_o->event_name); + auto _local_request_time = _o->local_request_time; + auto _device_clock_id = _o->device_clock_id.empty() ? 0 : _fbb.CreateString(_o->device_clock_id); + auto _device_time = _o->device_time; + auto _local_clock_id = _o->local_clock_id.empty() ? 0 : _fbb.CreateString(_o->local_clock_id); + auto _local_receive_time = _o->local_receive_time; + auto _corrected_local_time = _o->corrected_local_time; + auto _clock_skew = _o->clock_skew; + auto _min_transport_delay = _o->min_transport_delay; + return grl::flatbuffer::CreateTimeEvent( + _fbb, + _event_name, + _local_request_time, + _device_clock_id, + _device_time, + _local_clock_id, + _local_receive_time, + _corrected_local_time, + _clock_skew, + _min_transport_delay); +} + +} // namespace flatbuffer +} // namespace grl + +#endif // FLATBUFFERS_GENERATED_TIME_GRL_FLATBUFFER_H_ diff --git a/include/grl/flatbuffer/VrepControlPoint_generated.h b/include/grl/flatbuffer/VrepControlPoint_generated.h new file mode 100644 index 0000000..5582206 --- /dev/null +++ b/include/grl/flatbuffer/VrepControlPoint_generated.h @@ -0,0 +1,306 @@ +// automatically generated by the FlatBuffers compiler, do not modify + + +#ifndef FLATBUFFERS_GENERATED_VREPCONTROLPOINT_GRL_FLATBUFFER_H_ +#define FLATBUFFERS_GENERATED_VREPCONTROLPOINT_GRL_FLATBUFFER_H_ + +#include "flatbuffers/flatbuffers.h" + +#include "Euler_generated.h" +#include "Geometry_generated.h" + +namespace grl { +namespace flatbuffer { + +struct VrepControlPoint; +struct VrepControlPointT; + +struct VrepControlPointT : public flatbuffers::NativeTable { + typedef VrepControlPoint TableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.VrepControlPointT"; + } + std::unique_ptr position; + std::unique_ptr rotation; + double relativeVelocity; + int32_t bezierPointCount; + double interpolationFactor1; + double interpolationFactor2; + double virtualDistance; + int32_t auxiliaryFlags; + double auxiliaryChannel1; + double auxiliaryChannel2; + double auxiliaryChannel3; + double auxiliaryChannel4; + VrepControlPointT() + : relativeVelocity(1.0), + bezierPointCount(1), + interpolationFactor1(0.5), + interpolationFactor2(0.5), + virtualDistance(0.0), + auxiliaryFlags(0), + auxiliaryChannel1(0.0), + auxiliaryChannel2(0.0), + auxiliaryChannel3(0.0), + auxiliaryChannel4(0.0) { + } +}; + +struct VrepControlPoint FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { + typedef VrepControlPointT NativeTableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.VrepControlPoint"; + } + enum { + VT_POSITION = 4, + VT_ROTATION = 6, + VT_RELATIVEVELOCITY = 8, + VT_BEZIERPOINTCOUNT = 10, + VT_INTERPOLATIONFACTOR1 = 12, + VT_INTERPOLATIONFACTOR2 = 14, + VT_VIRTUALDISTANCE = 16, + VT_AUXILIARYFLAGS = 18, + VT_AUXILIARYCHANNEL1 = 20, + VT_AUXILIARYCHANNEL2 = 22, + VT_AUXILIARYCHANNEL3 = 24, + VT_AUXILIARYCHANNEL4 = 26 + }; + const Vector3d *position() const { + return GetStruct(VT_POSITION); + } + const EulerXYZd *rotation() const { + return GetStruct(VT_ROTATION); + } + double relativeVelocity() const { + return GetField(VT_RELATIVEVELOCITY, 1.0); + } + int32_t bezierPointCount() const { + return GetField(VT_BEZIERPOINTCOUNT, 1); + } + double interpolationFactor1() const { + return GetField(VT_INTERPOLATIONFACTOR1, 0.5); + } + double interpolationFactor2() const { + return GetField(VT_INTERPOLATIONFACTOR2, 0.5); + } + double virtualDistance() const { + return GetField(VT_VIRTUALDISTANCE, 0.0); + } + int32_t auxiliaryFlags() const { + return GetField(VT_AUXILIARYFLAGS, 0); + } + double auxiliaryChannel1() const { + return GetField(VT_AUXILIARYCHANNEL1, 0.0); + } + double auxiliaryChannel2() const { + return GetField(VT_AUXILIARYCHANNEL2, 0.0); + } + double auxiliaryChannel3() const { + return GetField(VT_AUXILIARYCHANNEL3, 0.0); + } + double auxiliaryChannel4() const { + return GetField(VT_AUXILIARYCHANNEL4, 0.0); + } + bool Verify(flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + VerifyField(verifier, VT_POSITION) && + VerifyField(verifier, VT_ROTATION) && + VerifyField(verifier, VT_RELATIVEVELOCITY) && + VerifyField(verifier, VT_BEZIERPOINTCOUNT) && + VerifyField(verifier, VT_INTERPOLATIONFACTOR1) && + VerifyField(verifier, VT_INTERPOLATIONFACTOR2) && + VerifyField(verifier, VT_VIRTUALDISTANCE) && + VerifyField(verifier, VT_AUXILIARYFLAGS) && + VerifyField(verifier, VT_AUXILIARYCHANNEL1) && + VerifyField(verifier, VT_AUXILIARYCHANNEL2) && + VerifyField(verifier, VT_AUXILIARYCHANNEL3) && + VerifyField(verifier, VT_AUXILIARYCHANNEL4) && + verifier.EndTable(); + } + VrepControlPointT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; + void UnPackTo(VrepControlPointT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; + static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const VrepControlPointT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); +}; + +struct VrepControlPointBuilder { + flatbuffers::FlatBufferBuilder &fbb_; + flatbuffers::uoffset_t start_; + void add_position(const Vector3d *position) { + fbb_.AddStruct(VrepControlPoint::VT_POSITION, position); + } + void add_rotation(const EulerXYZd *rotation) { + fbb_.AddStruct(VrepControlPoint::VT_ROTATION, rotation); + } + void add_relativeVelocity(double relativeVelocity) { + fbb_.AddElement(VrepControlPoint::VT_RELATIVEVELOCITY, relativeVelocity, 1.0); + } + void add_bezierPointCount(int32_t bezierPointCount) { + fbb_.AddElement(VrepControlPoint::VT_BEZIERPOINTCOUNT, bezierPointCount, 1); + } + void add_interpolationFactor1(double interpolationFactor1) { + fbb_.AddElement(VrepControlPoint::VT_INTERPOLATIONFACTOR1, interpolationFactor1, 0.5); + } + void add_interpolationFactor2(double interpolationFactor2) { + fbb_.AddElement(VrepControlPoint::VT_INTERPOLATIONFACTOR2, interpolationFactor2, 0.5); + } + void add_virtualDistance(double virtualDistance) { + fbb_.AddElement(VrepControlPoint::VT_VIRTUALDISTANCE, virtualDistance, 0.0); + } + void add_auxiliaryFlags(int32_t auxiliaryFlags) { + fbb_.AddElement(VrepControlPoint::VT_AUXILIARYFLAGS, auxiliaryFlags, 0); + } + void add_auxiliaryChannel1(double auxiliaryChannel1) { + fbb_.AddElement(VrepControlPoint::VT_AUXILIARYCHANNEL1, auxiliaryChannel1, 0.0); + } + void add_auxiliaryChannel2(double auxiliaryChannel2) { + fbb_.AddElement(VrepControlPoint::VT_AUXILIARYCHANNEL2, auxiliaryChannel2, 0.0); + } + void add_auxiliaryChannel3(double auxiliaryChannel3) { + fbb_.AddElement(VrepControlPoint::VT_AUXILIARYCHANNEL3, auxiliaryChannel3, 0.0); + } + void add_auxiliaryChannel4(double auxiliaryChannel4) { + fbb_.AddElement(VrepControlPoint::VT_AUXILIARYCHANNEL4, auxiliaryChannel4, 0.0); + } + explicit VrepControlPointBuilder(flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + VrepControlPointBuilder &operator=(const VrepControlPointBuilder &); + flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = flatbuffers::Offset(end); + return o; + } +}; + +inline flatbuffers::Offset CreateVrepControlPoint( + flatbuffers::FlatBufferBuilder &_fbb, + const Vector3d *position = 0, + const EulerXYZd *rotation = 0, + double relativeVelocity = 1.0, + int32_t bezierPointCount = 1, + double interpolationFactor1 = 0.5, + double interpolationFactor2 = 0.5, + double virtualDistance = 0.0, + int32_t auxiliaryFlags = 0, + double auxiliaryChannel1 = 0.0, + double auxiliaryChannel2 = 0.0, + double auxiliaryChannel3 = 0.0, + double auxiliaryChannel4 = 0.0) { + VrepControlPointBuilder builder_(_fbb); + builder_.add_auxiliaryChannel4(auxiliaryChannel4); + builder_.add_auxiliaryChannel3(auxiliaryChannel3); + builder_.add_auxiliaryChannel2(auxiliaryChannel2); + builder_.add_auxiliaryChannel1(auxiliaryChannel1); + builder_.add_virtualDistance(virtualDistance); + builder_.add_interpolationFactor2(interpolationFactor2); + builder_.add_interpolationFactor1(interpolationFactor1); + builder_.add_relativeVelocity(relativeVelocity); + builder_.add_auxiliaryFlags(auxiliaryFlags); + builder_.add_bezierPointCount(bezierPointCount); + builder_.add_rotation(rotation); + builder_.add_position(position); + return builder_.Finish(); +} + +flatbuffers::Offset CreateVrepControlPoint(flatbuffers::FlatBufferBuilder &_fbb, const VrepControlPointT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); + +inline VrepControlPointT *VrepControlPoint::UnPack(const flatbuffers::resolver_function_t *_resolver) const { + auto _o = new VrepControlPointT(); + UnPackTo(_o, _resolver); + return _o; +} + +inline void VrepControlPoint::UnPackTo(VrepControlPointT *_o, const flatbuffers::resolver_function_t *_resolver) const { + (void)_o; + (void)_resolver; + { auto _e = position(); if (_e) _o->position = std::unique_ptr(new Vector3d(*_e)); }; + { auto _e = rotation(); if (_e) _o->rotation = std::unique_ptr(new EulerXYZd(*_e)); }; + { auto _e = relativeVelocity(); _o->relativeVelocity = _e; }; + { auto _e = bezierPointCount(); _o->bezierPointCount = _e; }; + { auto _e = interpolationFactor1(); _o->interpolationFactor1 = _e; }; + { auto _e = interpolationFactor2(); _o->interpolationFactor2 = _e; }; + { auto _e = virtualDistance(); _o->virtualDistance = _e; }; + { auto _e = auxiliaryFlags(); _o->auxiliaryFlags = _e; }; + { auto _e = auxiliaryChannel1(); _o->auxiliaryChannel1 = _e; }; + { auto _e = auxiliaryChannel2(); _o->auxiliaryChannel2 = _e; }; + { auto _e = auxiliaryChannel3(); _o->auxiliaryChannel3 = _e; }; + { auto _e = auxiliaryChannel4(); _o->auxiliaryChannel4 = _e; }; +} + +inline flatbuffers::Offset VrepControlPoint::Pack(flatbuffers::FlatBufferBuilder &_fbb, const VrepControlPointT* _o, const flatbuffers::rehasher_function_t *_rehasher) { + return CreateVrepControlPoint(_fbb, _o, _rehasher); +} + +inline flatbuffers::Offset CreateVrepControlPoint(flatbuffers::FlatBufferBuilder &_fbb, const VrepControlPointT *_o, const flatbuffers::rehasher_function_t *_rehasher) { + (void)_rehasher; + (void)_o; + struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const VrepControlPointT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; + auto _position = _o->position ? _o->position.get() : 0; + auto _rotation = _o->rotation ? _o->rotation.get() : 0; + auto _relativeVelocity = _o->relativeVelocity; + auto _bezierPointCount = _o->bezierPointCount; + auto _interpolationFactor1 = _o->interpolationFactor1; + auto _interpolationFactor2 = _o->interpolationFactor2; + auto _virtualDistance = _o->virtualDistance; + auto _auxiliaryFlags = _o->auxiliaryFlags; + auto _auxiliaryChannel1 = _o->auxiliaryChannel1; + auto _auxiliaryChannel2 = _o->auxiliaryChannel2; + auto _auxiliaryChannel3 = _o->auxiliaryChannel3; + auto _auxiliaryChannel4 = _o->auxiliaryChannel4; + return grl::flatbuffer::CreateVrepControlPoint( + _fbb, + _position, + _rotation, + _relativeVelocity, + _bezierPointCount, + _interpolationFactor1, + _interpolationFactor2, + _virtualDistance, + _auxiliaryFlags, + _auxiliaryChannel1, + _auxiliaryChannel2, + _auxiliaryChannel3, + _auxiliaryChannel4); +} + +inline const grl::flatbuffer::VrepControlPoint *GetVrepControlPoint(const void *buf) { + return flatbuffers::GetRoot(buf); +} + +inline const grl::flatbuffer::VrepControlPoint *GetSizePrefixedVrepControlPoint(const void *buf) { + return flatbuffers::GetSizePrefixedRoot(buf); +} + +inline bool VerifyVrepControlPointBuffer( + flatbuffers::Verifier &verifier) { + return verifier.VerifyBuffer(nullptr); +} + +inline bool VerifySizePrefixedVrepControlPointBuffer( + flatbuffers::Verifier &verifier) { + return verifier.VerifySizePrefixedBuffer(nullptr); +} + +inline void FinishVrepControlPointBuffer( + flatbuffers::FlatBufferBuilder &fbb, + flatbuffers::Offset root) { + fbb.Finish(root); +} + +inline void FinishSizePrefixedVrepControlPointBuffer( + flatbuffers::FlatBufferBuilder &fbb, + flatbuffers::Offset root) { + fbb.FinishSizePrefixed(root); +} + +inline std::unique_ptr UnPackVrepControlPoint( + const void *buf, + const flatbuffers::resolver_function_t *res = nullptr) { + return std::unique_ptr(GetVrepControlPoint(buf)->UnPack(res)); +} + +} // namespace flatbuffer +} // namespace grl + +#endif // FLATBUFFERS_GENERATED_VREPCONTROLPOINT_GRL_FLATBUFFER_H_ diff --git a/include/grl/flatbuffer/VrepPath_generated.h b/include/grl/flatbuffer/VrepPath_generated.h new file mode 100644 index 0000000..da5343a --- /dev/null +++ b/include/grl/flatbuffer/VrepPath_generated.h @@ -0,0 +1,153 @@ +// automatically generated by the FlatBuffers compiler, do not modify + + +#ifndef FLATBUFFERS_GENERATED_VREPPATH_GRL_FLATBUFFER_H_ +#define FLATBUFFERS_GENERATED_VREPPATH_GRL_FLATBUFFER_H_ + +#include "flatbuffers/flatbuffers.h" + +#include "Euler_generated.h" +#include "Geometry_generated.h" +#include "VrepControlPoint_generated.h" + +namespace grl { +namespace flatbuffer { + +struct VrepPath; +struct VrepPathT; + +struct VrepPathT : public flatbuffers::NativeTable { + typedef VrepPath TableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.VrepPathT"; + } + std::vector> controlPoints; + VrepPathT() { + } +}; + +struct VrepPath FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { + typedef VrepPathT NativeTableType; + static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { + return "grl.flatbuffer.VrepPath"; + } + enum { + VT_CONTROLPOINTS = 4 + }; + const flatbuffers::Vector> *controlPoints() const { + return GetPointer> *>(VT_CONTROLPOINTS); + } + bool Verify(flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && + VerifyOffset(verifier, VT_CONTROLPOINTS) && + verifier.Verify(controlPoints()) && + verifier.VerifyVectorOfTables(controlPoints()) && + verifier.EndTable(); + } + VrepPathT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; + void UnPackTo(VrepPathT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; + static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const VrepPathT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); +}; + +struct VrepPathBuilder { + flatbuffers::FlatBufferBuilder &fbb_; + flatbuffers::uoffset_t start_; + void add_controlPoints(flatbuffers::Offset>> controlPoints) { + fbb_.AddOffset(VrepPath::VT_CONTROLPOINTS, controlPoints); + } + explicit VrepPathBuilder(flatbuffers::FlatBufferBuilder &_fbb) + : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + VrepPathBuilder &operator=(const VrepPathBuilder &); + flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = flatbuffers::Offset(end); + return o; + } +}; + +inline flatbuffers::Offset CreateVrepPath( + flatbuffers::FlatBufferBuilder &_fbb, + flatbuffers::Offset>> controlPoints = 0) { + VrepPathBuilder builder_(_fbb); + builder_.add_controlPoints(controlPoints); + return builder_.Finish(); +} + +inline flatbuffers::Offset CreateVrepPathDirect( + flatbuffers::FlatBufferBuilder &_fbb, + const std::vector> *controlPoints = nullptr) { + return grl::flatbuffer::CreateVrepPath( + _fbb, + controlPoints ? _fbb.CreateVector>(*controlPoints) : 0); +} + +flatbuffers::Offset CreateVrepPath(flatbuffers::FlatBufferBuilder &_fbb, const VrepPathT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); + +inline VrepPathT *VrepPath::UnPack(const flatbuffers::resolver_function_t *_resolver) const { + auto _o = new VrepPathT(); + UnPackTo(_o, _resolver); + return _o; +} + +inline void VrepPath::UnPackTo(VrepPathT *_o, const flatbuffers::resolver_function_t *_resolver) const { + (void)_o; + (void)_resolver; + { auto _e = controlPoints(); if (_e) { _o->controlPoints.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->controlPoints[_i] = std::unique_ptr(_e->Get(_i)->UnPack(_resolver)); } } }; +} + +inline flatbuffers::Offset VrepPath::Pack(flatbuffers::FlatBufferBuilder &_fbb, const VrepPathT* _o, const flatbuffers::rehasher_function_t *_rehasher) { + return CreateVrepPath(_fbb, _o, _rehasher); +} + +inline flatbuffers::Offset CreateVrepPath(flatbuffers::FlatBufferBuilder &_fbb, const VrepPathT *_o, const flatbuffers::rehasher_function_t *_rehasher) { + (void)_rehasher; + (void)_o; + struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const VrepPathT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; + auto _controlPoints = _o->controlPoints.size() ? _fbb.CreateVector> (_o->controlPoints.size(), [](size_t i, _VectorArgs *__va) { return CreateVrepControlPoint(*__va->__fbb, __va->__o->controlPoints[i].get(), __va->__rehasher); }, &_va ) : 0; + return grl::flatbuffer::CreateVrepPath( + _fbb, + _controlPoints); +} + +inline const grl::flatbuffer::VrepPath *GetVrepPath(const void *buf) { + return flatbuffers::GetRoot(buf); +} + +inline const grl::flatbuffer::VrepPath *GetSizePrefixedVrepPath(const void *buf) { + return flatbuffers::GetSizePrefixedRoot(buf); +} + +inline bool VerifyVrepPathBuffer( + flatbuffers::Verifier &verifier) { + return verifier.VerifyBuffer(nullptr); +} + +inline bool VerifySizePrefixedVrepPathBuffer( + flatbuffers::Verifier &verifier) { + return verifier.VerifySizePrefixedBuffer(nullptr); +} + +inline void FinishVrepPathBuffer( + flatbuffers::FlatBufferBuilder &fbb, + flatbuffers::Offset root) { + fbb.Finish(root); +} + +inline void FinishSizePrefixedVrepPathBuffer( + flatbuffers::FlatBufferBuilder &fbb, + flatbuffers::Offset root) { + fbb.FinishSizePrefixed(root); +} + +inline std::unique_ptr UnPackVrepPath( + const void *buf, + const flatbuffers::resolver_function_t *res = nullptr) { + return std::unique_ptr(GetVrepPath(buf)->UnPack(res)); +} + +} // namespace flatbuffer +} // namespace grl + +#endif // FLATBUFFERS_GENERATED_VREPPATH_GRL_FLATBUFFER_H_ diff --git a/include/grl/flatbuffer/grl/__init__.py b/include/grl/flatbuffer/grl/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/include/grl/flatbuffer/grl/flatbuffer/ArmControlSeries.java b/include/grl/flatbuffer/grl/flatbuffer/ArmControlSeries.java new file mode 100644 index 0000000..74d88f7 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/ArmControlSeries.java @@ -0,0 +1,40 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class ArmControlSeries extends Table { + public static ArmControlSeries getRootAsArmControlSeries(ByteBuffer _bb) { return getRootAsArmControlSeries(_bb, new ArmControlSeries()); } + public static ArmControlSeries getRootAsArmControlSeries(ByteBuffer _bb, ArmControlSeries obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } + public static boolean ArmControlSeriesBufferHasIdentifier(ByteBuffer _bb) { return __has_identifier(_bb, "armc"); } + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public ArmControlSeries __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + public ArmControlState states(int j) { return states(new ArmControlState(), j); } + public ArmControlState states(ArmControlState obj, int j) { int o = __offset(4); return o != 0 ? obj.__assign(__indirect(__vector(o) + j * 4), bb) : null; } + public int statesLength() { int o = __offset(4); return o != 0 ? __vector_len(o) : 0; } + + public static int createArmControlSeries(FlatBufferBuilder builder, + int statesOffset) { + builder.startObject(1); + ArmControlSeries.addStates(builder, statesOffset); + return ArmControlSeries.endArmControlSeries(builder); + } + + public static void startArmControlSeries(FlatBufferBuilder builder) { builder.startObject(1); } + public static void addStates(FlatBufferBuilder builder, int statesOffset) { builder.addOffset(0, statesOffset, 0); } + public static int createStatesVector(FlatBufferBuilder builder, int[] data) { builder.startVector(4, data.length, 4); for (int i = data.length - 1; i >= 0; i--) builder.addOffset(data[i]); return builder.endVector(); } + public static void startStatesVector(FlatBufferBuilder builder, int numElems) { builder.startVector(4, numElems, 4); } + public static int endArmControlSeries(FlatBufferBuilder builder) { + int o = builder.endObject(); + return o; + } + public static void finishArmControlSeriesBuffer(FlatBufferBuilder builder, int offset) { builder.finish(offset, "armc"); } + public static void finishSizePrefixedArmControlSeriesBuffer(FlatBufferBuilder builder, int offset) { builder.finishSizePrefixed(offset, "armc"); } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/ArmControlSeries.py b/include/grl/flatbuffer/grl/flatbuffer/ArmControlSeries.py new file mode 100644 index 0000000..0e768ab --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/ArmControlSeries.py @@ -0,0 +1,44 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class ArmControlSeries(object): + __slots__ = ['_tab'] + + @classmethod + def GetRootAsArmControlSeries(cls, buf, offset): + n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) + x = ArmControlSeries() + x.Init(buf, n + offset) + return x + + # ArmControlSeries + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + + # ArmControlSeries + def States(self, j): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + x = self._tab.Vector(o) + x += flatbuffers.number_types.UOffsetTFlags.py_type(j) * 4 + x = self._tab.Indirect(x) + from .ArmControlState import ArmControlState + obj = ArmControlState() + obj.Init(self._tab.Bytes, x) + return obj + return None + + # ArmControlSeries + def StatesLength(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + return self._tab.VectorLen(o) + return 0 + +def ArmControlSeriesStart(builder): builder.StartObject(1) +def ArmControlSeriesAddStates(builder, states): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(states), 0) +def ArmControlSeriesStartStatesVector(builder, numElems): return builder.StartVector(4, numElems, 4) +def ArmControlSeriesEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/ArmControlState.java b/include/grl/flatbuffer/grl/flatbuffer/ArmControlState.java new file mode 100644 index 0000000..dcc9a2a --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/ArmControlState.java @@ -0,0 +1,51 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class ArmControlState extends Table { + public static ArmControlState getRootAsArmControlState(ByteBuffer _bb) { return getRootAsArmControlState(_bb, new ArmControlState()); } + public static ArmControlState getRootAsArmControlState(ByteBuffer _bb, ArmControlState obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public ArmControlState __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + public String name() { int o = __offset(4); return o != 0 ? __string(o + bb_pos) : null; } + public ByteBuffer nameAsByteBuffer() { return __vector_as_bytebuffer(4, 1); } + public ByteBuffer nameInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 4, 1); } + public long sequenceNumber() { int o = __offset(6); return o != 0 ? bb.getLong(o + bb_pos) : 0L; } + public double timeStamp() { int o = __offset(8); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } + public byte stateType() { int o = __offset(10); return o != 0 ? bb.get(o + bb_pos) : 0; } + public Table state(Table obj) { int o = __offset(12); return o != 0 ? __union(obj, o) : null; } + + public static int createArmControlState(FlatBufferBuilder builder, + int nameOffset, + long sequenceNumber, + double timeStamp, + byte state_type, + int stateOffset) { + builder.startObject(5); + ArmControlState.addTimeStamp(builder, timeStamp); + ArmControlState.addSequenceNumber(builder, sequenceNumber); + ArmControlState.addState(builder, stateOffset); + ArmControlState.addName(builder, nameOffset); + ArmControlState.addStateType(builder, state_type); + return ArmControlState.endArmControlState(builder); + } + + public static void startArmControlState(FlatBufferBuilder builder) { builder.startObject(5); } + public static void addName(FlatBufferBuilder builder, int nameOffset) { builder.addOffset(0, nameOffset, 0); } + public static void addSequenceNumber(FlatBufferBuilder builder, long sequenceNumber) { builder.addLong(1, sequenceNumber, 0L); } + public static void addTimeStamp(FlatBufferBuilder builder, double timeStamp) { builder.addDouble(2, timeStamp, 0.0); } + public static void addStateType(FlatBufferBuilder builder, byte stateType) { builder.addByte(3, stateType, 0); } + public static void addState(FlatBufferBuilder builder, int stateOffset) { builder.addOffset(4, stateOffset, 0); } + public static int endArmControlState(FlatBufferBuilder builder) { + int o = builder.endObject(); + return o; + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/ArmControlState.py b/include/grl/flatbuffer/grl/flatbuffer/ArmControlState.py new file mode 100644 index 0000000..abb9979 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/ArmControlState.py @@ -0,0 +1,65 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class ArmControlState(object): + __slots__ = ['_tab'] + + @classmethod + def GetRootAsArmControlState(cls, buf, offset): + n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) + x = ArmControlState() + x.Init(buf, n + offset) + return x + + # ArmControlState + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + + # ArmControlState + def Name(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + return self._tab.String(o + self._tab.Pos) + return bytes() + + # ArmControlState + def SequenceNumber(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Int64Flags, o + self._tab.Pos) + return 0 + + # ArmControlState + def TimeStamp(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) + return 0.0 + + # ArmControlState + def StateType(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Uint8Flags, o + self._tab.Pos) + return 0 + + # ArmControlState + def State(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(12)) + if o != 0: + from flatbuffers.table import Table + obj = Table(bytearray(), 0) + self._tab.Union(obj, o) + return obj + return None + +def ArmControlStateStart(builder): builder.StartObject(5) +def ArmControlStateAddName(builder, name): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(name), 0) +def ArmControlStateAddSequenceNumber(builder, sequenceNumber): builder.PrependInt64Slot(1, sequenceNumber, 0) +def ArmControlStateAddTimeStamp(builder, timeStamp): builder.PrependFloat64Slot(2, timeStamp, 0.0) +def ArmControlStateAddStateType(builder, stateType): builder.PrependUint8Slot(3, stateType, 0) +def ArmControlStateAddState(builder, state): builder.PrependUOffsetTRelativeSlot(4, flatbuffers.number_types.UOffsetTFlags.py_type(state), 0) +def ArmControlStateEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/ArmState.java b/include/grl/flatbuffer/grl/flatbuffer/ArmState.java new file mode 100644 index 0000000..80885a9 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/ArmState.java @@ -0,0 +1,21 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +public final class ArmState { + private ArmState() { } + public static final byte NONE = 0; + public static final byte StartArm = 1; + public static final byte StopArm = 2; + public static final byte PauseArm = 3; + public static final byte ShutdownArm = 4; + public static final byte TeachArm = 5; + public static final byte MoveArmTrajectory = 6; + public static final byte MoveArmJointServo = 7; + public static final byte MoveArmCartesianServo = 8; + + public static final String[] names = { "NONE", "StartArm", "StopArm", "PauseArm", "ShutdownArm", "TeachArm", "MoveArmTrajectory", "MoveArmJointServo", "MoveArmCartesianServo", }; + + public static String name(int e) { return names[e]; } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/ArmState.py b/include/grl/flatbuffer/grl/flatbuffer/ArmState.py new file mode 100644 index 0000000..e056f75 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/ArmState.py @@ -0,0 +1,15 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +class ArmState(object): + NONE = 0 + StartArm = 1 + StopArm = 2 + PauseArm = 3 + ShutdownArm = 4 + TeachArm = 5 + MoveArmTrajectory = 6 + MoveArmJointServo = 7 + MoveArmCartesianServo = 8 + diff --git a/include/grl/flatbuffer/grl/flatbuffer/CartesianImpedenceControlMode.java b/include/grl/flatbuffer/grl/flatbuffer/CartesianImpedenceControlMode.java new file mode 100644 index 0000000..360b7e9 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/CartesianImpedenceControlMode.java @@ -0,0 +1,69 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class CartesianImpedenceControlMode extends Table { + public static CartesianImpedenceControlMode getRootAsCartesianImpedenceControlMode(ByteBuffer _bb) { return getRootAsCartesianImpedenceControlMode(_bb, new CartesianImpedenceControlMode()); } + public static CartesianImpedenceControlMode getRootAsCartesianImpedenceControlMode(ByteBuffer _bb, CartesianImpedenceControlMode obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public CartesianImpedenceControlMode __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + /** + * actual stiffness to set rot:[nm/rad] + */ + public EulerPose stiffness() { return stiffness(new EulerPose()); } + public EulerPose stiffness(EulerPose obj) { int o = __offset(4); return o != 0 ? obj.__assign(o + bb_pos, bb) : null; } + /** + * actual damping to set + */ + public EulerPose damping() { return damping(new EulerPose()); } + public EulerPose damping(EulerPose obj) { int o = __offset(6); return o != 0 ? obj.__assign(o + bb_pos, bb) : null; } + /** + * [Nm/rad] must be => 0.0 + */ + public double nullspaceStiffness() { int o = __offset(8); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } + /** + * must be between 0.3-1.0 suggested is 0.7 + */ + public double nullspaceDamping() { int o = __offset(10); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } + /** + * maximum deviation from set goal in mm and radians + */ + public EulerPose maxPathDeviation() { return maxPathDeviation(new EulerPose()); } + public EulerPose maxPathDeviation(EulerPose obj) { int o = __offset(12); return o != 0 ? obj.__assign(o + bb_pos, bb) : null; } + /** + * trans: [mm/s] rot: [rad/s] + */ + public EulerPose maxCartesianVelocity() { return maxCartesianVelocity(new EulerPose()); } + public EulerPose maxCartesianVelocity(EulerPose obj) { int o = __offset(14); return o != 0 ? obj.__assign(o + bb_pos, bb) : null; } + /** + * xyz: Newtons rpy:Nm (all >=0) + */ + public EulerPose maxControlForce() { return maxControlForce(new EulerPose()); } + public EulerPose maxControlForce(EulerPose obj) { int o = __offset(16); return o != 0 ? obj.__assign(o + bb_pos, bb) : null; } + /** + * stop if max control force is exceeded + */ + public boolean maxControlForceExceededStop() { int o = __offset(18); return o != 0 ? 0!=bb.get(o + bb_pos) : false; } + + public static void startCartesianImpedenceControlMode(FlatBufferBuilder builder) { builder.startObject(8); } + public static void addStiffness(FlatBufferBuilder builder, int stiffnessOffset) { builder.addStruct(0, stiffnessOffset, 0); } + public static void addDamping(FlatBufferBuilder builder, int dampingOffset) { builder.addStruct(1, dampingOffset, 0); } + public static void addNullspaceStiffness(FlatBufferBuilder builder, double nullspaceStiffness) { builder.addDouble(2, nullspaceStiffness, 0.0); } + public static void addNullspaceDamping(FlatBufferBuilder builder, double nullspaceDamping) { builder.addDouble(3, nullspaceDamping, 0.0); } + public static void addMaxPathDeviation(FlatBufferBuilder builder, int maxPathDeviationOffset) { builder.addStruct(4, maxPathDeviationOffset, 0); } + public static void addMaxCartesianVelocity(FlatBufferBuilder builder, int maxCartesianVelocityOffset) { builder.addStruct(5, maxCartesianVelocityOffset, 0); } + public static void addMaxControlForce(FlatBufferBuilder builder, int maxControlForceOffset) { builder.addStruct(6, maxControlForceOffset, 0); } + public static void addMaxControlForceExceededStop(FlatBufferBuilder builder, boolean maxControlForceExceededStop) { builder.addBoolean(7, maxControlForceExceededStop, false); } + public static int endCartesianImpedenceControlMode(FlatBufferBuilder builder) { + int o = builder.endObject(); + return o; + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/CartesianImpedenceControlMode.py b/include/grl/flatbuffer/grl/flatbuffer/CartesianImpedenceControlMode.py new file mode 100644 index 0000000..a74292d --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/CartesianImpedenceControlMode.py @@ -0,0 +1,114 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class CartesianImpedenceControlMode(object): + __slots__ = ['_tab'] + + @classmethod + def GetRootAsCartesianImpedenceControlMode(cls, buf, offset): + n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) + x = CartesianImpedenceControlMode() + x.Init(buf, n + offset) + return x + + # CartesianImpedenceControlMode + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + +# /// actual stiffness to set rot:[nm/rad] + # CartesianImpedenceControlMode + def Stiffness(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + x = o + self._tab.Pos + from .EulerPose import EulerPose + obj = EulerPose() + obj.Init(self._tab.Bytes, x) + return obj + return None + +# /// actual damping to set + # CartesianImpedenceControlMode + def Damping(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) + if o != 0: + x = o + self._tab.Pos + from .EulerPose import EulerPose + obj = EulerPose() + obj.Init(self._tab.Bytes, x) + return obj + return None + +# /// [Nm/rad] must be => 0.0 + # CartesianImpedenceControlMode + def NullspaceStiffness(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) + return 0.0 + +# /// must be between 0.3-1.0 suggested is 0.7 + # CartesianImpedenceControlMode + def NullspaceDamping(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) + return 0.0 + +# /// maximum deviation from set goal in mm and radians + # CartesianImpedenceControlMode + def MaxPathDeviation(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(12)) + if o != 0: + x = o + self._tab.Pos + from .EulerPose import EulerPose + obj = EulerPose() + obj.Init(self._tab.Bytes, x) + return obj + return None + +# /// trans: [mm/s] rot: [rad/s] + # CartesianImpedenceControlMode + def MaxCartesianVelocity(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(14)) + if o != 0: + x = o + self._tab.Pos + from .EulerPose import EulerPose + obj = EulerPose() + obj.Init(self._tab.Bytes, x) + return obj + return None + +# /// xyz: Newtons rpy:Nm (all >=0) + # CartesianImpedenceControlMode + def MaxControlForce(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(16)) + if o != 0: + x = o + self._tab.Pos + from .EulerPose import EulerPose + obj = EulerPose() + obj.Init(self._tab.Bytes, x) + return obj + return None + +# /// stop if max control force is exceeded + # CartesianImpedenceControlMode + def MaxControlForceExceededStop(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(18)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.BoolFlags, o + self._tab.Pos) + return 0 + +def CartesianImpedenceControlModeStart(builder): builder.StartObject(8) +def CartesianImpedenceControlModeAddStiffness(builder, stiffness): builder.PrependStructSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(stiffness), 0) +def CartesianImpedenceControlModeAddDamping(builder, damping): builder.PrependStructSlot(1, flatbuffers.number_types.UOffsetTFlags.py_type(damping), 0) +def CartesianImpedenceControlModeAddNullspaceStiffness(builder, nullspaceStiffness): builder.PrependFloat64Slot(2, nullspaceStiffness, 0.0) +def CartesianImpedenceControlModeAddNullspaceDamping(builder, nullspaceDamping): builder.PrependFloat64Slot(3, nullspaceDamping, 0.0) +def CartesianImpedenceControlModeAddMaxPathDeviation(builder, maxPathDeviation): builder.PrependStructSlot(4, flatbuffers.number_types.UOffsetTFlags.py_type(maxPathDeviation), 0) +def CartesianImpedenceControlModeAddMaxCartesianVelocity(builder, maxCartesianVelocity): builder.PrependStructSlot(5, flatbuffers.number_types.UOffsetTFlags.py_type(maxCartesianVelocity), 0) +def CartesianImpedenceControlModeAddMaxControlForce(builder, maxControlForce): builder.PrependStructSlot(6, flatbuffers.number_types.UOffsetTFlags.py_type(maxControlForce), 0) +def CartesianImpedenceControlModeAddMaxControlForceExceededStop(builder, maxControlForceExceededStop): builder.PrependBoolSlot(7, maxControlForceExceededStop, 0) +def CartesianImpedenceControlModeEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/DeviceState.java b/include/grl/flatbuffer/grl/flatbuffer/DeviceState.java new file mode 100644 index 0000000..d82dcd2 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/DeviceState.java @@ -0,0 +1,15 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +public final class DeviceState { + private DeviceState() { } + public static final byte NONE = 0; + public static final byte KUKAiiwaState = 1; + public static final byte FusionTrackMessage = 2; + + public static final String[] names = { "NONE", "KUKAiiwaState", "FusionTrackMessage", }; + + public static String name(int e) { return names[e]; } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/DeviceState.py b/include/grl/flatbuffer/grl/flatbuffer/DeviceState.py new file mode 100644 index 0000000..64f92d2 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/DeviceState.py @@ -0,0 +1,9 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +class DeviceState(object): + NONE = 0 + KUKAiiwaState = 1 + FusionTrackMessage = 2 + diff --git a/include/grl/flatbuffer/grl/flatbuffer/Disabled.java b/include/grl/flatbuffer/grl/flatbuffer/Disabled.java new file mode 100644 index 0000000..9b2f136 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/Disabled.java @@ -0,0 +1,24 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class Disabled extends Table { + public static Disabled getRootAsDisabled(ByteBuffer _bb) { return getRootAsDisabled(_bb, new Disabled()); } + public static Disabled getRootAsDisabled(ByteBuffer _bb, Disabled obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public Disabled __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + + public static void startDisabled(FlatBufferBuilder builder) { builder.startObject(0); } + public static int endDisabled(FlatBufferBuilder builder) { + int o = builder.endObject(); + return o; + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/Disabled.py b/include/grl/flatbuffer/grl/flatbuffer/Disabled.py new file mode 100644 index 0000000..a91db21 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/Disabled.py @@ -0,0 +1,22 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class Disabled(object): + __slots__ = ['_tab'] + + @classmethod + def GetRootAsDisabled(cls, buf, offset): + n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) + x = Disabled() + x.Init(buf, n + offset) + return x + + # Disabled + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + +def DisabledStart(builder): builder.StartObject(0) +def DisabledEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/EClientCommandMode.java b/include/grl/flatbuffer/grl/flatbuffer/EClientCommandMode.java new file mode 100644 index 0000000..c68bae8 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/EClientCommandMode.java @@ -0,0 +1,19 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +/** + * Type of command being sent to the arm (Dimensonal units) + */ +public final class EClientCommandMode { + private EClientCommandMode() { } + public static final byte NO_COMMAND_MODE = 0; + public static final byte POSITION = 1; + public static final byte WRENCH = 2; + public static final byte TORQUE = 3; + + public static final String[] names = { "NO_COMMAND_MODE", "POSITION", "WRENCH", "TORQUE", }; + + public static String name(int e) { return names[e]; } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/EClientCommandMode.py b/include/grl/flatbuffer/grl/flatbuffer/EClientCommandMode.py new file mode 100644 index 0000000..92ff046 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/EClientCommandMode.py @@ -0,0 +1,11 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +# /// Type of command being sent to the arm (Dimensonal units) +class EClientCommandMode(object): + NO_COMMAND_MODE = 0 + POSITION = 1 + WRENCH = 2 + TORQUE = 3 + diff --git a/include/grl/flatbuffer/grl/flatbuffer/EConnectionQuality.java b/include/grl/flatbuffer/grl/flatbuffer/EConnectionQuality.java new file mode 100644 index 0000000..830f5a7 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/EConnectionQuality.java @@ -0,0 +1,16 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +public final class EConnectionQuality { + private EConnectionQuality() { } + public static final byte POOR = 0; + public static final byte FAIR = 1; + public static final byte GOOD = 2; + public static final byte EXCELLENT = 3; + + public static final String[] names = { "POOR", "FAIR", "GOOD", "EXCELLENT", }; + + public static String name(int e) { return names[e]; } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/EConnectionQuality.py b/include/grl/flatbuffer/grl/flatbuffer/EConnectionQuality.py new file mode 100644 index 0000000..cc17e4b --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/EConnectionQuality.py @@ -0,0 +1,10 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +class EConnectionQuality(object): + POOR = 0 + FAIR = 1 + GOOD = 2 + EXCELLENT = 3 + diff --git a/include/grl/flatbuffer/grl/flatbuffer/EControlMode.java b/include/grl/flatbuffer/grl/flatbuffer/EControlMode.java new file mode 100644 index 0000000..41c97a5 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/EControlMode.java @@ -0,0 +1,16 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +public final class EControlMode { + private EControlMode() { } + public static final byte POSITION_CONTROL_MODE = 0; + public static final byte CART_IMP_CONTROL_MODE = 1; + public static final byte JOINT_IMP_CONTROL_MODE = 2; + public static final byte NO_CONTROL = 3; + + public static final String[] names = { "POSITION_CONTROL_MODE", "CART_IMP_CONTROL_MODE", "JOINT_IMP_CONTROL_MODE", "NO_CONTROL", }; + + public static String name(int e) { return names[e]; } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/EControlMode.py b/include/grl/flatbuffer/grl/flatbuffer/EControlMode.py new file mode 100644 index 0000000..2ea13d6 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/EControlMode.py @@ -0,0 +1,10 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +class EControlMode(object): + POSITION_CONTROL_MODE = 0 + CART_IMP_CONTROL_MODE = 1 + JOINT_IMP_CONTROL_MODE = 2 + NO_CONTROL = 3 + diff --git a/include/grl/flatbuffer/grl/flatbuffer/EDriveState.java b/include/grl/flatbuffer/grl/flatbuffer/EDriveState.java new file mode 100644 index 0000000..fca0cf4 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/EDriveState.java @@ -0,0 +1,24 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +public final class EDriveState { + private EDriveState() { } + /** + * Driving mode currently unused + */ + public static final byte OFF = 1; + /** + * About to drive + */ + public static final byte TRANSITIONING = 2; + /** + * Actively commanding arm + */ + public static final byte ACTIVE = 3; + + public static final String[] names = { "OFF", "TRANSITIONING", "ACTIVE", }; + + public static String name(int e) { return names[e - OFF]; } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/EDriveState.py b/include/grl/flatbuffer/grl/flatbuffer/EDriveState.py new file mode 100644 index 0000000..4a504e1 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/EDriveState.py @@ -0,0 +1,12 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +class EDriveState(object): +# /// Driving mode currently unused + OFF = 1 +# /// About to drive + TRANSITIONING = 2 +# /// Actively commanding arm + ACTIVE = 3 + diff --git a/include/grl/flatbuffer/grl/flatbuffer/EOperationMode.java b/include/grl/flatbuffer/grl/flatbuffer/EOperationMode.java new file mode 100644 index 0000000..30b049d --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/EOperationMode.java @@ -0,0 +1,15 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +public final class EOperationMode { + private EOperationMode() { } + public static final byte TEST_MODE_1 = 0; + public static final byte TEST_MODE_2 = 1; + public static final byte AUTOMATIC_MODE = 2; + + public static final String[] names = { "TEST_MODE_1", "TEST_MODE_2", "AUTOMATIC_MODE", }; + + public static String name(int e) { return names[e]; } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/EOperationMode.py b/include/grl/flatbuffer/grl/flatbuffer/EOperationMode.py new file mode 100644 index 0000000..898ee70 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/EOperationMode.py @@ -0,0 +1,9 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +class EOperationMode(object): + TEST_MODE_1 = 0 + TEST_MODE_2 = 1 + AUTOMATIC_MODE = 2 + diff --git a/include/grl/flatbuffer/grl/flatbuffer/EOverlayType.java b/include/grl/flatbuffer/grl/flatbuffer/EOverlayType.java new file mode 100644 index 0000000..61e5cf2 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/EOverlayType.java @@ -0,0 +1,15 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +public final class EOverlayType { + private EOverlayType() { } + public static final byte NO_OVERLAY = 0; + public static final byte JOINT = 1; + public static final byte CARTESIAN = 2; + + public static final String[] names = { "NO_OVERLAY", "JOINT", "CARTESIAN", }; + + public static String name(int e) { return names[e]; } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/EOverlayType.py b/include/grl/flatbuffer/grl/flatbuffer/EOverlayType.py new file mode 100644 index 0000000..b3e63f5 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/EOverlayType.py @@ -0,0 +1,9 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +class EOverlayType(object): + NO_OVERLAY = 0 + JOINT = 1 + CARTESIAN = 2 + diff --git a/include/grl/flatbuffer/grl/flatbuffer/ESafetyState.java b/include/grl/flatbuffer/grl/flatbuffer/ESafetyState.java new file mode 100644 index 0000000..4c463ee --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/ESafetyState.java @@ -0,0 +1,16 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +public final class ESafetyState { + private ESafetyState() { } + public static final byte NORMAL_OPERATION = 0; + public static final byte SAFETY_STOP_LEVEL_0 = 1; + public static final byte SAFETY_STOP_LEVEL_1 = 2; + public static final byte SAFETY_STOP_LEVEL_2 = 3; + + public static final String[] names = { "NORMAL_OPERATION", "SAFETY_STOP_LEVEL_0", "SAFETY_STOP_LEVEL_1", "SAFETY_STOP_LEVEL_2", }; + + public static String name(int e) { return names[e]; } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/ESafetyState.py b/include/grl/flatbuffer/grl/flatbuffer/ESafetyState.py new file mode 100644 index 0000000..82af46a --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/ESafetyState.py @@ -0,0 +1,10 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +class ESafetyState(object): + NORMAL_OPERATION = 0 + SAFETY_STOP_LEVEL_0 = 1 + SAFETY_STOP_LEVEL_1 = 2 + SAFETY_STOP_LEVEL_2 = 3 + diff --git a/include/grl/flatbuffer/grl/flatbuffer/ESessionState.java b/include/grl/flatbuffer/grl/flatbuffer/ESessionState.java new file mode 100644 index 0000000..41fc63f --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/ESessionState.java @@ -0,0 +1,32 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +public final class ESessionState { + private ESessionState() { } + /** + * No session + */ + public static final byte IDLE = 0; + /** + * Monitoring mode, receive state but connection too inconsistent to command + */ + public static final byte MONITORING_WAIT = 1; + /** + * Monitoring mode + */ + public static final byte MONITORING_READY = 2; + /** + * About to command (Overlay created in Java interface) + */ + public static final byte COMMANDING_WAIT = 3; + /** + * Actively commanding the arm with FRI + */ + public static final byte COMMANDING_ACTIVE = 4; + + public static final String[] names = { "IDLE", "MONITORING_WAIT", "MONITORING_READY", "COMMANDING_WAIT", "COMMANDING_ACTIVE", }; + + public static String name(int e) { return names[e]; } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/ESessionState.py b/include/grl/flatbuffer/grl/flatbuffer/ESessionState.py new file mode 100644 index 0000000..efcb0cd --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/ESessionState.py @@ -0,0 +1,16 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +class ESessionState(object): +# /// No session + IDLE = 0 +# /// Monitoring mode, receive state but connection too inconsistent to command + MONITORING_WAIT = 1 +# /// Monitoring mode + MONITORING_READY = 2 +# /// About to command (Overlay created in Java interface) + COMMANDING_WAIT = 3 +# /// Actively commanding the arm with FRI + COMMANDING_ACTIVE = 4 + diff --git a/include/grl/flatbuffer/grl/flatbuffer/EulerOrder.java b/include/grl/flatbuffer/grl/flatbuffer/EulerOrder.java new file mode 100644 index 0000000..5d507ef --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/EulerOrder.java @@ -0,0 +1,23 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +public final class EulerOrder { + private EulerOrder() { } + public static final byte xyz = 0; + public static final byte yzx = 1; + public static final byte zxy = 2; + public static final byte xzy = 3; + public static final byte zyx = 4; + public static final byte yxz = 5; + public static final byte zxz = 6; + public static final byte xyx = 7; + public static final byte yzy = 8; + public static final byte xzx = 9; + public static final byte yxy = 10; + + public static final String[] names = { "xyz", "yzx", "zxy", "xzy", "zyx", "yxz", "zxz", "xyx", "yzy", "xzx", "yxy", }; + + public static String name(int e) { return names[e]; } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/EulerOrder.py b/include/grl/flatbuffer/grl/flatbuffer/EulerOrder.py new file mode 100644 index 0000000..c95a5ec --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/EulerOrder.py @@ -0,0 +1,17 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +class EulerOrder(object): + xyz = 0 + yzx = 1 + zxy = 2 + xzy = 3 + zyx = 4 + yxz = 5 + zxz = 6 + xyx = 7 + yzy = 8 + xzx = 9 + yxy = 10 + diff --git a/include/grl/flatbuffer/grl/flatbuffer/EulerPose.java b/include/grl/flatbuffer/grl/flatbuffer/EulerPose.java new file mode 100644 index 0000000..70141bc --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/EulerPose.java @@ -0,0 +1,35 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class EulerPose extends Struct { + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public EulerPose __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + public Vector3d position() { return position(new Vector3d()); } + public Vector3d position(Vector3d obj) { return obj.__assign(bb_pos + 0, bb); } + public EulerRotation rotation() { return rotation(new EulerRotation()); } + public EulerRotation rotation(EulerRotation obj) { return obj.__assign(bb_pos + 24, bb); } + + public static int createEulerPose(FlatBufferBuilder builder, double position_x, double position_y, double position_z, double rotation_r1, double rotation_r2, double rotation_r3, byte rotation_eulerOrder) { + builder.prep(8, 56); + builder.prep(8, 32); + builder.pad(7); + builder.putByte(rotation_eulerOrder); + builder.putDouble(rotation_r3); + builder.putDouble(rotation_r2); + builder.putDouble(rotation_r1); + builder.prep(8, 24); + builder.putDouble(position_z); + builder.putDouble(position_y); + builder.putDouble(position_x); + return builder.offset(); + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/EulerPose.py b/include/grl/flatbuffer/grl/flatbuffer/EulerPose.py new file mode 100644 index 0000000..54497ad --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/EulerPose.py @@ -0,0 +1,37 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class EulerPose(object): + __slots__ = ['_tab'] + + # EulerPose + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + + # EulerPose + def Position(self, obj): + obj.Init(self._tab.Bytes, self._tab.Pos + 0) + return obj + + # EulerPose + def Rotation(self, obj): + obj.Init(self._tab.Bytes, self._tab.Pos + 24) + return obj + + +def CreateEulerPose(builder, position_x, position_y, position_z, rotation_r1, rotation_r2, rotation_r3, rotation_eulerOrder): + builder.Prep(8, 56) + builder.Prep(8, 32) + builder.Pad(7) + builder.PrependInt8(rotation_eulerOrder) + builder.PrependFloat64(rotation_r3) + builder.PrependFloat64(rotation_r2) + builder.PrependFloat64(rotation_r1) + builder.Prep(8, 24) + builder.PrependFloat64(position_z) + builder.PrependFloat64(position_y) + builder.PrependFloat64(position_x) + return builder.Offset() diff --git a/include/grl/flatbuffer/grl/flatbuffer/EulerPoseParams.java b/include/grl/flatbuffer/grl/flatbuffer/EulerPoseParams.java new file mode 100644 index 0000000..ec8a1a2 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/EulerPoseParams.java @@ -0,0 +1,30 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class EulerPoseParams extends Table { + public static EulerPoseParams getRootAsEulerPoseParams(ByteBuffer _bb) { return getRootAsEulerPoseParams(_bb, new EulerPoseParams()); } + public static EulerPoseParams getRootAsEulerPoseParams(ByteBuffer _bb, EulerPoseParams obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public EulerPoseParams __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + public Vector3d position() { return position(new Vector3d()); } + public Vector3d position(Vector3d obj) { int o = __offset(4); return o != 0 ? obj.__assign(o + bb_pos, bb) : null; } + public EulerRotation rotation() { return rotation(new EulerRotation()); } + public EulerRotation rotation(EulerRotation obj) { int o = __offset(6); return o != 0 ? obj.__assign(o + bb_pos, bb) : null; } + + public static void startEulerPoseParams(FlatBufferBuilder builder) { builder.startObject(2); } + public static void addPosition(FlatBufferBuilder builder, int positionOffset) { builder.addStruct(0, positionOffset, 0); } + public static void addRotation(FlatBufferBuilder builder, int rotationOffset) { builder.addStruct(1, rotationOffset, 0); } + public static int endEulerPoseParams(FlatBufferBuilder builder) { + int o = builder.endObject(); + return o; + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/EulerPoseParams.py b/include/grl/flatbuffer/grl/flatbuffer/EulerPoseParams.py new file mode 100644 index 0000000..39dd403 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/EulerPoseParams.py @@ -0,0 +1,46 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class EulerPoseParams(object): + __slots__ = ['_tab'] + + @classmethod + def GetRootAsEulerPoseParams(cls, buf, offset): + n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) + x = EulerPoseParams() + x.Init(buf, n + offset) + return x + + # EulerPoseParams + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + + # EulerPoseParams + def Position(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + x = o + self._tab.Pos + from .Vector3d import Vector3d + obj = Vector3d() + obj.Init(self._tab.Bytes, x) + return obj + return None + + # EulerPoseParams + def Rotation(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) + if o != 0: + x = o + self._tab.Pos + from .EulerRotation import EulerRotation + obj = EulerRotation() + obj.Init(self._tab.Bytes, x) + return obj + return None + +def EulerPoseParamsStart(builder): builder.StartObject(2) +def EulerPoseParamsAddPosition(builder, position): builder.PrependStructSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(position), 0) +def EulerPoseParamsAddRotation(builder, rotation): builder.PrependStructSlot(1, flatbuffers.number_types.UOffsetTFlags.py_type(rotation), 0) +def EulerPoseParamsEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/EulerRotation.java b/include/grl/flatbuffer/grl/flatbuffer/EulerRotation.java new file mode 100644 index 0000000..e7c810f --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/EulerRotation.java @@ -0,0 +1,30 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class EulerRotation extends Struct { + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public EulerRotation __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + public double r1() { return bb.getDouble(bb_pos + 0); } + public double r2() { return bb.getDouble(bb_pos + 8); } + public double r3() { return bb.getDouble(bb_pos + 16); } + public byte eulerOrder() { return bb.get(bb_pos + 24); } + + public static int createEulerRotation(FlatBufferBuilder builder, double r1, double r2, double r3, byte eulerOrder) { + builder.prep(8, 32); + builder.pad(7); + builder.putByte(eulerOrder); + builder.putDouble(r3); + builder.putDouble(r2); + builder.putDouble(r1); + return builder.offset(); + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/EulerRotation.py b/include/grl/flatbuffer/grl/flatbuffer/EulerRotation.py new file mode 100644 index 0000000..c8353a5 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/EulerRotation.py @@ -0,0 +1,30 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class EulerRotation(object): + __slots__ = ['_tab'] + + # EulerRotation + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + + # EulerRotation + def R1(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(0)) + # EulerRotation + def R2(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(8)) + # EulerRotation + def R3(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(16)) + # EulerRotation + def EulerOrder(self): return self._tab.Get(flatbuffers.number_types.Int8Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(24)) + +def CreateEulerRotation(builder, r1, r2, r3, eulerOrder): + builder.Prep(8, 32) + builder.Pad(7) + builder.PrependInt8(eulerOrder) + builder.PrependFloat64(r3) + builder.PrependFloat64(r2) + builder.PrependFloat64(r1) + return builder.Offset() diff --git a/include/grl/flatbuffer/grl/flatbuffer/EulerRotationParams.java b/include/grl/flatbuffer/grl/flatbuffer/EulerRotationParams.java new file mode 100644 index 0000000..b6c8b5d --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/EulerRotationParams.java @@ -0,0 +1,45 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class EulerRotationParams extends Table { + public static EulerRotationParams getRootAsEulerRotationParams(ByteBuffer _bb) { return getRootAsEulerRotationParams(_bb, new EulerRotationParams()); } + public static EulerRotationParams getRootAsEulerRotationParams(ByteBuffer _bb, EulerRotationParams obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public EulerRotationParams __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + public double r1() { int o = __offset(4); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } + public double r2() { int o = __offset(6); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } + public double r3() { int o = __offset(8); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } + public byte eulerOrder() { int o = __offset(10); return o != 0 ? bb.get(o + bb_pos) : 0; } + + public static int createEulerRotationParams(FlatBufferBuilder builder, + double r1, + double r2, + double r3, + byte eulerOrder) { + builder.startObject(4); + EulerRotationParams.addR3(builder, r3); + EulerRotationParams.addR2(builder, r2); + EulerRotationParams.addR1(builder, r1); + EulerRotationParams.addEulerOrder(builder, eulerOrder); + return EulerRotationParams.endEulerRotationParams(builder); + } + + public static void startEulerRotationParams(FlatBufferBuilder builder) { builder.startObject(4); } + public static void addR1(FlatBufferBuilder builder, double r1) { builder.addDouble(0, r1, 0.0); } + public static void addR2(FlatBufferBuilder builder, double r2) { builder.addDouble(1, r2, 0.0); } + public static void addR3(FlatBufferBuilder builder, double r3) { builder.addDouble(2, r3, 0.0); } + public static void addEulerOrder(FlatBufferBuilder builder, byte eulerOrder) { builder.addByte(3, eulerOrder, 0); } + public static int endEulerRotationParams(FlatBufferBuilder builder) { + int o = builder.endObject(); + return o; + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/EulerRotationParams.py b/include/grl/flatbuffer/grl/flatbuffer/EulerRotationParams.py new file mode 100644 index 0000000..e1ec4df --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/EulerRotationParams.py @@ -0,0 +1,54 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class EulerRotationParams(object): + __slots__ = ['_tab'] + + @classmethod + def GetRootAsEulerRotationParams(cls, buf, offset): + n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) + x = EulerRotationParams() + x.Init(buf, n + offset) + return x + + # EulerRotationParams + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + + # EulerRotationParams + def R1(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) + return 0.0 + + # EulerRotationParams + def R2(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) + return 0.0 + + # EulerRotationParams + def R3(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) + return 0.0 + + # EulerRotationParams + def EulerOrder(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Int8Flags, o + self._tab.Pos) + return 0 + +def EulerRotationParamsStart(builder): builder.StartObject(4) +def EulerRotationParamsAddR1(builder, r1): builder.PrependFloat64Slot(0, r1, 0.0) +def EulerRotationParamsAddR2(builder, r2): builder.PrependFloat64Slot(1, r2, 0.0) +def EulerRotationParamsAddR3(builder, r3): builder.PrependFloat64Slot(2, r3, 0.0) +def EulerRotationParamsAddEulerOrder(builder, eulerOrder): builder.PrependInt8Slot(3, eulerOrder, 0) +def EulerRotationParamsEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/EulerTranslationParams.java b/include/grl/flatbuffer/grl/flatbuffer/EulerTranslationParams.java new file mode 100644 index 0000000..6a12b1f --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/EulerTranslationParams.java @@ -0,0 +1,41 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class EulerTranslationParams extends Table { + public static EulerTranslationParams getRootAsEulerTranslationParams(ByteBuffer _bb) { return getRootAsEulerTranslationParams(_bb, new EulerTranslationParams()); } + public static EulerTranslationParams getRootAsEulerTranslationParams(ByteBuffer _bb, EulerTranslationParams obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public EulerTranslationParams __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + public double x() { int o = __offset(4); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } + public double y() { int o = __offset(6); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } + public double z() { int o = __offset(8); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } + + public static int createEulerTranslationParams(FlatBufferBuilder builder, + double x, + double y, + double z) { + builder.startObject(3); + EulerTranslationParams.addZ(builder, z); + EulerTranslationParams.addY(builder, y); + EulerTranslationParams.addX(builder, x); + return EulerTranslationParams.endEulerTranslationParams(builder); + } + + public static void startEulerTranslationParams(FlatBufferBuilder builder) { builder.startObject(3); } + public static void addX(FlatBufferBuilder builder, double x) { builder.addDouble(0, x, 0.0); } + public static void addY(FlatBufferBuilder builder, double y) { builder.addDouble(1, y, 0.0); } + public static void addZ(FlatBufferBuilder builder, double z) { builder.addDouble(2, z, 0.0); } + public static int endEulerTranslationParams(FlatBufferBuilder builder) { + int o = builder.endObject(); + return o; + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/EulerTranslationParams.py b/include/grl/flatbuffer/grl/flatbuffer/EulerTranslationParams.py new file mode 100644 index 0000000..eca6b41 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/EulerTranslationParams.py @@ -0,0 +1,46 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class EulerTranslationParams(object): + __slots__ = ['_tab'] + + @classmethod + def GetRootAsEulerTranslationParams(cls, buf, offset): + n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) + x = EulerTranslationParams() + x.Init(buf, n + offset) + return x + + # EulerTranslationParams + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + + # EulerTranslationParams + def X(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) + return 0.0 + + # EulerTranslationParams + def Y(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) + return 0.0 + + # EulerTranslationParams + def Z(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) + return 0.0 + +def EulerTranslationParamsStart(builder): builder.StartObject(3) +def EulerTranslationParamsAddX(builder, x): builder.PrependFloat64Slot(0, x, 0.0) +def EulerTranslationParamsAddY(builder, y): builder.PrependFloat64Slot(1, y, 0.0) +def EulerTranslationParamsAddZ(builder, z): builder.PrependFloat64Slot(2, z, 0.0) +def EulerTranslationParamsEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/EulerXYZd.java b/include/grl/flatbuffer/grl/flatbuffer/EulerXYZd.java new file mode 100644 index 0000000..17084ff --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/EulerXYZd.java @@ -0,0 +1,27 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class EulerXYZd extends Struct { + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public EulerXYZd __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + public double rx() { return bb.getDouble(bb_pos + 0); } + public double ry() { return bb.getDouble(bb_pos + 8); } + public double rz() { return bb.getDouble(bb_pos + 16); } + + public static int createEulerXYZd(FlatBufferBuilder builder, double rx, double ry, double rz) { + builder.prep(8, 24); + builder.putDouble(rz); + builder.putDouble(ry); + builder.putDouble(rx); + return builder.offset(); + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/EulerXYZd.py b/include/grl/flatbuffer/grl/flatbuffer/EulerXYZd.py new file mode 100644 index 0000000..cb2120b --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/EulerXYZd.py @@ -0,0 +1,26 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class EulerXYZd(object): + __slots__ = ['_tab'] + + # EulerXYZd + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + + # EulerXYZd + def Rx(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(0)) + # EulerXYZd + def Ry(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(8)) + # EulerXYZd + def Rz(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(16)) + +def CreateEulerXYZd(builder, rx, ry, rz): + builder.Prep(8, 24) + builder.PrependFloat64(rz) + builder.PrependFloat64(ry) + builder.PrependFloat64(rx) + return builder.Offset() diff --git a/include/grl/flatbuffer/grl/flatbuffer/FRI.java b/include/grl/flatbuffer/grl/flatbuffer/FRI.java new file mode 100644 index 0000000..8a43874 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/FRI.java @@ -0,0 +1,97 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class FRI extends Table { + public static FRI getRootAsFRI(ByteBuffer _bb) { return getRootAsFRI(_bb, new FRI()); } + public static FRI getRootAsFRI(ByteBuffer _bb, FRI obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public FRI __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + public byte overlayType() { int o = __offset(4); return o != 0 ? bb.get(o + bb_pos) : 1; } + /** + * Set the value for the send period of the connection from the KUKA controller to the remote side in [ms]. + * This means, the KUKA controller will send cyclic FRI messages every sendPeriod milliseconds to the remote side. + * + * + * Parameters: + * sendPeriod - the send period in milliseconds, 1 <= sendPeriod <= 100. + * Note: The recommended value for good performance should be between 1-5 milliseconds. + */ + public int sendPeriodMillisec() { int o = __offset(6); return o != 0 ? bb.getInt(o + bb_pos) : 4; } + /** + * Set the receive multiplier of the cycle time from the remote side to the KUKA controller. + * This multiplier defines the value of the receivePeriod which is calculated: + * receivePeriod = receiveMultiplier * sendPeriod + * + * The KUKA controller will expect a FRI response message every receivePeriod milliseconds from the remote side. + * + * The receivePeriod has to be within the range of: + * 1 <= receivePeriod <= 100. + */ + public int setReceiveMultiplier() { int o = __offset(8); return o != 0 ? bb.getInt(o + bb_pos) : 5; } + public boolean updatePortOnRemote() { int o = __offset(10); return o != 0 ? 0!=bb.get(o + bb_pos) : false; } + /** + * Set the port ID of the socket at the controller side. + * Note: Do not change this port ID, unless your application requires different port IDs on both ends of the FRI channel. + * For changing the FRI port ID on both sides, it is sufficient to call setPortOnRemote(int). + * Values of controllerPortID: + * "-1" - The configuration of setPortOnRemote(int) is used. This is the default. + * recommended range of port IDs: 30200 <= controllerPortID < 30210 + */ + public short portOnRemote() { int o = __offset(12); return o != 0 ? bb.getShort(o + bb_pos) : 0; } + public boolean updatePortOnController() { int o = __offset(14); return o != 0 ? 0!=bb.get(o + bb_pos) : false; } + /** + * Set the port ID of the FRI channel at the remote side. + * By default, this port ID is used on both sides of the FRI channel, unless specified otherwise by setPortOnController(int). + * + * Values of portID: + * + * default port ID: 30200 + * recommended range of port IDs: 30200 <= portID < 30210 + * Since the FRI channel utilizes UDP as connection layer, make sure, that your network topology (firewall, network services) are chosen accordingly. + * + * Parameters: + * portID - the port ID > 0 (also known as UDP port number) + */ + public short portOnController() { int o = __offset(16); return o != 0 ? bb.getShort(o + bb_pos) : 0; } + + public static int createFRI(FlatBufferBuilder builder, + byte overlayType, + int sendPeriodMillisec, + int setReceiveMultiplier, + boolean updatePortOnRemote, + short portOnRemote, + boolean updatePortOnController, + short portOnController) { + builder.startObject(7); + FRI.addSetReceiveMultiplier(builder, setReceiveMultiplier); + FRI.addSendPeriodMillisec(builder, sendPeriodMillisec); + FRI.addPortOnController(builder, portOnController); + FRI.addPortOnRemote(builder, portOnRemote); + FRI.addUpdatePortOnController(builder, updatePortOnController); + FRI.addUpdatePortOnRemote(builder, updatePortOnRemote); + FRI.addOverlayType(builder, overlayType); + return FRI.endFRI(builder); + } + + public static void startFRI(FlatBufferBuilder builder) { builder.startObject(7); } + public static void addOverlayType(FlatBufferBuilder builder, byte overlayType) { builder.addByte(0, overlayType, 1); } + public static void addSendPeriodMillisec(FlatBufferBuilder builder, int sendPeriodMillisec) { builder.addInt(1, sendPeriodMillisec, 4); } + public static void addSetReceiveMultiplier(FlatBufferBuilder builder, int setReceiveMultiplier) { builder.addInt(2, setReceiveMultiplier, 5); } + public static void addUpdatePortOnRemote(FlatBufferBuilder builder, boolean updatePortOnRemote) { builder.addBoolean(3, updatePortOnRemote, false); } + public static void addPortOnRemote(FlatBufferBuilder builder, short portOnRemote) { builder.addShort(4, portOnRemote, 0); } + public static void addUpdatePortOnController(FlatBufferBuilder builder, boolean updatePortOnController) { builder.addBoolean(5, updatePortOnController, false); } + public static void addPortOnController(FlatBufferBuilder builder, short portOnController) { builder.addShort(6, portOnController, 0); } + public static int endFRI(FlatBufferBuilder builder) { + int o = builder.endObject(); + return o; + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/FRI.py b/include/grl/flatbuffer/grl/flatbuffer/FRI.py new file mode 100644 index 0000000..96844c6 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/FRI.py @@ -0,0 +1,110 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class FRI(object): + __slots__ = ['_tab'] + + @classmethod + def GetRootAsFRI(cls, buf, offset): + n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) + x = FRI() + x.Init(buf, n + offset) + return x + + # FRI + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + + # FRI + def OverlayType(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Int8Flags, o + self._tab.Pos) + return 1 + +# /// Set the value for the send period of the connection from the KUKA controller to the remote side in [ms]. +# /// This means, the KUKA controller will send cyclic FRI messages every sendPeriod milliseconds to the remote side. +# /// +# /// +# /// Parameters: +# /// sendPeriod - the send period in milliseconds, 1 <= sendPeriod <= 100. +# /// Note: The recommended value for good performance should be between 1-5 milliseconds. + # FRI + def SendPeriodMillisec(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Int32Flags, o + self._tab.Pos) + return 4 + +# /// Set the receive multiplier of the cycle time from the remote side to the KUKA controller. +# /// This multiplier defines the value of the receivePeriod which is calculated: +# /// receivePeriod = receiveMultiplier * sendPeriod +# /// +# /// The KUKA controller will expect a FRI response message every receivePeriod milliseconds from the remote side. +# /// +# /// The receivePeriod has to be within the range of: +# /// 1 <= receivePeriod <= 100. + # FRI + def SetReceiveMultiplier(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Int32Flags, o + self._tab.Pos) + return 5 + + # FRI + def UpdatePortOnRemote(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.BoolFlags, o + self._tab.Pos) + return 0 + +# /// Set the port ID of the socket at the controller side. +# /// Note: Do not change this port ID, unless your application requires different port IDs on both ends of the FRI channel. +# /// For changing the FRI port ID on both sides, it is sufficient to call setPortOnRemote(int). +# /// Values of controllerPortID: +# /// "-1" - The configuration of setPortOnRemote(int) is used. This is the default. +# /// recommended range of port IDs: 30200 <= controllerPortID < 30210 + # FRI + def PortOnRemote(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(12)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Int16Flags, o + self._tab.Pos) + return 0 + + # FRI + def UpdatePortOnController(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(14)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.BoolFlags, o + self._tab.Pos) + return 0 + +# /// Set the port ID of the FRI channel at the remote side. +# /// By default, this port ID is used on both sides of the FRI channel, unless specified otherwise by setPortOnController(int). +# /// +# /// Values of portID: +# /// +# /// default port ID: 30200 +# /// recommended range of port IDs: 30200 <= portID < 30210 +# /// Since the FRI channel utilizes UDP as connection layer, make sure, that your network topology (firewall, network services) are chosen accordingly. +# /// +# /// Parameters: +# /// portID - the port ID > 0 (also known as UDP port number) + # FRI + def PortOnController(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(16)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Int16Flags, o + self._tab.Pos) + return 0 + +def FRIStart(builder): builder.StartObject(7) +def FRIAddOverlayType(builder, overlayType): builder.PrependInt8Slot(0, overlayType, 1) +def FRIAddSendPeriodMillisec(builder, sendPeriodMillisec): builder.PrependInt32Slot(1, sendPeriodMillisec, 4) +def FRIAddSetReceiveMultiplier(builder, setReceiveMultiplier): builder.PrependInt32Slot(2, setReceiveMultiplier, 5) +def FRIAddUpdatePortOnRemote(builder, updatePortOnRemote): builder.PrependBoolSlot(3, updatePortOnRemote, 0) +def FRIAddPortOnRemote(builder, portOnRemote): builder.PrependInt16Slot(4, portOnRemote, 0) +def FRIAddUpdatePortOnController(builder, updatePortOnController): builder.PrependBoolSlot(5, updatePortOnController, 0) +def FRIAddPortOnController(builder, portOnController): builder.PrependInt16Slot(6, portOnController, 0) +def FRIEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/FRIMessageLog.java b/include/grl/flatbuffer/grl/flatbuffer/FRIMessageLog.java new file mode 100644 index 0000000..3268eb8 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/FRIMessageLog.java @@ -0,0 +1,112 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class FRIMessageLog extends Table { + public static FRIMessageLog getRootAsFRIMessageLog(ByteBuffer _bb) { return getRootAsFRIMessageLog(_bb, new FRIMessageLog()); } + public static FRIMessageLog getRootAsFRIMessageLog(ByteBuffer _bb, FRIMessageLog obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public FRIMessageLog __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + public byte sessionState() { int o = __offset(4); return o != 0 ? bb.get(o + bb_pos) : 0; } + public byte connectionQuality() { int o = __offset(6); return o != 0 ? bb.get(o + bb_pos) : 0; } + public byte controlMode() { int o = __offset(8); return o != 0 ? bb.get(o + bb_pos) : 0; } + public int messageIdentifier() { int o = __offset(10); return o != 0 ? bb.getInt(o + bb_pos) : 0; } + public int sequenceCounter() { int o = __offset(12); return o != 0 ? bb.getInt(o + bb_pos) : 0; } + public int reflectedSequenceCounter() { int o = __offset(14); return o != 0 ? bb.getInt(o + bb_pos) : 0; } + public double measuredJointPosition(int j) { int o = __offset(16); return o != 0 ? bb.getDouble(__vector(o) + j * 8) : 0; } + public int measuredJointPositionLength() { int o = __offset(16); return o != 0 ? __vector_len(o) : 0; } + public ByteBuffer measuredJointPositionAsByteBuffer() { return __vector_as_bytebuffer(16, 8); } + public ByteBuffer measuredJointPositionInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 16, 8); } + public double measuredTorque(int j) { int o = __offset(18); return o != 0 ? bb.getDouble(__vector(o) + j * 8) : 0; } + public int measuredTorqueLength() { int o = __offset(18); return o != 0 ? __vector_len(o) : 0; } + public ByteBuffer measuredTorqueAsByteBuffer() { return __vector_as_bytebuffer(18, 8); } + public ByteBuffer measuredTorqueInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 18, 8); } + public double commandedJointPosition(int j) { int o = __offset(20); return o != 0 ? bb.getDouble(__vector(o) + j * 8) : 0; } + public int commandedJointPositionLength() { int o = __offset(20); return o != 0 ? __vector_len(o) : 0; } + public ByteBuffer commandedJointPositionAsByteBuffer() { return __vector_as_bytebuffer(20, 8); } + public ByteBuffer commandedJointPositionInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 20, 8); } + public double commandedTorque(int j) { int o = __offset(22); return o != 0 ? bb.getDouble(__vector(o) + j * 8) : 0; } + public int commandedTorqueLength() { int o = __offset(22); return o != 0 ? __vector_len(o) : 0; } + public ByteBuffer commandedTorqueAsByteBuffer() { return __vector_as_bytebuffer(22, 8); } + public ByteBuffer commandedTorqueInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 22, 8); } + public double externalTorque(int j) { int o = __offset(24); return o != 0 ? bb.getDouble(__vector(o) + j * 8) : 0; } + public int externalTorqueLength() { int o = __offset(24); return o != 0 ? __vector_len(o) : 0; } + public ByteBuffer externalTorqueAsByteBuffer() { return __vector_as_bytebuffer(24, 8); } + public ByteBuffer externalTorqueInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 24, 8); } + public double jointStateInterpolated(int j) { int o = __offset(26); return o != 0 ? bb.getDouble(__vector(o) + j * 8) : 0; } + public int jointStateInterpolatedLength() { int o = __offset(26); return o != 0 ? __vector_len(o) : 0; } + public ByteBuffer jointStateInterpolatedAsByteBuffer() { return __vector_as_bytebuffer(26, 8); } + public ByteBuffer jointStateInterpolatedInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 26, 8); } + public TimeEvent timeStamp() { return timeStamp(new TimeEvent()); } + public TimeEvent timeStamp(TimeEvent obj) { int o = __offset(28); return o != 0 ? obj.__assign(__indirect(o + bb_pos), bb) : null; } + + public static int createFRIMessageLog(FlatBufferBuilder builder, + byte sessionState, + byte connectionQuality, + byte controlMode, + int messageIdentifier, + int sequenceCounter, + int reflectedSequenceCounter, + int measuredJointPositionOffset, + int measuredTorqueOffset, + int commandedJointPositionOffset, + int commandedTorqueOffset, + int externalTorqueOffset, + int jointStateInterpolatedOffset, + int timeStampOffset) { + builder.startObject(13); + FRIMessageLog.addTimeStamp(builder, timeStampOffset); + FRIMessageLog.addJointStateInterpolated(builder, jointStateInterpolatedOffset); + FRIMessageLog.addExternalTorque(builder, externalTorqueOffset); + FRIMessageLog.addCommandedTorque(builder, commandedTorqueOffset); + FRIMessageLog.addCommandedJointPosition(builder, commandedJointPositionOffset); + FRIMessageLog.addMeasuredTorque(builder, measuredTorqueOffset); + FRIMessageLog.addMeasuredJointPosition(builder, measuredJointPositionOffset); + FRIMessageLog.addReflectedSequenceCounter(builder, reflectedSequenceCounter); + FRIMessageLog.addSequenceCounter(builder, sequenceCounter); + FRIMessageLog.addMessageIdentifier(builder, messageIdentifier); + FRIMessageLog.addControlMode(builder, controlMode); + FRIMessageLog.addConnectionQuality(builder, connectionQuality); + FRIMessageLog.addSessionState(builder, sessionState); + return FRIMessageLog.endFRIMessageLog(builder); + } + + public static void startFRIMessageLog(FlatBufferBuilder builder) { builder.startObject(13); } + public static void addSessionState(FlatBufferBuilder builder, byte sessionState) { builder.addByte(0, sessionState, 0); } + public static void addConnectionQuality(FlatBufferBuilder builder, byte connectionQuality) { builder.addByte(1, connectionQuality, 0); } + public static void addControlMode(FlatBufferBuilder builder, byte controlMode) { builder.addByte(2, controlMode, 0); } + public static void addMessageIdentifier(FlatBufferBuilder builder, int messageIdentifier) { builder.addInt(3, messageIdentifier, 0); } + public static void addSequenceCounter(FlatBufferBuilder builder, int sequenceCounter) { builder.addInt(4, sequenceCounter, 0); } + public static void addReflectedSequenceCounter(FlatBufferBuilder builder, int reflectedSequenceCounter) { builder.addInt(5, reflectedSequenceCounter, 0); } + public static void addMeasuredJointPosition(FlatBufferBuilder builder, int measuredJointPositionOffset) { builder.addOffset(6, measuredJointPositionOffset, 0); } + public static int createMeasuredJointPositionVector(FlatBufferBuilder builder, double[] data) { builder.startVector(8, data.length, 8); for (int i = data.length - 1; i >= 0; i--) builder.addDouble(data[i]); return builder.endVector(); } + public static void startMeasuredJointPositionVector(FlatBufferBuilder builder, int numElems) { builder.startVector(8, numElems, 8); } + public static void addMeasuredTorque(FlatBufferBuilder builder, int measuredTorqueOffset) { builder.addOffset(7, measuredTorqueOffset, 0); } + public static int createMeasuredTorqueVector(FlatBufferBuilder builder, double[] data) { builder.startVector(8, data.length, 8); for (int i = data.length - 1; i >= 0; i--) builder.addDouble(data[i]); return builder.endVector(); } + public static void startMeasuredTorqueVector(FlatBufferBuilder builder, int numElems) { builder.startVector(8, numElems, 8); } + public static void addCommandedJointPosition(FlatBufferBuilder builder, int commandedJointPositionOffset) { builder.addOffset(8, commandedJointPositionOffset, 0); } + public static int createCommandedJointPositionVector(FlatBufferBuilder builder, double[] data) { builder.startVector(8, data.length, 8); for (int i = data.length - 1; i >= 0; i--) builder.addDouble(data[i]); return builder.endVector(); } + public static void startCommandedJointPositionVector(FlatBufferBuilder builder, int numElems) { builder.startVector(8, numElems, 8); } + public static void addCommandedTorque(FlatBufferBuilder builder, int commandedTorqueOffset) { builder.addOffset(9, commandedTorqueOffset, 0); } + public static int createCommandedTorqueVector(FlatBufferBuilder builder, double[] data) { builder.startVector(8, data.length, 8); for (int i = data.length - 1; i >= 0; i--) builder.addDouble(data[i]); return builder.endVector(); } + public static void startCommandedTorqueVector(FlatBufferBuilder builder, int numElems) { builder.startVector(8, numElems, 8); } + public static void addExternalTorque(FlatBufferBuilder builder, int externalTorqueOffset) { builder.addOffset(10, externalTorqueOffset, 0); } + public static int createExternalTorqueVector(FlatBufferBuilder builder, double[] data) { builder.startVector(8, data.length, 8); for (int i = data.length - 1; i >= 0; i--) builder.addDouble(data[i]); return builder.endVector(); } + public static void startExternalTorqueVector(FlatBufferBuilder builder, int numElems) { builder.startVector(8, numElems, 8); } + public static void addJointStateInterpolated(FlatBufferBuilder builder, int jointStateInterpolatedOffset) { builder.addOffset(11, jointStateInterpolatedOffset, 0); } + public static int createJointStateInterpolatedVector(FlatBufferBuilder builder, double[] data) { builder.startVector(8, data.length, 8); for (int i = data.length - 1; i >= 0; i--) builder.addDouble(data[i]); return builder.endVector(); } + public static void startJointStateInterpolatedVector(FlatBufferBuilder builder, int numElems) { builder.startVector(8, numElems, 8); } + public static void addTimeStamp(FlatBufferBuilder builder, int timeStampOffset) { builder.addOffset(12, timeStampOffset, 0); } + public static int endFRIMessageLog(FlatBufferBuilder builder) { + int o = builder.endObject(); + return o; + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/FRIMessageLog.py b/include/grl/flatbuffer/grl/flatbuffer/FRIMessageLog.py new file mode 100644 index 0000000..c8b9336 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/FRIMessageLog.py @@ -0,0 +1,226 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class FRIMessageLog(object): + __slots__ = ['_tab'] + + @classmethod + def GetRootAsFRIMessageLog(cls, buf, offset): + n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) + x = FRIMessageLog() + x.Init(buf, n + offset) + return x + + # FRIMessageLog + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + + # FRIMessageLog + def SessionState(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Int8Flags, o + self._tab.Pos) + return 0 + + # FRIMessageLog + def ConnectionQuality(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Int8Flags, o + self._tab.Pos) + return 0 + + # FRIMessageLog + def ControlMode(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Int8Flags, o + self._tab.Pos) + return 0 + + # FRIMessageLog + def MessageIdentifier(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Int32Flags, o + self._tab.Pos) + return 0 + + # FRIMessageLog + def SequenceCounter(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(12)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Int32Flags, o + self._tab.Pos) + return 0 + + # FRIMessageLog + def ReflectedSequenceCounter(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(14)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Int32Flags, o + self._tab.Pos) + return 0 + + # FRIMessageLog + def MeasuredJointPosition(self, j): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(16)) + if o != 0: + a = self._tab.Vector(o) + return self._tab.Get(flatbuffers.number_types.Float64Flags, a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 8)) + return 0 + + # FRIMessageLog + def MeasuredJointPositionAsNumpy(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(16)) + if o != 0: + return self._tab.GetVectorAsNumpy(flatbuffers.number_types.Float64Flags, o) + return 0 + + # FRIMessageLog + def MeasuredJointPositionLength(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(16)) + if o != 0: + return self._tab.VectorLen(o) + return 0 + + # FRIMessageLog + def MeasuredTorque(self, j): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(18)) + if o != 0: + a = self._tab.Vector(o) + return self._tab.Get(flatbuffers.number_types.Float64Flags, a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 8)) + return 0 + + # FRIMessageLog + def MeasuredTorqueAsNumpy(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(18)) + if o != 0: + return self._tab.GetVectorAsNumpy(flatbuffers.number_types.Float64Flags, o) + return 0 + + # FRIMessageLog + def MeasuredTorqueLength(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(18)) + if o != 0: + return self._tab.VectorLen(o) + return 0 + + # FRIMessageLog + def CommandedJointPosition(self, j): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(20)) + if o != 0: + a = self._tab.Vector(o) + return self._tab.Get(flatbuffers.number_types.Float64Flags, a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 8)) + return 0 + + # FRIMessageLog + def CommandedJointPositionAsNumpy(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(20)) + if o != 0: + return self._tab.GetVectorAsNumpy(flatbuffers.number_types.Float64Flags, o) + return 0 + + # FRIMessageLog + def CommandedJointPositionLength(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(20)) + if o != 0: + return self._tab.VectorLen(o) + return 0 + + # FRIMessageLog + def CommandedTorque(self, j): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(22)) + if o != 0: + a = self._tab.Vector(o) + return self._tab.Get(flatbuffers.number_types.Float64Flags, a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 8)) + return 0 + + # FRIMessageLog + def CommandedTorqueAsNumpy(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(22)) + if o != 0: + return self._tab.GetVectorAsNumpy(flatbuffers.number_types.Float64Flags, o) + return 0 + + # FRIMessageLog + def CommandedTorqueLength(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(22)) + if o != 0: + return self._tab.VectorLen(o) + return 0 + + # FRIMessageLog + def ExternalTorque(self, j): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(24)) + if o != 0: + a = self._tab.Vector(o) + return self._tab.Get(flatbuffers.number_types.Float64Flags, a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 8)) + return 0 + + # FRIMessageLog + def ExternalTorqueAsNumpy(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(24)) + if o != 0: + return self._tab.GetVectorAsNumpy(flatbuffers.number_types.Float64Flags, o) + return 0 + + # FRIMessageLog + def ExternalTorqueLength(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(24)) + if o != 0: + return self._tab.VectorLen(o) + return 0 + + # FRIMessageLog + def JointStateInterpolated(self, j): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(26)) + if o != 0: + a = self._tab.Vector(o) + return self._tab.Get(flatbuffers.number_types.Float64Flags, a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 8)) + return 0 + + # FRIMessageLog + def JointStateInterpolatedAsNumpy(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(26)) + if o != 0: + return self._tab.GetVectorAsNumpy(flatbuffers.number_types.Float64Flags, o) + return 0 + + # FRIMessageLog + def JointStateInterpolatedLength(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(26)) + if o != 0: + return self._tab.VectorLen(o) + return 0 + + # FRIMessageLog + def TimeStamp(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(28)) + if o != 0: + x = self._tab.Indirect(o + self._tab.Pos) + from .TimeEvent import TimeEvent + obj = TimeEvent() + obj.Init(self._tab.Bytes, x) + return obj + return None + +def FRIMessageLogStart(builder): builder.StartObject(13) +def FRIMessageLogAddSessionState(builder, sessionState): builder.PrependInt8Slot(0, sessionState, 0) +def FRIMessageLogAddConnectionQuality(builder, connectionQuality): builder.PrependInt8Slot(1, connectionQuality, 0) +def FRIMessageLogAddControlMode(builder, controlMode): builder.PrependInt8Slot(2, controlMode, 0) +def FRIMessageLogAddMessageIdentifier(builder, messageIdentifier): builder.PrependInt32Slot(3, messageIdentifier, 0) +def FRIMessageLogAddSequenceCounter(builder, sequenceCounter): builder.PrependInt32Slot(4, sequenceCounter, 0) +def FRIMessageLogAddReflectedSequenceCounter(builder, reflectedSequenceCounter): builder.PrependInt32Slot(5, reflectedSequenceCounter, 0) +def FRIMessageLogAddMeasuredJointPosition(builder, measuredJointPosition): builder.PrependUOffsetTRelativeSlot(6, flatbuffers.number_types.UOffsetTFlags.py_type(measuredJointPosition), 0) +def FRIMessageLogStartMeasuredJointPositionVector(builder, numElems): return builder.StartVector(8, numElems, 8) +def FRIMessageLogAddMeasuredTorque(builder, measuredTorque): builder.PrependUOffsetTRelativeSlot(7, flatbuffers.number_types.UOffsetTFlags.py_type(measuredTorque), 0) +def FRIMessageLogStartMeasuredTorqueVector(builder, numElems): return builder.StartVector(8, numElems, 8) +def FRIMessageLogAddCommandedJointPosition(builder, commandedJointPosition): builder.PrependUOffsetTRelativeSlot(8, flatbuffers.number_types.UOffsetTFlags.py_type(commandedJointPosition), 0) +def FRIMessageLogStartCommandedJointPositionVector(builder, numElems): return builder.StartVector(8, numElems, 8) +def FRIMessageLogAddCommandedTorque(builder, commandedTorque): builder.PrependUOffsetTRelativeSlot(9, flatbuffers.number_types.UOffsetTFlags.py_type(commandedTorque), 0) +def FRIMessageLogStartCommandedTorqueVector(builder, numElems): return builder.StartVector(8, numElems, 8) +def FRIMessageLogAddExternalTorque(builder, externalTorque): builder.PrependUOffsetTRelativeSlot(10, flatbuffers.number_types.UOffsetTFlags.py_type(externalTorque), 0) +def FRIMessageLogStartExternalTorqueVector(builder, numElems): return builder.StartVector(8, numElems, 8) +def FRIMessageLogAddJointStateInterpolated(builder, jointStateInterpolated): builder.PrependUOffsetTRelativeSlot(11, flatbuffers.number_types.UOffsetTFlags.py_type(jointStateInterpolated), 0) +def FRIMessageLogStartJointStateInterpolatedVector(builder, numElems): return builder.StartVector(8, numElems, 8) +def FRIMessageLogAddTimeStamp(builder, timeStamp): builder.PrependUOffsetTRelativeSlot(12, flatbuffers.number_types.UOffsetTFlags.py_type(timeStamp), 0) +def FRIMessageLogEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/FRITimeStamp.java b/include/grl/flatbuffer/grl/flatbuffer/FRITimeStamp.java new file mode 100644 index 0000000..8604f8c --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/FRITimeStamp.java @@ -0,0 +1,37 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class FRITimeStamp extends Table { + public static FRITimeStamp getRootAsFRITimeStamp(ByteBuffer _bb) { return getRootAsFRITimeStamp(_bb, new FRITimeStamp()); } + public static FRITimeStamp getRootAsFRITimeStamp(ByteBuffer _bb, FRITimeStamp obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public FRITimeStamp __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + public int sec() { int o = __offset(4); return o != 0 ? bb.getInt(o + bb_pos) : 0; } + public int nanosec() { int o = __offset(6); return o != 0 ? bb.getInt(o + bb_pos) : 0; } + + public static int createFRITimeStamp(FlatBufferBuilder builder, + int sec, + int nanosec) { + builder.startObject(2); + FRITimeStamp.addNanosec(builder, nanosec); + FRITimeStamp.addSec(builder, sec); + return FRITimeStamp.endFRITimeStamp(builder); + } + + public static void startFRITimeStamp(FlatBufferBuilder builder) { builder.startObject(2); } + public static void addSec(FlatBufferBuilder builder, int sec) { builder.addInt(0, sec, 0); } + public static void addNanosec(FlatBufferBuilder builder, int nanosec) { builder.addInt(1, nanosec, 0); } + public static int endFRITimeStamp(FlatBufferBuilder builder) { + int o = builder.endObject(); + return o; + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/FRITimeStamp.py b/include/grl/flatbuffer/grl/flatbuffer/FRITimeStamp.py new file mode 100644 index 0000000..1b7c07e --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/FRITimeStamp.py @@ -0,0 +1,38 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class FRITimeStamp(object): + __slots__ = ['_tab'] + + @classmethod + def GetRootAsFRITimeStamp(cls, buf, offset): + n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) + x = FRITimeStamp() + x.Init(buf, n + offset) + return x + + # FRITimeStamp + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + + # FRITimeStamp + def Sec(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Int32Flags, o + self._tab.Pos) + return 0 + + # FRITimeStamp + def Nanosec(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Int32Flags, o + self._tab.Pos) + return 0 + +def FRITimeStampStart(builder): builder.StartObject(2) +def FRITimeStampAddSec(builder, sec): builder.PrependInt32Slot(0, sec, 0) +def FRITimeStampAddNanosec(builder, nanosec): builder.PrependInt32Slot(1, nanosec, 0) +def FRITimeStampEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/FusionTrackFrame.java b/include/grl/flatbuffer/grl/flatbuffer/FusionTrackFrame.java new file mode 100644 index 0000000..58548e6 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/FusionTrackFrame.java @@ -0,0 +1,179 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +/** + * Data for one frame capture + * On the Atracsys FusionTrack optical tracker + * look at ftkInterface.h for details + * Frame class is defined in FusionTrack.hpp + */ +public final class FusionTrackFrame extends Table { + public static FusionTrackFrame getRootAsFusionTrackFrame(ByteBuffer _bb) { return getRootAsFusionTrackFrame(_bb, new FusionTrackFrame()); } + public static FusionTrackFrame getRootAsFusionTrackFrame(ByteBuffer _bb, FusionTrackFrame obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public FusionTrackFrame __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + public double timestamp() { int o = __offset(4); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } + public long serialNumber() { int o = __offset(6); return o != 0 ? bb.getLong(o + bb_pos) : 0L; } + public long hardwareTimestampUS() { int o = __offset(8); return o != 0 ? bb.getLong(o + bb_pos) : 0L; } + public long desynchroUS() { int o = __offset(10); return o != 0 ? bb.getLong(o + bb_pos) : 0L; } + public long counter() { int o = __offset(12); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } + public long format() { int o = __offset(14); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } + public long width() { int o = __offset(16); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } + public long height() { int o = __offset(18); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } + public int imageStrideInBytes() { int o = __offset(20); return o != 0 ? bb.getInt(o + bb_pos) : 0; } + public long imageHeaderVersion() { int o = __offset(22); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } + public int imageHeaderStatus() { int o = __offset(24); return o != 0 ? bb.getInt(o + bb_pos) : 0; } + public String imageLeftPixels() { int o = __offset(26); return o != 0 ? __string(o + bb_pos) : null; } + public ByteBuffer imageLeftPixelsAsByteBuffer() { return __vector_as_bytebuffer(26, 1); } + public ByteBuffer imageLeftPixelsInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 26, 1); } + public long imageLeftPixelsVersion() { int o = __offset(28); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } + public int imageLeftStatus() { int o = __offset(30); return o != 0 ? bb.getInt(o + bb_pos) : 0; } + public String imageRightPixels() { int o = __offset(32); return o != 0 ? __string(o + bb_pos) : null; } + public ByteBuffer imageRightPixelsAsByteBuffer() { return __vector_as_bytebuffer(32, 1); } + public ByteBuffer imageRightPixelsInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 32, 1); } + public long imageRightPixelsVersion() { int o = __offset(34); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } + public int imageRightStatus() { int o = __offset(36); return o != 0 ? bb.getInt(o + bb_pos) : 0; } + public ftkRegionOfInterest regionsOfInterestLeft(int j) { return regionsOfInterestLeft(new ftkRegionOfInterest(), j); } + public ftkRegionOfInterest regionsOfInterestLeft(ftkRegionOfInterest obj, int j) { int o = __offset(38); return o != 0 ? obj.__assign(__indirect(__vector(o) + j * 4), bb) : null; } + public int regionsOfInterestLeftLength() { int o = __offset(38); return o != 0 ? __vector_len(o) : 0; } + public long regionsOfInterestLeftVersion() { int o = __offset(40); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } + public int regionsOfInterestLeftStatus() { int o = __offset(42); return o != 0 ? bb.getInt(o + bb_pos) : 0; } + public ftkRegionOfInterest regionsOfInterestRight(int j) { return regionsOfInterestRight(new ftkRegionOfInterest(), j); } + public ftkRegionOfInterest regionsOfInterestRight(ftkRegionOfInterest obj, int j) { int o = __offset(44); return o != 0 ? obj.__assign(__indirect(__vector(o) + j * 4), bb) : null; } + public int regionsOfInterestRightLength() { int o = __offset(44); return o != 0 ? __vector_len(o) : 0; } + public long regionsOfInterestRightVersion() { int o = __offset(46); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } + public int regionsOfInterestRightStatus() { int o = __offset(48); return o != 0 ? bb.getInt(o + bb_pos) : 0; } + public ftk3DFiducial threeDFiducials(int j) { return threeDFiducials(new ftk3DFiducial(), j); } + public ftk3DFiducial threeDFiducials(ftk3DFiducial obj, int j) { int o = __offset(50); return o != 0 ? obj.__assign(__indirect(__vector(o) + j * 4), bb) : null; } + public int threeDFiducialsLength() { int o = __offset(50); return o != 0 ? __vector_len(o) : 0; } + public long threeDFiducialsVersion() { int o = __offset(52); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } + public int threeDFiducialsStatus() { int o = __offset(54); return o != 0 ? bb.getInt(o + bb_pos) : 0; } + public ftkMarker markers(int j) { return markers(new ftkMarker(), j); } + public ftkMarker markers(ftkMarker obj, int j) { int o = __offset(56); return o != 0 ? obj.__assign(__indirect(__vector(o) + j * 4), bb) : null; } + public int markersLength() { int o = __offset(56); return o != 0 ? __vector_len(o) : 0; } + public long markersVersion() { int o = __offset(58); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } + public int markersStatus() { int o = __offset(60); return o != 0 ? bb.getInt(o + bb_pos) : 0; } + public int deviceType() { int o = __offset(62); return o != 0 ? bb.getInt(o + bb_pos) : 0; } + public long ftkError() { int o = __offset(64); return o != 0 ? bb.getLong(o + bb_pos) : 0L; } + + public static int createFusionTrackFrame(FlatBufferBuilder builder, + double timestamp, + long serialNumber, + long hardwareTimestampUS, + long desynchroUS, + long counter, + long format, + long width, + long height, + int imageStrideInBytes, + long imageHeaderVersion, + int imageHeaderStatus, + int imageLeftPixelsOffset, + long imageLeftPixelsVersion, + int imageLeftStatus, + int imageRightPixelsOffset, + long imageRightPixelsVersion, + int imageRightStatus, + int regionsOfInterestLeftOffset, + long regionsOfInterestLeftVersion, + int regionsOfInterestLeftStatus, + int regionsOfInterestRightOffset, + long regionsOfInterestRightVersion, + int regionsOfInterestRightStatus, + int threeDFiducialsOffset, + long threeDFiducialsVersion, + int threeDFiducialsStatus, + int markersOffset, + long markersVersion, + int markersStatus, + int deviceType, + long ftkError) { + builder.startObject(31); + FusionTrackFrame.addFtkError(builder, ftkError); + FusionTrackFrame.addDesynchroUS(builder, desynchroUS); + FusionTrackFrame.addHardwareTimestampUS(builder, hardwareTimestampUS); + FusionTrackFrame.addSerialNumber(builder, serialNumber); + FusionTrackFrame.addTimestamp(builder, timestamp); + FusionTrackFrame.addDeviceType(builder, deviceType); + FusionTrackFrame.addMarkersStatus(builder, markersStatus); + FusionTrackFrame.addMarkersVersion(builder, markersVersion); + FusionTrackFrame.addMarkers(builder, markersOffset); + FusionTrackFrame.addThreeDFiducialsStatus(builder, threeDFiducialsStatus); + FusionTrackFrame.addThreeDFiducialsVersion(builder, threeDFiducialsVersion); + FusionTrackFrame.addThreeDFiducials(builder, threeDFiducialsOffset); + FusionTrackFrame.addRegionsOfInterestRightStatus(builder, regionsOfInterestRightStatus); + FusionTrackFrame.addRegionsOfInterestRightVersion(builder, regionsOfInterestRightVersion); + FusionTrackFrame.addRegionsOfInterestRight(builder, regionsOfInterestRightOffset); + FusionTrackFrame.addRegionsOfInterestLeftStatus(builder, regionsOfInterestLeftStatus); + FusionTrackFrame.addRegionsOfInterestLeftVersion(builder, regionsOfInterestLeftVersion); + FusionTrackFrame.addRegionsOfInterestLeft(builder, regionsOfInterestLeftOffset); + FusionTrackFrame.addImageRightStatus(builder, imageRightStatus); + FusionTrackFrame.addImageRightPixelsVersion(builder, imageRightPixelsVersion); + FusionTrackFrame.addImageRightPixels(builder, imageRightPixelsOffset); + FusionTrackFrame.addImageLeftStatus(builder, imageLeftStatus); + FusionTrackFrame.addImageLeftPixelsVersion(builder, imageLeftPixelsVersion); + FusionTrackFrame.addImageLeftPixels(builder, imageLeftPixelsOffset); + FusionTrackFrame.addImageHeaderStatus(builder, imageHeaderStatus); + FusionTrackFrame.addImageHeaderVersion(builder, imageHeaderVersion); + FusionTrackFrame.addImageStrideInBytes(builder, imageStrideInBytes); + FusionTrackFrame.addHeight(builder, height); + FusionTrackFrame.addWidth(builder, width); + FusionTrackFrame.addFormat(builder, format); + FusionTrackFrame.addCounter(builder, counter); + return FusionTrackFrame.endFusionTrackFrame(builder); + } + + public static void startFusionTrackFrame(FlatBufferBuilder builder) { builder.startObject(31); } + public static void addTimestamp(FlatBufferBuilder builder, double timestamp) { builder.addDouble(0, timestamp, 0.0); } + public static void addSerialNumber(FlatBufferBuilder builder, long serialNumber) { builder.addLong(1, serialNumber, 0L); } + public static void addHardwareTimestampUS(FlatBufferBuilder builder, long hardwareTimestampUS) { builder.addLong(2, hardwareTimestampUS, 0L); } + public static void addDesynchroUS(FlatBufferBuilder builder, long desynchroUS) { builder.addLong(3, desynchroUS, 0L); } + public static void addCounter(FlatBufferBuilder builder, long counter) { builder.addInt(4, (int)counter, (int)0L); } + public static void addFormat(FlatBufferBuilder builder, long format) { builder.addInt(5, (int)format, (int)0L); } + public static void addWidth(FlatBufferBuilder builder, long width) { builder.addInt(6, (int)width, (int)0L); } + public static void addHeight(FlatBufferBuilder builder, long height) { builder.addInt(7, (int)height, (int)0L); } + public static void addImageStrideInBytes(FlatBufferBuilder builder, int imageStrideInBytes) { builder.addInt(8, imageStrideInBytes, 0); } + public static void addImageHeaderVersion(FlatBufferBuilder builder, long imageHeaderVersion) { builder.addInt(9, (int)imageHeaderVersion, (int)0L); } + public static void addImageHeaderStatus(FlatBufferBuilder builder, int imageHeaderStatus) { builder.addInt(10, imageHeaderStatus, 0); } + public static void addImageLeftPixels(FlatBufferBuilder builder, int imageLeftPixelsOffset) { builder.addOffset(11, imageLeftPixelsOffset, 0); } + public static void addImageLeftPixelsVersion(FlatBufferBuilder builder, long imageLeftPixelsVersion) { builder.addInt(12, (int)imageLeftPixelsVersion, (int)0L); } + public static void addImageLeftStatus(FlatBufferBuilder builder, int imageLeftStatus) { builder.addInt(13, imageLeftStatus, 0); } + public static void addImageRightPixels(FlatBufferBuilder builder, int imageRightPixelsOffset) { builder.addOffset(14, imageRightPixelsOffset, 0); } + public static void addImageRightPixelsVersion(FlatBufferBuilder builder, long imageRightPixelsVersion) { builder.addInt(15, (int)imageRightPixelsVersion, (int)0L); } + public static void addImageRightStatus(FlatBufferBuilder builder, int imageRightStatus) { builder.addInt(16, imageRightStatus, 0); } + public static void addRegionsOfInterestLeft(FlatBufferBuilder builder, int regionsOfInterestLeftOffset) { builder.addOffset(17, regionsOfInterestLeftOffset, 0); } + public static int createRegionsOfInterestLeftVector(FlatBufferBuilder builder, int[] data) { builder.startVector(4, data.length, 4); for (int i = data.length - 1; i >= 0; i--) builder.addOffset(data[i]); return builder.endVector(); } + public static void startRegionsOfInterestLeftVector(FlatBufferBuilder builder, int numElems) { builder.startVector(4, numElems, 4); } + public static void addRegionsOfInterestLeftVersion(FlatBufferBuilder builder, long regionsOfInterestLeftVersion) { builder.addInt(18, (int)regionsOfInterestLeftVersion, (int)0L); } + public static void addRegionsOfInterestLeftStatus(FlatBufferBuilder builder, int regionsOfInterestLeftStatus) { builder.addInt(19, regionsOfInterestLeftStatus, 0); } + public static void addRegionsOfInterestRight(FlatBufferBuilder builder, int regionsOfInterestRightOffset) { builder.addOffset(20, regionsOfInterestRightOffset, 0); } + public static int createRegionsOfInterestRightVector(FlatBufferBuilder builder, int[] data) { builder.startVector(4, data.length, 4); for (int i = data.length - 1; i >= 0; i--) builder.addOffset(data[i]); return builder.endVector(); } + public static void startRegionsOfInterestRightVector(FlatBufferBuilder builder, int numElems) { builder.startVector(4, numElems, 4); } + public static void addRegionsOfInterestRightVersion(FlatBufferBuilder builder, long regionsOfInterestRightVersion) { builder.addInt(21, (int)regionsOfInterestRightVersion, (int)0L); } + public static void addRegionsOfInterestRightStatus(FlatBufferBuilder builder, int regionsOfInterestRightStatus) { builder.addInt(22, regionsOfInterestRightStatus, 0); } + public static void addThreeDFiducials(FlatBufferBuilder builder, int threeDFiducialsOffset) { builder.addOffset(23, threeDFiducialsOffset, 0); } + public static int createThreeDFiducialsVector(FlatBufferBuilder builder, int[] data) { builder.startVector(4, data.length, 4); for (int i = data.length - 1; i >= 0; i--) builder.addOffset(data[i]); return builder.endVector(); } + public static void startThreeDFiducialsVector(FlatBufferBuilder builder, int numElems) { builder.startVector(4, numElems, 4); } + public static void addThreeDFiducialsVersion(FlatBufferBuilder builder, long threeDFiducialsVersion) { builder.addInt(24, (int)threeDFiducialsVersion, (int)0L); } + public static void addThreeDFiducialsStatus(FlatBufferBuilder builder, int threeDFiducialsStatus) { builder.addInt(25, threeDFiducialsStatus, 0); } + public static void addMarkers(FlatBufferBuilder builder, int markersOffset) { builder.addOffset(26, markersOffset, 0); } + public static int createMarkersVector(FlatBufferBuilder builder, int[] data) { builder.startVector(4, data.length, 4); for (int i = data.length - 1; i >= 0; i--) builder.addOffset(data[i]); return builder.endVector(); } + public static void startMarkersVector(FlatBufferBuilder builder, int numElems) { builder.startVector(4, numElems, 4); } + public static void addMarkersVersion(FlatBufferBuilder builder, long markersVersion) { builder.addInt(27, (int)markersVersion, (int)0L); } + public static void addMarkersStatus(FlatBufferBuilder builder, int markersStatus) { builder.addInt(28, markersStatus, 0); } + public static void addDeviceType(FlatBufferBuilder builder, int deviceType) { builder.addInt(29, deviceType, 0); } + public static void addFtkError(FlatBufferBuilder builder, long ftkError) { builder.addLong(30, ftkError, 0L); } + public static int endFusionTrackFrame(FlatBufferBuilder builder) { + int o = builder.endObject(); + return o; + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/FusionTrackFrame.py b/include/grl/flatbuffer/grl/flatbuffer/FusionTrackFrame.py new file mode 100644 index 0000000..9150bfd --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/FusionTrackFrame.py @@ -0,0 +1,330 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +# /// Data for one frame capture +# /// On the Atracsys FusionTrack optical tracker +# /// look at ftkInterface.h for details +# /// Frame class is defined in FusionTrack.hpp +class FusionTrackFrame(object): + __slots__ = ['_tab'] + + @classmethod + def GetRootAsFusionTrackFrame(cls, buf, offset): + n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) + x = FusionTrackFrame() + x.Init(buf, n + offset) + return x + + # FusionTrackFrame + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + + # FusionTrackFrame + def Timestamp(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) + return 0.0 + + # FusionTrackFrame + def SerialNumber(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Uint64Flags, o + self._tab.Pos) + return 0 + + # FusionTrackFrame + def HardwareTimestampUS(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Uint64Flags, o + self._tab.Pos) + return 0 + + # FusionTrackFrame + def DesynchroUS(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Uint64Flags, o + self._tab.Pos) + return 0 + + # FusionTrackFrame + def Counter(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(12)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) + return 0 + + # FusionTrackFrame + def Format(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(14)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) + return 0 + + # FusionTrackFrame + def Width(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(16)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) + return 0 + + # FusionTrackFrame + def Height(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(18)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) + return 0 + + # FusionTrackFrame + def ImageStrideInBytes(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(20)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Int32Flags, o + self._tab.Pos) + return 0 + + # FusionTrackFrame + def ImageHeaderVersion(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(22)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) + return 0 + + # FusionTrackFrame + def ImageHeaderStatus(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(24)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Int32Flags, o + self._tab.Pos) + return 0 + + # FusionTrackFrame + def ImageLeftPixels(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(26)) + if o != 0: + return self._tab.String(o + self._tab.Pos) + return bytes() + + # FusionTrackFrame + def ImageLeftPixelsVersion(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(28)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) + return 0 + + # FusionTrackFrame + def ImageLeftStatus(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(30)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Int32Flags, o + self._tab.Pos) + return 0 + + # FusionTrackFrame + def ImageRightPixels(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(32)) + if o != 0: + return self._tab.String(o + self._tab.Pos) + return bytes() + + # FusionTrackFrame + def ImageRightPixelsVersion(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(34)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) + return 0 + + # FusionTrackFrame + def ImageRightStatus(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(36)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Int32Flags, o + self._tab.Pos) + return 0 + + # FusionTrackFrame + def RegionsOfInterestLeft(self, j): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(38)) + if o != 0: + x = self._tab.Vector(o) + x += flatbuffers.number_types.UOffsetTFlags.py_type(j) * 4 + x = self._tab.Indirect(x) + from .ftkRegionOfInterest import ftkRegionOfInterest + obj = ftkRegionOfInterest() + obj.Init(self._tab.Bytes, x) + return obj + return None + + # FusionTrackFrame + def RegionsOfInterestLeftLength(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(38)) + if o != 0: + return self._tab.VectorLen(o) + return 0 + + # FusionTrackFrame + def RegionsOfInterestLeftVersion(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(40)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) + return 0 + + # FusionTrackFrame + def RegionsOfInterestLeftStatus(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(42)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Int32Flags, o + self._tab.Pos) + return 0 + + # FusionTrackFrame + def RegionsOfInterestRight(self, j): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(44)) + if o != 0: + x = self._tab.Vector(o) + x += flatbuffers.number_types.UOffsetTFlags.py_type(j) * 4 + x = self._tab.Indirect(x) + from .ftkRegionOfInterest import ftkRegionOfInterest + obj = ftkRegionOfInterest() + obj.Init(self._tab.Bytes, x) + return obj + return None + + # FusionTrackFrame + def RegionsOfInterestRightLength(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(44)) + if o != 0: + return self._tab.VectorLen(o) + return 0 + + # FusionTrackFrame + def RegionsOfInterestRightVersion(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(46)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) + return 0 + + # FusionTrackFrame + def RegionsOfInterestRightStatus(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(48)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Int32Flags, o + self._tab.Pos) + return 0 + + # FusionTrackFrame + def ThreeDFiducials(self, j): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(50)) + if o != 0: + x = self._tab.Vector(o) + x += flatbuffers.number_types.UOffsetTFlags.py_type(j) * 4 + x = self._tab.Indirect(x) + from .ftk3DFiducial import ftk3DFiducial + obj = ftk3DFiducial() + obj.Init(self._tab.Bytes, x) + return obj + return None + + # FusionTrackFrame + def ThreeDFiducialsLength(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(50)) + if o != 0: + return self._tab.VectorLen(o) + return 0 + + # FusionTrackFrame + def ThreeDFiducialsVersion(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(52)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) + return 0 + + # FusionTrackFrame + def ThreeDFiducialsStatus(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(54)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Int32Flags, o + self._tab.Pos) + return 0 + + # FusionTrackFrame + def Markers(self, j): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(56)) + if o != 0: + x = self._tab.Vector(o) + x += flatbuffers.number_types.UOffsetTFlags.py_type(j) * 4 + x = self._tab.Indirect(x) + from .ftkMarker import ftkMarker + obj = ftkMarker() + obj.Init(self._tab.Bytes, x) + return obj + return None + + # FusionTrackFrame + def MarkersLength(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(56)) + if o != 0: + return self._tab.VectorLen(o) + return 0 + + # FusionTrackFrame + def MarkersVersion(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(58)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) + return 0 + + # FusionTrackFrame + def MarkersStatus(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(60)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Int32Flags, o + self._tab.Pos) + return 0 + + # FusionTrackFrame + def DeviceType(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(62)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Int32Flags, o + self._tab.Pos) + return 0 + + # FusionTrackFrame + def FtkError(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(64)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Int64Flags, o + self._tab.Pos) + return 0 + +def FusionTrackFrameStart(builder): builder.StartObject(31) +def FusionTrackFrameAddTimestamp(builder, timestamp): builder.PrependFloat64Slot(0, timestamp, 0.0) +def FusionTrackFrameAddSerialNumber(builder, serialNumber): builder.PrependUint64Slot(1, serialNumber, 0) +def FusionTrackFrameAddHardwareTimestampUS(builder, hardwareTimestampUS): builder.PrependUint64Slot(2, hardwareTimestampUS, 0) +def FusionTrackFrameAddDesynchroUS(builder, desynchroUS): builder.PrependUint64Slot(3, desynchroUS, 0) +def FusionTrackFrameAddCounter(builder, counter): builder.PrependUint32Slot(4, counter, 0) +def FusionTrackFrameAddFormat(builder, format): builder.PrependUint32Slot(5, format, 0) +def FusionTrackFrameAddWidth(builder, width): builder.PrependUint32Slot(6, width, 0) +def FusionTrackFrameAddHeight(builder, height): builder.PrependUint32Slot(7, height, 0) +def FusionTrackFrameAddImageStrideInBytes(builder, imageStrideInBytes): builder.PrependInt32Slot(8, imageStrideInBytes, 0) +def FusionTrackFrameAddImageHeaderVersion(builder, imageHeaderVersion): builder.PrependUint32Slot(9, imageHeaderVersion, 0) +def FusionTrackFrameAddImageHeaderStatus(builder, imageHeaderStatus): builder.PrependInt32Slot(10, imageHeaderStatus, 0) +def FusionTrackFrameAddImageLeftPixels(builder, imageLeftPixels): builder.PrependUOffsetTRelativeSlot(11, flatbuffers.number_types.UOffsetTFlags.py_type(imageLeftPixels), 0) +def FusionTrackFrameAddImageLeftPixelsVersion(builder, imageLeftPixelsVersion): builder.PrependUint32Slot(12, imageLeftPixelsVersion, 0) +def FusionTrackFrameAddImageLeftStatus(builder, imageLeftStatus): builder.PrependInt32Slot(13, imageLeftStatus, 0) +def FusionTrackFrameAddImageRightPixels(builder, imageRightPixels): builder.PrependUOffsetTRelativeSlot(14, flatbuffers.number_types.UOffsetTFlags.py_type(imageRightPixels), 0) +def FusionTrackFrameAddImageRightPixelsVersion(builder, imageRightPixelsVersion): builder.PrependUint32Slot(15, imageRightPixelsVersion, 0) +def FusionTrackFrameAddImageRightStatus(builder, imageRightStatus): builder.PrependInt32Slot(16, imageRightStatus, 0) +def FusionTrackFrameAddRegionsOfInterestLeft(builder, regionsOfInterestLeft): builder.PrependUOffsetTRelativeSlot(17, flatbuffers.number_types.UOffsetTFlags.py_type(regionsOfInterestLeft), 0) +def FusionTrackFrameStartRegionsOfInterestLeftVector(builder, numElems): return builder.StartVector(4, numElems, 4) +def FusionTrackFrameAddRegionsOfInterestLeftVersion(builder, regionsOfInterestLeftVersion): builder.PrependUint32Slot(18, regionsOfInterestLeftVersion, 0) +def FusionTrackFrameAddRegionsOfInterestLeftStatus(builder, regionsOfInterestLeftStatus): builder.PrependInt32Slot(19, regionsOfInterestLeftStatus, 0) +def FusionTrackFrameAddRegionsOfInterestRight(builder, regionsOfInterestRight): builder.PrependUOffsetTRelativeSlot(20, flatbuffers.number_types.UOffsetTFlags.py_type(regionsOfInterestRight), 0) +def FusionTrackFrameStartRegionsOfInterestRightVector(builder, numElems): return builder.StartVector(4, numElems, 4) +def FusionTrackFrameAddRegionsOfInterestRightVersion(builder, regionsOfInterestRightVersion): builder.PrependUint32Slot(21, regionsOfInterestRightVersion, 0) +def FusionTrackFrameAddRegionsOfInterestRightStatus(builder, regionsOfInterestRightStatus): builder.PrependInt32Slot(22, regionsOfInterestRightStatus, 0) +def FusionTrackFrameAddThreeDFiducials(builder, threeDFiducials): builder.PrependUOffsetTRelativeSlot(23, flatbuffers.number_types.UOffsetTFlags.py_type(threeDFiducials), 0) +def FusionTrackFrameStartThreeDFiducialsVector(builder, numElems): return builder.StartVector(4, numElems, 4) +def FusionTrackFrameAddThreeDFiducialsVersion(builder, threeDFiducialsVersion): builder.PrependUint32Slot(24, threeDFiducialsVersion, 0) +def FusionTrackFrameAddThreeDFiducialsStatus(builder, threeDFiducialsStatus): builder.PrependInt32Slot(25, threeDFiducialsStatus, 0) +def FusionTrackFrameAddMarkers(builder, markers): builder.PrependUOffsetTRelativeSlot(26, flatbuffers.number_types.UOffsetTFlags.py_type(markers), 0) +def FusionTrackFrameStartMarkersVector(builder, numElems): return builder.StartVector(4, numElems, 4) +def FusionTrackFrameAddMarkersVersion(builder, markersVersion): builder.PrependUint32Slot(27, markersVersion, 0) +def FusionTrackFrameAddMarkersStatus(builder, markersStatus): builder.PrependInt32Slot(28, markersStatus, 0) +def FusionTrackFrameAddDeviceType(builder, deviceType): builder.PrependInt32Slot(29, deviceType, 0) +def FusionTrackFrameAddFtkError(builder, ftkError): builder.PrependInt64Slot(30, ftkError, 0) +def FusionTrackFrameEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/FusionTrackMessage.java b/include/grl/flatbuffer/grl/flatbuffer/FusionTrackMessage.java new file mode 100644 index 0000000..1fa352d --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/FusionTrackMessage.java @@ -0,0 +1,48 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class FusionTrackMessage extends Table { + public static FusionTrackMessage getRootAsFusionTrackMessage(ByteBuffer _bb) { return getRootAsFusionTrackMessage(_bb, new FusionTrackMessage()); } + public static FusionTrackMessage getRootAsFusionTrackMessage(ByteBuffer _bb, FusionTrackMessage obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public FusionTrackMessage __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + public double timestamp() { int o = __offset(4); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } + public FusionTrackParameters parameters() { return parameters(new FusionTrackParameters()); } + public FusionTrackParameters parameters(FusionTrackParameters obj) { int o = __offset(6); return o != 0 ? obj.__assign(__indirect(o + bb_pos), bb) : null; } + public TimeEvent timeEvent() { return timeEvent(new TimeEvent()); } + public TimeEvent timeEvent(TimeEvent obj) { int o = __offset(8); return o != 0 ? obj.__assign(__indirect(o + bb_pos), bb) : null; } + public FusionTrackFrame frame() { return frame(new FusionTrackFrame()); } + public FusionTrackFrame frame(FusionTrackFrame obj) { int o = __offset(10); return o != 0 ? obj.__assign(__indirect(o + bb_pos), bb) : null; } + + public static int createFusionTrackMessage(FlatBufferBuilder builder, + double timestamp, + int parametersOffset, + int timeEventOffset, + int frameOffset) { + builder.startObject(4); + FusionTrackMessage.addTimestamp(builder, timestamp); + FusionTrackMessage.addFrame(builder, frameOffset); + FusionTrackMessage.addTimeEvent(builder, timeEventOffset); + FusionTrackMessage.addParameters(builder, parametersOffset); + return FusionTrackMessage.endFusionTrackMessage(builder); + } + + public static void startFusionTrackMessage(FlatBufferBuilder builder) { builder.startObject(4); } + public static void addTimestamp(FlatBufferBuilder builder, double timestamp) { builder.addDouble(0, timestamp, 0.0); } + public static void addParameters(FlatBufferBuilder builder, int parametersOffset) { builder.addOffset(1, parametersOffset, 0); } + public static void addTimeEvent(FlatBufferBuilder builder, int timeEventOffset) { builder.addOffset(2, timeEventOffset, 0); } + public static void addFrame(FlatBufferBuilder builder, int frameOffset) { builder.addOffset(3, frameOffset, 0); } + public static int endFusionTrackMessage(FlatBufferBuilder builder) { + int o = builder.endObject(); + return o; + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/FusionTrackMessage.py b/include/grl/flatbuffer/grl/flatbuffer/FusionTrackMessage.py new file mode 100644 index 0000000..2b085c2 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/FusionTrackMessage.py @@ -0,0 +1,66 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class FusionTrackMessage(object): + __slots__ = ['_tab'] + + @classmethod + def GetRootAsFusionTrackMessage(cls, buf, offset): + n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) + x = FusionTrackMessage() + x.Init(buf, n + offset) + return x + + # FusionTrackMessage + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + + # FusionTrackMessage + def Timestamp(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) + return 0.0 + + # FusionTrackMessage + def Parameters(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) + if o != 0: + x = self._tab.Indirect(o + self._tab.Pos) + from .FusionTrackParameters import FusionTrackParameters + obj = FusionTrackParameters() + obj.Init(self._tab.Bytes, x) + return obj + return None + + # FusionTrackMessage + def TimeEvent(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) + if o != 0: + x = self._tab.Indirect(o + self._tab.Pos) + from .TimeEvent import TimeEvent + obj = TimeEvent() + obj.Init(self._tab.Bytes, x) + return obj + return None + + # FusionTrackMessage + def Frame(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) + if o != 0: + x = self._tab.Indirect(o + self._tab.Pos) + from .FusionTrackFrame import FusionTrackFrame + obj = FusionTrackFrame() + obj.Init(self._tab.Bytes, x) + return obj + return None + +def FusionTrackMessageStart(builder): builder.StartObject(4) +def FusionTrackMessageAddTimestamp(builder, timestamp): builder.PrependFloat64Slot(0, timestamp, 0.0) +def FusionTrackMessageAddParameters(builder, parameters): builder.PrependUOffsetTRelativeSlot(1, flatbuffers.number_types.UOffsetTFlags.py_type(parameters), 0) +def FusionTrackMessageAddTimeEvent(builder, timeEvent): builder.PrependUOffsetTRelativeSlot(2, flatbuffers.number_types.UOffsetTFlags.py_type(timeEvent), 0) +def FusionTrackMessageAddFrame(builder, frame): builder.PrependUOffsetTRelativeSlot(3, flatbuffers.number_types.UOffsetTFlags.py_type(frame), 0) +def FusionTrackMessageEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/FusionTrackParameters.java b/include/grl/flatbuffer/grl/flatbuffer/FusionTrackParameters.java new file mode 100644 index 0000000..c50156d --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/FusionTrackParameters.java @@ -0,0 +1,145 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class FusionTrackParameters extends Table { + public static FusionTrackParameters getRootAsFusionTrackParameters(ByteBuffer _bb) { return getRootAsFusionTrackParameters(_bb, new FusionTrackParameters()); } + public static FusionTrackParameters getRootAsFusionTrackParameters(ByteBuffer _bb, FusionTrackParameters obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public FusionTrackParameters __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + /** + * Name for this connection / FusionTrack driver instance + * useful for debugging and when multiple data sources are used + */ + public String name() { int o = __offset(4); return o != 0 ? __string(o + bb_pos) : null; } + public ByteBuffer nameAsByteBuffer() { return __vector_as_bytebuffer(4, 1); } + public ByteBuffer nameInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 4, 1); } + /** + * Name for the clock on the FusionTrack + * Useful for timing calculations and debugging. + */ + public String deviceClockID() { int o = __offset(6); return o != 0 ? __string(o + bb_pos) : null; } + public ByteBuffer deviceClockIDAsByteBuffer() { return __vector_as_bytebuffer(6, 1); } + public ByteBuffer deviceClockIDInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 6, 1); } + /** + * Name for the local clock on which this driver runs + * Useful for timing calculations and debugging. + */ + public String localClockID() { int o = __offset(8); return o != 0 ? __string(o + bb_pos) : null; } + public ByteBuffer localClockIDAsByteBuffer() { return __vector_as_bytebuffer(8, 1); } + public ByteBuffer localClockIDInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 8, 1); } + /** + * dimensions of the markers that may be present + */ + public ftkGeometry geometries(int j) { return geometries(new ftkGeometry(), j); } + public ftkGeometry geometries(ftkGeometry obj, int j) { int o = __offset(10); return o != 0 ? obj.__assign(__indirect(__vector(o) + j * 4), bb) : null; } + public int geometriesLength() { int o = __offset(10); return o != 0 ? __vector_len(o) : 0; } + /** + * Geometries aka fiducials aka markers to be loaded from ini files. + * The data loaded should not repeat IDs from MarkerIDs. + */ + public String geometryFilenames(int j) { int o = __offset(12); return o != 0 ? __string(__vector(o) + j * 4) : null; } + public int geometryFilenamesLength() { int o = __offset(12); return o != 0 ? __vector_len(o) : 0; } + /** + * Path to the directory with the marker ini files listed above + * Uses the default current working directory if empty + * geometryDir:[string]; + */ + public String geometryDir() { int o = __offset(14); return o != 0 ? __string(o + bb_pos) : null; } + public ByteBuffer geometryDirAsByteBuffer() { return __vector_as_bytebuffer(14, 1); } + public ByteBuffer geometryDirInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 14, 1); } + /** + * Optional list of optical tracker device ids to expect + * will be loaded automatically if empty + */ + public long TrackerDeviceIDs(int j) { int o = __offset(16); return o != 0 ? bb.getLong(__vector(o) + j * 8) : 0; } + public int TrackerDeviceIDsLength() { int o = __offset(16); return o != 0 ? __vector_len(o) : 0; } + public ByteBuffer TrackerDeviceIDsAsByteBuffer() { return __vector_as_bytebuffer(16, 8); } + public ByteBuffer TrackerDeviceIDsInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 16, 8); } + /** + * Marker geometry unique integer IDs + */ + public long markerIDs(int j) { int o = __offset(18); return o != 0 ? bb.getLong(__vector(o) + j * 8) : 0; } + public int markerIDsLength() { int o = __offset(18); return o != 0 ? __vector_len(o) : 0; } + public ByteBuffer markerIDsAsByteBuffer() { return __vector_as_bytebuffer(18, 8); } + public ByteBuffer markerIDsInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 18, 8); } + /** + * Optional Marker geometry names with one for each ID, none otherwise + */ + public String markerNames(int j) { int o = __offset(20); return o != 0 ? __string(__vector(o) + j * 4) : null; } + public int markerNamesLength() { int o = __offset(20); return o != 0 ? __vector_len(o) : 0; } + public long mDeviceSerialNumbers(int j) { int o = __offset(22); return o != 0 ? bb.getLong(__vector(o) + j * 8) : 0; } + public int mDeviceSerialNumbersLength() { int o = __offset(22); return o != 0 ? __vector_len(o) : 0; } + public ByteBuffer mDeviceSerialNumbersAsByteBuffer() { return __vector_as_bytebuffer(22, 8); } + public ByteBuffer mDeviceSerialNumbersInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 22, 8); } + public int mDeviceTypes(int j) { int o = __offset(24); return o != 0 ? bb.get(__vector(o) + j * 1) & 0xFF : 0; } + public int mDeviceTypesLength() { int o = __offset(24); return o != 0 ? __vector_len(o) : 0; } + public ByteBuffer mDeviceTypesAsByteBuffer() { return __vector_as_bytebuffer(24, 1); } + public ByteBuffer mDeviceTypesInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 24, 1); } + + public static int createFusionTrackParameters(FlatBufferBuilder builder, + int nameOffset, + int deviceClockIDOffset, + int localClockIDOffset, + int geometriesOffset, + int geometryFilenamesOffset, + int geometryDirOffset, + int TrackerDeviceIDsOffset, + int markerIDsOffset, + int markerNamesOffset, + int m_deviceSerialNumbersOffset, + int m_device_typesOffset) { + builder.startObject(11); + FusionTrackParameters.addMDeviceTypes(builder, m_device_typesOffset); + FusionTrackParameters.addMDeviceSerialNumbers(builder, m_deviceSerialNumbersOffset); + FusionTrackParameters.addMarkerNames(builder, markerNamesOffset); + FusionTrackParameters.addMarkerIDs(builder, markerIDsOffset); + FusionTrackParameters.addTrackerDeviceIDs(builder, TrackerDeviceIDsOffset); + FusionTrackParameters.addGeometryDir(builder, geometryDirOffset); + FusionTrackParameters.addGeometryFilenames(builder, geometryFilenamesOffset); + FusionTrackParameters.addGeometries(builder, geometriesOffset); + FusionTrackParameters.addLocalClockID(builder, localClockIDOffset); + FusionTrackParameters.addDeviceClockID(builder, deviceClockIDOffset); + FusionTrackParameters.addName(builder, nameOffset); + return FusionTrackParameters.endFusionTrackParameters(builder); + } + + public static void startFusionTrackParameters(FlatBufferBuilder builder) { builder.startObject(11); } + public static void addName(FlatBufferBuilder builder, int nameOffset) { builder.addOffset(0, nameOffset, 0); } + public static void addDeviceClockID(FlatBufferBuilder builder, int deviceClockIDOffset) { builder.addOffset(1, deviceClockIDOffset, 0); } + public static void addLocalClockID(FlatBufferBuilder builder, int localClockIDOffset) { builder.addOffset(2, localClockIDOffset, 0); } + public static void addGeometries(FlatBufferBuilder builder, int geometriesOffset) { builder.addOffset(3, geometriesOffset, 0); } + public static int createGeometriesVector(FlatBufferBuilder builder, int[] data) { builder.startVector(4, data.length, 4); for (int i = data.length - 1; i >= 0; i--) builder.addOffset(data[i]); return builder.endVector(); } + public static void startGeometriesVector(FlatBufferBuilder builder, int numElems) { builder.startVector(4, numElems, 4); } + public static void addGeometryFilenames(FlatBufferBuilder builder, int geometryFilenamesOffset) { builder.addOffset(4, geometryFilenamesOffset, 0); } + public static int createGeometryFilenamesVector(FlatBufferBuilder builder, int[] data) { builder.startVector(4, data.length, 4); for (int i = data.length - 1; i >= 0; i--) builder.addOffset(data[i]); return builder.endVector(); } + public static void startGeometryFilenamesVector(FlatBufferBuilder builder, int numElems) { builder.startVector(4, numElems, 4); } + public static void addGeometryDir(FlatBufferBuilder builder, int geometryDirOffset) { builder.addOffset(5, geometryDirOffset, 0); } + public static void addTrackerDeviceIDs(FlatBufferBuilder builder, int TrackerDeviceIDsOffset) { builder.addOffset(6, TrackerDeviceIDsOffset, 0); } + public static int createTrackerDeviceIDsVector(FlatBufferBuilder builder, long[] data) { builder.startVector(8, data.length, 8); for (int i = data.length - 1; i >= 0; i--) builder.addLong(data[i]); return builder.endVector(); } + public static void startTrackerDeviceIDsVector(FlatBufferBuilder builder, int numElems) { builder.startVector(8, numElems, 8); } + public static void addMarkerIDs(FlatBufferBuilder builder, int markerIDsOffset) { builder.addOffset(7, markerIDsOffset, 0); } + public static int createMarkerIDsVector(FlatBufferBuilder builder, long[] data) { builder.startVector(8, data.length, 8); for (int i = data.length - 1; i >= 0; i--) builder.addLong(data[i]); return builder.endVector(); } + public static void startMarkerIDsVector(FlatBufferBuilder builder, int numElems) { builder.startVector(8, numElems, 8); } + public static void addMarkerNames(FlatBufferBuilder builder, int markerNamesOffset) { builder.addOffset(8, markerNamesOffset, 0); } + public static int createMarkerNamesVector(FlatBufferBuilder builder, int[] data) { builder.startVector(4, data.length, 4); for (int i = data.length - 1; i >= 0; i--) builder.addOffset(data[i]); return builder.endVector(); } + public static void startMarkerNamesVector(FlatBufferBuilder builder, int numElems) { builder.startVector(4, numElems, 4); } + public static void addMDeviceSerialNumbers(FlatBufferBuilder builder, int mDeviceSerialNumbersOffset) { builder.addOffset(9, mDeviceSerialNumbersOffset, 0); } + public static int createMDeviceSerialNumbersVector(FlatBufferBuilder builder, long[] data) { builder.startVector(8, data.length, 8); for (int i = data.length - 1; i >= 0; i--) builder.addLong(data[i]); return builder.endVector(); } + public static void startMDeviceSerialNumbersVector(FlatBufferBuilder builder, int numElems) { builder.startVector(8, numElems, 8); } + public static void addMDeviceTypes(FlatBufferBuilder builder, int mDeviceTypesOffset) { builder.addOffset(10, mDeviceTypesOffset, 0); } + public static int createMDeviceTypesVector(FlatBufferBuilder builder, byte[] data) { builder.startVector(1, data.length, 1); for (int i = data.length - 1; i >= 0; i--) builder.addByte(data[i]); return builder.endVector(); } + public static void startMDeviceTypesVector(FlatBufferBuilder builder, int numElems) { builder.startVector(1, numElems, 1); } + public static int endFusionTrackParameters(FlatBufferBuilder builder) { + int o = builder.endObject(); + return o; + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/FusionTrackParameters.py b/include/grl/flatbuffer/grl/flatbuffer/FusionTrackParameters.py new file mode 100644 index 0000000..1354fe8 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/FusionTrackParameters.py @@ -0,0 +1,222 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class FusionTrackParameters(object): + __slots__ = ['_tab'] + + @classmethod + def GetRootAsFusionTrackParameters(cls, buf, offset): + n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) + x = FusionTrackParameters() + x.Init(buf, n + offset) + return x + + # FusionTrackParameters + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + +# /// Name for this connection / FusionTrack driver instance +# /// useful for debugging and when multiple data sources are used + # FusionTrackParameters + def Name(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + return self._tab.String(o + self._tab.Pos) + return bytes() + +# /// Name for the clock on the FusionTrack +# /// Useful for timing calculations and debugging. + # FusionTrackParameters + def DeviceClockID(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) + if o != 0: + return self._tab.String(o + self._tab.Pos) + return bytes() + +# /// Name for the local clock on which this driver runs +# /// Useful for timing calculations and debugging. + # FusionTrackParameters + def LocalClockID(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) + if o != 0: + return self._tab.String(o + self._tab.Pos) + return bytes() + +# /// dimensions of the markers that may be present + # FusionTrackParameters + def Geometries(self, j): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) + if o != 0: + x = self._tab.Vector(o) + x += flatbuffers.number_types.UOffsetTFlags.py_type(j) * 4 + x = self._tab.Indirect(x) + from .ftkGeometry import ftkGeometry + obj = ftkGeometry() + obj.Init(self._tab.Bytes, x) + return obj + return None + + # FusionTrackParameters + def GeometriesLength(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) + if o != 0: + return self._tab.VectorLen(o) + return 0 + +# /// Geometries aka fiducials aka markers to be loaded from ini files. +# /// The data loaded should not repeat IDs from MarkerIDs. + # FusionTrackParameters + def GeometryFilenames(self, j): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(12)) + if o != 0: + a = self._tab.Vector(o) + return self._tab.String(a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 4)) + return "" + + # FusionTrackParameters + def GeometryFilenamesLength(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(12)) + if o != 0: + return self._tab.VectorLen(o) + return 0 + +# /// Path to the directory with the marker ini files listed above +# /// Uses the default current working directory if empty +# /// geometryDir:[string]; + # FusionTrackParameters + def GeometryDir(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(14)) + if o != 0: + return self._tab.String(o + self._tab.Pos) + return bytes() + +# /// Optional list of optical tracker device ids to expect +# /// will be loaded automatically if empty + # FusionTrackParameters + def TrackerDeviceIDs(self, j): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(16)) + if o != 0: + a = self._tab.Vector(o) + return self._tab.Get(flatbuffers.number_types.Uint64Flags, a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 8)) + return 0 + + # FusionTrackParameters + def TrackerDeviceIDsAsNumpy(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(16)) + if o != 0: + return self._tab.GetVectorAsNumpy(flatbuffers.number_types.Uint64Flags, o) + return 0 + + # FusionTrackParameters + def TrackerDeviceIDsLength(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(16)) + if o != 0: + return self._tab.VectorLen(o) + return 0 + +# /// Marker geometry unique integer IDs + # FusionTrackParameters + def MarkerIDs(self, j): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(18)) + if o != 0: + a = self._tab.Vector(o) + return self._tab.Get(flatbuffers.number_types.Uint64Flags, a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 8)) + return 0 + + # FusionTrackParameters + def MarkerIDsAsNumpy(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(18)) + if o != 0: + return self._tab.GetVectorAsNumpy(flatbuffers.number_types.Uint64Flags, o) + return 0 + + # FusionTrackParameters + def MarkerIDsLength(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(18)) + if o != 0: + return self._tab.VectorLen(o) + return 0 + +# /// Optional Marker geometry names with one for each ID, none otherwise + # FusionTrackParameters + def MarkerNames(self, j): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(20)) + if o != 0: + a = self._tab.Vector(o) + return self._tab.String(a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 4)) + return "" + + # FusionTrackParameters + def MarkerNamesLength(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(20)) + if o != 0: + return self._tab.VectorLen(o) + return 0 + + # FusionTrackParameters + def MDeviceSerialNumbers(self, j): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(22)) + if o != 0: + a = self._tab.Vector(o) + return self._tab.Get(flatbuffers.number_types.Uint64Flags, a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 8)) + return 0 + + # FusionTrackParameters + def MDeviceSerialNumbersAsNumpy(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(22)) + if o != 0: + return self._tab.GetVectorAsNumpy(flatbuffers.number_types.Uint64Flags, o) + return 0 + + # FusionTrackParameters + def MDeviceSerialNumbersLength(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(22)) + if o != 0: + return self._tab.VectorLen(o) + return 0 + + # FusionTrackParameters + def MDeviceTypes(self, j): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(24)) + if o != 0: + a = self._tab.Vector(o) + return self._tab.Get(flatbuffers.number_types.Uint8Flags, a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 1)) + return 0 + + # FusionTrackParameters + def MDeviceTypesAsNumpy(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(24)) + if o != 0: + return self._tab.GetVectorAsNumpy(flatbuffers.number_types.Uint8Flags, o) + return 0 + + # FusionTrackParameters + def MDeviceTypesLength(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(24)) + if o != 0: + return self._tab.VectorLen(o) + return 0 + +def FusionTrackParametersStart(builder): builder.StartObject(11) +def FusionTrackParametersAddName(builder, name): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(name), 0) +def FusionTrackParametersAddDeviceClockID(builder, deviceClockID): builder.PrependUOffsetTRelativeSlot(1, flatbuffers.number_types.UOffsetTFlags.py_type(deviceClockID), 0) +def FusionTrackParametersAddLocalClockID(builder, localClockID): builder.PrependUOffsetTRelativeSlot(2, flatbuffers.number_types.UOffsetTFlags.py_type(localClockID), 0) +def FusionTrackParametersAddGeometries(builder, geometries): builder.PrependUOffsetTRelativeSlot(3, flatbuffers.number_types.UOffsetTFlags.py_type(geometries), 0) +def FusionTrackParametersStartGeometriesVector(builder, numElems): return builder.StartVector(4, numElems, 4) +def FusionTrackParametersAddGeometryFilenames(builder, geometryFilenames): builder.PrependUOffsetTRelativeSlot(4, flatbuffers.number_types.UOffsetTFlags.py_type(geometryFilenames), 0) +def FusionTrackParametersStartGeometryFilenamesVector(builder, numElems): return builder.StartVector(4, numElems, 4) +def FusionTrackParametersAddGeometryDir(builder, geometryDir): builder.PrependUOffsetTRelativeSlot(5, flatbuffers.number_types.UOffsetTFlags.py_type(geometryDir), 0) +def FusionTrackParametersAddTrackerDeviceIDs(builder, TrackerDeviceIDs): builder.PrependUOffsetTRelativeSlot(6, flatbuffers.number_types.UOffsetTFlags.py_type(TrackerDeviceIDs), 0) +def FusionTrackParametersStartTrackerDeviceIDsVector(builder, numElems): return builder.StartVector(8, numElems, 8) +def FusionTrackParametersAddMarkerIDs(builder, markerIDs): builder.PrependUOffsetTRelativeSlot(7, flatbuffers.number_types.UOffsetTFlags.py_type(markerIDs), 0) +def FusionTrackParametersStartMarkerIDsVector(builder, numElems): return builder.StartVector(8, numElems, 8) +def FusionTrackParametersAddMarkerNames(builder, markerNames): builder.PrependUOffsetTRelativeSlot(8, flatbuffers.number_types.UOffsetTFlags.py_type(markerNames), 0) +def FusionTrackParametersStartMarkerNamesVector(builder, numElems): return builder.StartVector(4, numElems, 4) +def FusionTrackParametersAddMDeviceSerialNumbers(builder, mDeviceSerialNumbers): builder.PrependUOffsetTRelativeSlot(9, flatbuffers.number_types.UOffsetTFlags.py_type(mDeviceSerialNumbers), 0) +def FusionTrackParametersStartMDeviceSerialNumbersVector(builder, numElems): return builder.StartVector(8, numElems, 8) +def FusionTrackParametersAddMDeviceTypes(builder, mDeviceTypes): builder.PrependUOffsetTRelativeSlot(10, flatbuffers.number_types.UOffsetTFlags.py_type(mDeviceTypes), 0) +def FusionTrackParametersStartMDeviceTypesVector(builder, numElems): return builder.StartVector(1, numElems, 1) +def FusionTrackParametersEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/Inertia.java b/include/grl/flatbuffer/grl/flatbuffer/Inertia.java new file mode 100644 index 0000000..7311a09 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/Inertia.java @@ -0,0 +1,47 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class Inertia extends Struct { + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public Inertia __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + public double mass() { return bb.getDouble(bb_pos + 0); } + public Pose pose() { return pose(new Pose()); } + public Pose pose(Pose obj) { return obj.__assign(bb_pos + 8, bb); } + public double ixx() { return bb.getDouble(bb_pos + 64); } + public double ixy() { return bb.getDouble(bb_pos + 72); } + public double ixz() { return bb.getDouble(bb_pos + 80); } + public double iyy() { return bb.getDouble(bb_pos + 88); } + public double iyz() { return bb.getDouble(bb_pos + 96); } + public double izz() { return bb.getDouble(bb_pos + 104); } + + public static int createInertia(FlatBufferBuilder builder, double mass, double pose_position_x, double pose_position_y, double pose_position_z, double pose_orientation_x, double pose_orientation_y, double pose_orientation_z, double pose_orientation_w, double ixx, double ixy, double ixz, double iyy, double iyz, double izz) { + builder.prep(8, 112); + builder.putDouble(izz); + builder.putDouble(iyz); + builder.putDouble(iyy); + builder.putDouble(ixz); + builder.putDouble(ixy); + builder.putDouble(ixx); + builder.prep(8, 56); + builder.prep(8, 32); + builder.putDouble(pose_orientation_w); + builder.putDouble(pose_orientation_z); + builder.putDouble(pose_orientation_y); + builder.putDouble(pose_orientation_x); + builder.prep(8, 24); + builder.putDouble(pose_position_z); + builder.putDouble(pose_position_y); + builder.putDouble(pose_position_x); + builder.putDouble(mass); + return builder.offset(); + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/Inertia.py b/include/grl/flatbuffer/grl/flatbuffer/Inertia.py new file mode 100644 index 0000000..9b0121a --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/Inertia.py @@ -0,0 +1,53 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class Inertia(object): + __slots__ = ['_tab'] + + # Inertia + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + + # Inertia + def Mass(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(0)) + # Inertia + def Pose(self, obj): + obj.Init(self._tab.Bytes, self._tab.Pos + 8) + return obj + + # Inertia + def Ixx(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(64)) + # Inertia + def Ixy(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(72)) + # Inertia + def Ixz(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(80)) + # Inertia + def Iyy(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(88)) + # Inertia + def Iyz(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(96)) + # Inertia + def Izz(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(104)) + +def CreateInertia(builder, mass, pose_position_x, pose_position_y, pose_position_z, pose_orientation_x, pose_orientation_y, pose_orientation_z, pose_orientation_w, ixx, ixy, ixz, iyy, iyz, izz): + builder.Prep(8, 112) + builder.PrependFloat64(izz) + builder.PrependFloat64(iyz) + builder.PrependFloat64(iyy) + builder.PrependFloat64(ixz) + builder.PrependFloat64(ixy) + builder.PrependFloat64(ixx) + builder.Prep(8, 56) + builder.Prep(8, 32) + builder.PrependFloat64(pose_orientation_w) + builder.PrependFloat64(pose_orientation_z) + builder.PrependFloat64(pose_orientation_y) + builder.PrependFloat64(pose_orientation_x) + builder.Prep(8, 24) + builder.PrependFloat64(pose_position_z) + builder.PrependFloat64(pose_position_y) + builder.PrependFloat64(pose_position_x) + builder.PrependFloat64(mass) + return builder.Offset() diff --git a/include/grl/flatbuffer/grl/flatbuffer/JointImpedenceControlMode.java b/include/grl/flatbuffer/grl/flatbuffer/JointImpedenceControlMode.java new file mode 100644 index 0000000..0e2510c --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/JointImpedenceControlMode.java @@ -0,0 +1,47 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class JointImpedenceControlMode extends Table { + public static JointImpedenceControlMode getRootAsJointImpedenceControlMode(ByteBuffer _bb) { return getRootAsJointImpedenceControlMode(_bb, new JointImpedenceControlMode()); } + public static JointImpedenceControlMode getRootAsJointImpedenceControlMode(ByteBuffer _bb, JointImpedenceControlMode obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public JointImpedenceControlMode __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + public double stiffness(int j) { int o = __offset(4); return o != 0 ? bb.getDouble(__vector(o) + j * 8) : 0; } + public int stiffnessLength() { int o = __offset(4); return o != 0 ? __vector_len(o) : 0; } + public ByteBuffer stiffnessAsByteBuffer() { return __vector_as_bytebuffer(4, 8); } + public ByteBuffer stiffnessInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 4, 8); } + public double damping(int j) { int o = __offset(6); return o != 0 ? bb.getDouble(__vector(o) + j * 8) : 0; } + public int dampingLength() { int o = __offset(6); return o != 0 ? __vector_len(o) : 0; } + public ByteBuffer dampingAsByteBuffer() { return __vector_as_bytebuffer(6, 8); } + public ByteBuffer dampingInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 6, 8); } + + public static int createJointImpedenceControlMode(FlatBufferBuilder builder, + int stiffnessOffset, + int dampingOffset) { + builder.startObject(2); + JointImpedenceControlMode.addDamping(builder, dampingOffset); + JointImpedenceControlMode.addStiffness(builder, stiffnessOffset); + return JointImpedenceControlMode.endJointImpedenceControlMode(builder); + } + + public static void startJointImpedenceControlMode(FlatBufferBuilder builder) { builder.startObject(2); } + public static void addStiffness(FlatBufferBuilder builder, int stiffnessOffset) { builder.addOffset(0, stiffnessOffset, 0); } + public static int createStiffnessVector(FlatBufferBuilder builder, double[] data) { builder.startVector(8, data.length, 8); for (int i = data.length - 1; i >= 0; i--) builder.addDouble(data[i]); return builder.endVector(); } + public static void startStiffnessVector(FlatBufferBuilder builder, int numElems) { builder.startVector(8, numElems, 8); } + public static void addDamping(FlatBufferBuilder builder, int dampingOffset) { builder.addOffset(1, dampingOffset, 0); } + public static int createDampingVector(FlatBufferBuilder builder, double[] data) { builder.startVector(8, data.length, 8); for (int i = data.length - 1; i >= 0; i--) builder.addDouble(data[i]); return builder.endVector(); } + public static void startDampingVector(FlatBufferBuilder builder, int numElems) { builder.startVector(8, numElems, 8); } + public static int endJointImpedenceControlMode(FlatBufferBuilder builder) { + int o = builder.endObject(); + return o; + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/JointImpedenceControlMode.py b/include/grl/flatbuffer/grl/flatbuffer/JointImpedenceControlMode.py new file mode 100644 index 0000000..85f4824 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/JointImpedenceControlMode.py @@ -0,0 +1,70 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class JointImpedenceControlMode(object): + __slots__ = ['_tab'] + + @classmethod + def GetRootAsJointImpedenceControlMode(cls, buf, offset): + n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) + x = JointImpedenceControlMode() + x.Init(buf, n + offset) + return x + + # JointImpedenceControlMode + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + + # JointImpedenceControlMode + def Stiffness(self, j): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + a = self._tab.Vector(o) + return self._tab.Get(flatbuffers.number_types.Float64Flags, a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 8)) + return 0 + + # JointImpedenceControlMode + def StiffnessAsNumpy(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + return self._tab.GetVectorAsNumpy(flatbuffers.number_types.Float64Flags, o) + return 0 + + # JointImpedenceControlMode + def StiffnessLength(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + return self._tab.VectorLen(o) + return 0 + + # JointImpedenceControlMode + def Damping(self, j): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) + if o != 0: + a = self._tab.Vector(o) + return self._tab.Get(flatbuffers.number_types.Float64Flags, a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 8)) + return 0 + + # JointImpedenceControlMode + def DampingAsNumpy(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) + if o != 0: + return self._tab.GetVectorAsNumpy(flatbuffers.number_types.Float64Flags, o) + return 0 + + # JointImpedenceControlMode + def DampingLength(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) + if o != 0: + return self._tab.VectorLen(o) + return 0 + +def JointImpedenceControlModeStart(builder): builder.StartObject(2) +def JointImpedenceControlModeAddStiffness(builder, stiffness): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(stiffness), 0) +def JointImpedenceControlModeStartStiffnessVector(builder, numElems): return builder.StartVector(8, numElems, 8) +def JointImpedenceControlModeAddDamping(builder, damping): builder.PrependUOffsetTRelativeSlot(1, flatbuffers.number_types.UOffsetTFlags.py_type(damping), 0) +def JointImpedenceControlModeStartDampingVector(builder, numElems): return builder.StartVector(8, numElems, 8) +def JointImpedenceControlModeEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/JointState.java b/include/grl/flatbuffer/grl/flatbuffer/JointState.java new file mode 100644 index 0000000..0380c56 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/JointState.java @@ -0,0 +1,65 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class JointState extends Table { + public static JointState getRootAsJointState(ByteBuffer _bb) { return getRootAsJointState(_bb, new JointState()); } + public static JointState getRootAsJointState(ByteBuffer _bb, JointState obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public JointState __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + public double position(int j) { int o = __offset(4); return o != 0 ? bb.getDouble(__vector(o) + j * 8) : 0; } + public int positionLength() { int o = __offset(4); return o != 0 ? __vector_len(o) : 0; } + public ByteBuffer positionAsByteBuffer() { return __vector_as_bytebuffer(4, 8); } + public ByteBuffer positionInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 4, 8); } + public double velocity(int j) { int o = __offset(6); return o != 0 ? bb.getDouble(__vector(o) + j * 8) : 0; } + public int velocityLength() { int o = __offset(6); return o != 0 ? __vector_len(o) : 0; } + public ByteBuffer velocityAsByteBuffer() { return __vector_as_bytebuffer(6, 8); } + public ByteBuffer velocityInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 6, 8); } + public double acceleration(int j) { int o = __offset(8); return o != 0 ? bb.getDouble(__vector(o) + j * 8) : 0; } + public int accelerationLength() { int o = __offset(8); return o != 0 ? __vector_len(o) : 0; } + public ByteBuffer accelerationAsByteBuffer() { return __vector_as_bytebuffer(8, 8); } + public ByteBuffer accelerationInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 8, 8); } + public double torque(int j) { int o = __offset(10); return o != 0 ? bb.getDouble(__vector(o) + j * 8) : 0; } + public int torqueLength() { int o = __offset(10); return o != 0 ? __vector_len(o) : 0; } + public ByteBuffer torqueAsByteBuffer() { return __vector_as_bytebuffer(10, 8); } + public ByteBuffer torqueInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 10, 8); } + + public static int createJointState(FlatBufferBuilder builder, + int positionOffset, + int velocityOffset, + int accelerationOffset, + int torqueOffset) { + builder.startObject(4); + JointState.addTorque(builder, torqueOffset); + JointState.addAcceleration(builder, accelerationOffset); + JointState.addVelocity(builder, velocityOffset); + JointState.addPosition(builder, positionOffset); + return JointState.endJointState(builder); + } + + public static void startJointState(FlatBufferBuilder builder) { builder.startObject(4); } + public static void addPosition(FlatBufferBuilder builder, int positionOffset) { builder.addOffset(0, positionOffset, 0); } + public static int createPositionVector(FlatBufferBuilder builder, double[] data) { builder.startVector(8, data.length, 8); for (int i = data.length - 1; i >= 0; i--) builder.addDouble(data[i]); return builder.endVector(); } + public static void startPositionVector(FlatBufferBuilder builder, int numElems) { builder.startVector(8, numElems, 8); } + public static void addVelocity(FlatBufferBuilder builder, int velocityOffset) { builder.addOffset(1, velocityOffset, 0); } + public static int createVelocityVector(FlatBufferBuilder builder, double[] data) { builder.startVector(8, data.length, 8); for (int i = data.length - 1; i >= 0; i--) builder.addDouble(data[i]); return builder.endVector(); } + public static void startVelocityVector(FlatBufferBuilder builder, int numElems) { builder.startVector(8, numElems, 8); } + public static void addAcceleration(FlatBufferBuilder builder, int accelerationOffset) { builder.addOffset(2, accelerationOffset, 0); } + public static int createAccelerationVector(FlatBufferBuilder builder, double[] data) { builder.startVector(8, data.length, 8); for (int i = data.length - 1; i >= 0; i--) builder.addDouble(data[i]); return builder.endVector(); } + public static void startAccelerationVector(FlatBufferBuilder builder, int numElems) { builder.startVector(8, numElems, 8); } + public static void addTorque(FlatBufferBuilder builder, int torqueOffset) { builder.addOffset(3, torqueOffset, 0); } + public static int createTorqueVector(FlatBufferBuilder builder, double[] data) { builder.startVector(8, data.length, 8); for (int i = data.length - 1; i >= 0; i--) builder.addDouble(data[i]); return builder.endVector(); } + public static void startTorqueVector(FlatBufferBuilder builder, int numElems) { builder.startVector(8, numElems, 8); } + public static int endJointState(FlatBufferBuilder builder) { + int o = builder.endObject(); + return o; + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/JointState.py b/include/grl/flatbuffer/grl/flatbuffer/JointState.py new file mode 100644 index 0000000..4746e33 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/JointState.py @@ -0,0 +1,118 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class JointState(object): + __slots__ = ['_tab'] + + @classmethod + def GetRootAsJointState(cls, buf, offset): + n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) + x = JointState() + x.Init(buf, n + offset) + return x + + # JointState + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + + # JointState + def Position(self, j): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + a = self._tab.Vector(o) + return self._tab.Get(flatbuffers.number_types.Float64Flags, a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 8)) + return 0 + + # JointState + def PositionAsNumpy(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + return self._tab.GetVectorAsNumpy(flatbuffers.number_types.Float64Flags, o) + return 0 + + # JointState + def PositionLength(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + return self._tab.VectorLen(o) + return 0 + + # JointState + def Velocity(self, j): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) + if o != 0: + a = self._tab.Vector(o) + return self._tab.Get(flatbuffers.number_types.Float64Flags, a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 8)) + return 0 + + # JointState + def VelocityAsNumpy(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) + if o != 0: + return self._tab.GetVectorAsNumpy(flatbuffers.number_types.Float64Flags, o) + return 0 + + # JointState + def VelocityLength(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) + if o != 0: + return self._tab.VectorLen(o) + return 0 + + # JointState + def Acceleration(self, j): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) + if o != 0: + a = self._tab.Vector(o) + return self._tab.Get(flatbuffers.number_types.Float64Flags, a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 8)) + return 0 + + # JointState + def AccelerationAsNumpy(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) + if o != 0: + return self._tab.GetVectorAsNumpy(flatbuffers.number_types.Float64Flags, o) + return 0 + + # JointState + def AccelerationLength(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) + if o != 0: + return self._tab.VectorLen(o) + return 0 + + # JointState + def Torque(self, j): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) + if o != 0: + a = self._tab.Vector(o) + return self._tab.Get(flatbuffers.number_types.Float64Flags, a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 8)) + return 0 + + # JointState + def TorqueAsNumpy(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) + if o != 0: + return self._tab.GetVectorAsNumpy(flatbuffers.number_types.Float64Flags, o) + return 0 + + # JointState + def TorqueLength(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) + if o != 0: + return self._tab.VectorLen(o) + return 0 + +def JointStateStart(builder): builder.StartObject(4) +def JointStateAddPosition(builder, position): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(position), 0) +def JointStateStartPositionVector(builder, numElems): return builder.StartVector(8, numElems, 8) +def JointStateAddVelocity(builder, velocity): builder.PrependUOffsetTRelativeSlot(1, flatbuffers.number_types.UOffsetTFlags.py_type(velocity), 0) +def JointStateStartVelocityVector(builder, numElems): return builder.StartVector(8, numElems, 8) +def JointStateAddAcceleration(builder, acceleration): builder.PrependUOffsetTRelativeSlot(2, flatbuffers.number_types.UOffsetTFlags.py_type(acceleration), 0) +def JointStateStartAccelerationVector(builder, numElems): return builder.StartVector(8, numElems, 8) +def JointStateAddTorque(builder, torque): builder.PrependUOffsetTRelativeSlot(3, flatbuffers.number_types.UOffsetTFlags.py_type(torque), 0) +def JointStateStartTorqueVector(builder, numElems): return builder.StartVector(8, numElems, 8) +def JointStateEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaArmConfiguration.java b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaArmConfiguration.java new file mode 100644 index 0000000..822026a --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaArmConfiguration.java @@ -0,0 +1,119 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class KUKAiiwaArmConfiguration extends Table { + public static KUKAiiwaArmConfiguration getRootAsKUKAiiwaArmConfiguration(ByteBuffer _bb) { return getRootAsKUKAiiwaArmConfiguration(_bb, new KUKAiiwaArmConfiguration()); } + public static KUKAiiwaArmConfiguration getRootAsKUKAiiwaArmConfiguration(ByteBuffer _bb, KUKAiiwaArmConfiguration obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public KUKAiiwaArmConfiguration __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + public String name() { int o = __offset(4); return o != 0 ? __string(o + bb_pos) : null; } + public ByteBuffer nameAsByteBuffer() { return __vector_as_bytebuffer(4, 1); } + public ByteBuffer nameInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 4, 1); } + /** + * how commands will be sent to robot + */ + public byte commandInterface() { int o = __offset(6); return o != 0 ? bb.get(o + bb_pos) : 0; } + /** + * how robot state will be sent to driver + */ + public byte monitorInterface() { int o = __offset(8); return o != 0 ? bb.get(o + bb_pos) : 0; } + /** + * motion command mode: cartesian, wrench, torque commands + */ + public byte clientCommandMode() { int o = __offset(10); return o != 0 ? bb.get(o + bb_pos) : 0; } + /** + * The type of commands FRI will use: cartesian, joint + */ + public byte overlayType() { int o = __offset(12); return o != 0 ? bb.get(o + bb_pos) : 0; } + /** + * position, cartesian impedence, or joint impedence low level controller adjustments + */ + public byte controlMode() { int o = __offset(14); return o != 0 ? bb.get(o + bb_pos) : 0; } + public CartesianImpedenceControlMode setCartImpedance() { return setCartImpedance(new CartesianImpedenceControlMode()); } + public CartesianImpedenceControlMode setCartImpedance(CartesianImpedenceControlMode obj) { int o = __offset(16); return o != 0 ? obj.__assign(__indirect(o + bb_pos), bb) : null; } + public JointImpedenceControlMode setJointImpedance() { return setJointImpedance(new JointImpedenceControlMode()); } + public JointImpedenceControlMode setJointImpedance(JointImpedenceControlMode obj) { int o = __offset(18); return o != 0 ? obj.__assign(__indirect(o + bb_pos), bb) : null; } + public SmartServo smartServoConfig() { return smartServoConfig(new SmartServo()); } + public SmartServo smartServoConfig(SmartServo obj) { int o = __offset(20); return o != 0 ? obj.__assign(__indirect(o + bb_pos), bb) : null; } + public FRI FRIConfig() { return FRIConfig(new FRI()); } + public FRI FRIConfig(FRI obj) { int o = __offset(22); return o != 0 ? obj.__assign(__indirect(o + bb_pos), bb) : null; } + public LinkObject tools(int j) { return tools(new LinkObject(), j); } + public LinkObject tools(LinkObject obj, int j) { int o = __offset(24); return o != 0 ? obj.__assign(__indirect(__vector(o) + j * 4), bb) : null; } + public int toolsLength() { int o = __offset(24); return o != 0 ? __vector_len(o) : 0; } + /** + * set kuka tablet "processData" panel UI config strings + */ + public ProcessData processData(int j) { return processData(new ProcessData(), j); } + public ProcessData processData(ProcessData obj, int j) { int o = __offset(26); return o != 0 ? obj.__assign(__indirect(__vector(o) + j * 4), bb) : null; } + public int processDataLength() { int o = __offset(26); return o != 0 ? __vector_len(o) : 0; } + public String currentMotionCenter() { int o = __offset(28); return o != 0 ? __string(o + bb_pos) : null; } + public ByteBuffer currentMotionCenterAsByteBuffer() { return __vector_as_bytebuffer(28, 1); } + public ByteBuffer currentMotionCenterInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 28, 1); } + public boolean requestMonitorProcessData() { int o = __offset(30); return o != 0 ? 0!=bb.get(o + bb_pos) : false; } + + public static int createKUKAiiwaArmConfiguration(FlatBufferBuilder builder, + int nameOffset, + byte commandInterface, + byte monitorInterface, + byte clientCommandMode, + byte overlayType, + byte controlMode, + int setCartImpedanceOffset, + int setJointImpedanceOffset, + int smartServoConfigOffset, + int FRIConfigOffset, + int toolsOffset, + int processDataOffset, + int currentMotionCenterOffset, + boolean requestMonitorProcessData) { + builder.startObject(14); + KUKAiiwaArmConfiguration.addCurrentMotionCenter(builder, currentMotionCenterOffset); + KUKAiiwaArmConfiguration.addProcessData(builder, processDataOffset); + KUKAiiwaArmConfiguration.addTools(builder, toolsOffset); + KUKAiiwaArmConfiguration.addFRIConfig(builder, FRIConfigOffset); + KUKAiiwaArmConfiguration.addSmartServoConfig(builder, smartServoConfigOffset); + KUKAiiwaArmConfiguration.addSetJointImpedance(builder, setJointImpedanceOffset); + KUKAiiwaArmConfiguration.addSetCartImpedance(builder, setCartImpedanceOffset); + KUKAiiwaArmConfiguration.addName(builder, nameOffset); + KUKAiiwaArmConfiguration.addRequestMonitorProcessData(builder, requestMonitorProcessData); + KUKAiiwaArmConfiguration.addControlMode(builder, controlMode); + KUKAiiwaArmConfiguration.addOverlayType(builder, overlayType); + KUKAiiwaArmConfiguration.addClientCommandMode(builder, clientCommandMode); + KUKAiiwaArmConfiguration.addMonitorInterface(builder, monitorInterface); + KUKAiiwaArmConfiguration.addCommandInterface(builder, commandInterface); + return KUKAiiwaArmConfiguration.endKUKAiiwaArmConfiguration(builder); + } + + public static void startKUKAiiwaArmConfiguration(FlatBufferBuilder builder) { builder.startObject(14); } + public static void addName(FlatBufferBuilder builder, int nameOffset) { builder.addOffset(0, nameOffset, 0); } + public static void addCommandInterface(FlatBufferBuilder builder, byte commandInterface) { builder.addByte(1, commandInterface, 0); } + public static void addMonitorInterface(FlatBufferBuilder builder, byte monitorInterface) { builder.addByte(2, monitorInterface, 0); } + public static void addClientCommandMode(FlatBufferBuilder builder, byte clientCommandMode) { builder.addByte(3, clientCommandMode, 0); } + public static void addOverlayType(FlatBufferBuilder builder, byte overlayType) { builder.addByte(4, overlayType, 0); } + public static void addControlMode(FlatBufferBuilder builder, byte controlMode) { builder.addByte(5, controlMode, 0); } + public static void addSetCartImpedance(FlatBufferBuilder builder, int setCartImpedanceOffset) { builder.addOffset(6, setCartImpedanceOffset, 0); } + public static void addSetJointImpedance(FlatBufferBuilder builder, int setJointImpedanceOffset) { builder.addOffset(7, setJointImpedanceOffset, 0); } + public static void addSmartServoConfig(FlatBufferBuilder builder, int smartServoConfigOffset) { builder.addOffset(8, smartServoConfigOffset, 0); } + public static void addFRIConfig(FlatBufferBuilder builder, int FRIConfigOffset) { builder.addOffset(9, FRIConfigOffset, 0); } + public static void addTools(FlatBufferBuilder builder, int toolsOffset) { builder.addOffset(10, toolsOffset, 0); } + public static int createToolsVector(FlatBufferBuilder builder, int[] data) { builder.startVector(4, data.length, 4); for (int i = data.length - 1; i >= 0; i--) builder.addOffset(data[i]); return builder.endVector(); } + public static void startToolsVector(FlatBufferBuilder builder, int numElems) { builder.startVector(4, numElems, 4); } + public static void addProcessData(FlatBufferBuilder builder, int processDataOffset) { builder.addOffset(11, processDataOffset, 0); } + public static int createProcessDataVector(FlatBufferBuilder builder, int[] data) { builder.startVector(4, data.length, 4); for (int i = data.length - 1; i >= 0; i--) builder.addOffset(data[i]); return builder.endVector(); } + public static void startProcessDataVector(FlatBufferBuilder builder, int numElems) { builder.startVector(4, numElems, 4); } + public static void addCurrentMotionCenter(FlatBufferBuilder builder, int currentMotionCenterOffset) { builder.addOffset(12, currentMotionCenterOffset, 0); } + public static void addRequestMonitorProcessData(FlatBufferBuilder builder, boolean requestMonitorProcessData) { builder.addBoolean(13, requestMonitorProcessData, false); } + public static int endKUKAiiwaArmConfiguration(FlatBufferBuilder builder) { + int o = builder.endObject(); + return o; + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaArmConfiguration.py b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaArmConfiguration.py new file mode 100644 index 0000000..94ea9fb --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaArmConfiguration.py @@ -0,0 +1,184 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class KUKAiiwaArmConfiguration(object): + __slots__ = ['_tab'] + + @classmethod + def GetRootAsKUKAiiwaArmConfiguration(cls, buf, offset): + n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) + x = KUKAiiwaArmConfiguration() + x.Init(buf, n + offset) + return x + + # KUKAiiwaArmConfiguration + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + + # KUKAiiwaArmConfiguration + def Name(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + return self._tab.String(o + self._tab.Pos) + return bytes() + +# /// how commands will be sent to robot + # KUKAiiwaArmConfiguration + def CommandInterface(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Int8Flags, o + self._tab.Pos) + return 0 + +# /// how robot state will be sent to driver + # KUKAiiwaArmConfiguration + def MonitorInterface(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Int8Flags, o + self._tab.Pos) + return 0 + +# /// motion command mode: cartesian, wrench, torque commands + # KUKAiiwaArmConfiguration + def ClientCommandMode(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Int8Flags, o + self._tab.Pos) + return 0 + +# /// The type of commands FRI will use: cartesian, joint + # KUKAiiwaArmConfiguration + def OverlayType(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(12)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Int8Flags, o + self._tab.Pos) + return 0 + +# /// position, cartesian impedence, or joint impedence low level controller adjustments + # KUKAiiwaArmConfiguration + def ControlMode(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(14)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Int8Flags, o + self._tab.Pos) + return 0 + + # KUKAiiwaArmConfiguration + def SetCartImpedance(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(16)) + if o != 0: + x = self._tab.Indirect(o + self._tab.Pos) + from .CartesianImpedenceControlMode import CartesianImpedenceControlMode + obj = CartesianImpedenceControlMode() + obj.Init(self._tab.Bytes, x) + return obj + return None + + # KUKAiiwaArmConfiguration + def SetJointImpedance(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(18)) + if o != 0: + x = self._tab.Indirect(o + self._tab.Pos) + from .JointImpedenceControlMode import JointImpedenceControlMode + obj = JointImpedenceControlMode() + obj.Init(self._tab.Bytes, x) + return obj + return None + + # KUKAiiwaArmConfiguration + def SmartServoConfig(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(20)) + if o != 0: + x = self._tab.Indirect(o + self._tab.Pos) + from .SmartServo import SmartServo + obj = SmartServo() + obj.Init(self._tab.Bytes, x) + return obj + return None + + # KUKAiiwaArmConfiguration + def FRIConfig(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(22)) + if o != 0: + x = self._tab.Indirect(o + self._tab.Pos) + from .FRI import FRI + obj = FRI() + obj.Init(self._tab.Bytes, x) + return obj + return None + + # KUKAiiwaArmConfiguration + def Tools(self, j): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(24)) + if o != 0: + x = self._tab.Vector(o) + x += flatbuffers.number_types.UOffsetTFlags.py_type(j) * 4 + x = self._tab.Indirect(x) + from .LinkObject import LinkObject + obj = LinkObject() + obj.Init(self._tab.Bytes, x) + return obj + return None + + # KUKAiiwaArmConfiguration + def ToolsLength(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(24)) + if o != 0: + return self._tab.VectorLen(o) + return 0 + +# /// set kuka tablet "processData" panel UI config strings + # KUKAiiwaArmConfiguration + def ProcessData(self, j): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(26)) + if o != 0: + x = self._tab.Vector(o) + x += flatbuffers.number_types.UOffsetTFlags.py_type(j) * 4 + x = self._tab.Indirect(x) + from .ProcessData import ProcessData + obj = ProcessData() + obj.Init(self._tab.Bytes, x) + return obj + return None + + # KUKAiiwaArmConfiguration + def ProcessDataLength(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(26)) + if o != 0: + return self._tab.VectorLen(o) + return 0 + + # KUKAiiwaArmConfiguration + def CurrentMotionCenter(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(28)) + if o != 0: + return self._tab.String(o + self._tab.Pos) + return bytes() + + # KUKAiiwaArmConfiguration + def RequestMonitorProcessData(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(30)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.BoolFlags, o + self._tab.Pos) + return 0 + +def KUKAiiwaArmConfigurationStart(builder): builder.StartObject(14) +def KUKAiiwaArmConfigurationAddName(builder, name): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(name), 0) +def KUKAiiwaArmConfigurationAddCommandInterface(builder, commandInterface): builder.PrependInt8Slot(1, commandInterface, 0) +def KUKAiiwaArmConfigurationAddMonitorInterface(builder, monitorInterface): builder.PrependInt8Slot(2, monitorInterface, 0) +def KUKAiiwaArmConfigurationAddClientCommandMode(builder, clientCommandMode): builder.PrependInt8Slot(3, clientCommandMode, 0) +def KUKAiiwaArmConfigurationAddOverlayType(builder, overlayType): builder.PrependInt8Slot(4, overlayType, 0) +def KUKAiiwaArmConfigurationAddControlMode(builder, controlMode): builder.PrependInt8Slot(5, controlMode, 0) +def KUKAiiwaArmConfigurationAddSetCartImpedance(builder, setCartImpedance): builder.PrependUOffsetTRelativeSlot(6, flatbuffers.number_types.UOffsetTFlags.py_type(setCartImpedance), 0) +def KUKAiiwaArmConfigurationAddSetJointImpedance(builder, setJointImpedance): builder.PrependUOffsetTRelativeSlot(7, flatbuffers.number_types.UOffsetTFlags.py_type(setJointImpedance), 0) +def KUKAiiwaArmConfigurationAddSmartServoConfig(builder, smartServoConfig): builder.PrependUOffsetTRelativeSlot(8, flatbuffers.number_types.UOffsetTFlags.py_type(smartServoConfig), 0) +def KUKAiiwaArmConfigurationAddFRIConfig(builder, FRIConfig): builder.PrependUOffsetTRelativeSlot(9, flatbuffers.number_types.UOffsetTFlags.py_type(FRIConfig), 0) +def KUKAiiwaArmConfigurationAddTools(builder, tools): builder.PrependUOffsetTRelativeSlot(10, flatbuffers.number_types.UOffsetTFlags.py_type(tools), 0) +def KUKAiiwaArmConfigurationStartToolsVector(builder, numElems): return builder.StartVector(4, numElems, 4) +def KUKAiiwaArmConfigurationAddProcessData(builder, processData): builder.PrependUOffsetTRelativeSlot(11, flatbuffers.number_types.UOffsetTFlags.py_type(processData), 0) +def KUKAiiwaArmConfigurationStartProcessDataVector(builder, numElems): return builder.StartVector(4, numElems, 4) +def KUKAiiwaArmConfigurationAddCurrentMotionCenter(builder, currentMotionCenter): builder.PrependUOffsetTRelativeSlot(12, flatbuffers.number_types.UOffsetTFlags.py_type(currentMotionCenter), 0) +def KUKAiiwaArmConfigurationAddRequestMonitorProcessData(builder, requestMonitorProcessData): builder.PrependBoolSlot(13, requestMonitorProcessData, 0) +def KUKAiiwaArmConfigurationEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaFusionTrackMessage.java b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaFusionTrackMessage.java new file mode 100644 index 0000000..7d956ff --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaFusionTrackMessage.java @@ -0,0 +1,46 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class KUKAiiwaFusionTrackMessage extends Table { + public static KUKAiiwaFusionTrackMessage getRootAsKUKAiiwaFusionTrackMessage(ByteBuffer _bb) { return getRootAsKUKAiiwaFusionTrackMessage(_bb, new KUKAiiwaFusionTrackMessage()); } + public static KUKAiiwaFusionTrackMessage getRootAsKUKAiiwaFusionTrackMessage(ByteBuffer _bb, KUKAiiwaFusionTrackMessage obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public KUKAiiwaFusionTrackMessage __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + public double timestamp() { int o = __offset(4); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } + public TimeEvent timeEvent() { return timeEvent(new TimeEvent()); } + public TimeEvent timeEvent(TimeEvent obj) { int o = __offset(6); return o != 0 ? obj.__assign(__indirect(o + bb_pos), bb) : null; } + public byte deviceStateType() { int o = __offset(8); return o != 0 ? bb.get(o + bb_pos) : 0; } + public Table deviceState(Table obj) { int o = __offset(10); return o != 0 ? __union(obj, o) : null; } + + public static int createKUKAiiwaFusionTrackMessage(FlatBufferBuilder builder, + double timestamp, + int timeEventOffset, + byte deviceState_type, + int deviceStateOffset) { + builder.startObject(4); + KUKAiiwaFusionTrackMessage.addTimestamp(builder, timestamp); + KUKAiiwaFusionTrackMessage.addDeviceState(builder, deviceStateOffset); + KUKAiiwaFusionTrackMessage.addTimeEvent(builder, timeEventOffset); + KUKAiiwaFusionTrackMessage.addDeviceStateType(builder, deviceState_type); + return KUKAiiwaFusionTrackMessage.endKUKAiiwaFusionTrackMessage(builder); + } + + public static void startKUKAiiwaFusionTrackMessage(FlatBufferBuilder builder) { builder.startObject(4); } + public static void addTimestamp(FlatBufferBuilder builder, double timestamp) { builder.addDouble(0, timestamp, 0.0); } + public static void addTimeEvent(FlatBufferBuilder builder, int timeEventOffset) { builder.addOffset(1, timeEventOffset, 0); } + public static void addDeviceStateType(FlatBufferBuilder builder, byte deviceStateType) { builder.addByte(2, deviceStateType, 0); } + public static void addDeviceState(FlatBufferBuilder builder, int deviceStateOffset) { builder.addOffset(3, deviceStateOffset, 0); } + public static int endKUKAiiwaFusionTrackMessage(FlatBufferBuilder builder) { + int o = builder.endObject(); + return o; + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaFusionTrackMessage.py b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaFusionTrackMessage.py new file mode 100644 index 0000000..78bef25 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaFusionTrackMessage.py @@ -0,0 +1,61 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class KUKAiiwaFusionTrackMessage(object): + __slots__ = ['_tab'] + + @classmethod + def GetRootAsKUKAiiwaFusionTrackMessage(cls, buf, offset): + n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) + x = KUKAiiwaFusionTrackMessage() + x.Init(buf, n + offset) + return x + + # KUKAiiwaFusionTrackMessage + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + + # KUKAiiwaFusionTrackMessage + def Timestamp(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) + return 0.0 + + # KUKAiiwaFusionTrackMessage + def TimeEvent(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) + if o != 0: + x = self._tab.Indirect(o + self._tab.Pos) + from .TimeEvent import TimeEvent + obj = TimeEvent() + obj.Init(self._tab.Bytes, x) + return obj + return None + + # KUKAiiwaFusionTrackMessage + def DeviceStateType(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Uint8Flags, o + self._tab.Pos) + return 0 + + # KUKAiiwaFusionTrackMessage + def DeviceState(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) + if o != 0: + from flatbuffers.table import Table + obj = Table(bytearray(), 0) + self._tab.Union(obj, o) + return obj + return None + +def KUKAiiwaFusionTrackMessageStart(builder): builder.StartObject(4) +def KUKAiiwaFusionTrackMessageAddTimestamp(builder, timestamp): builder.PrependFloat64Slot(0, timestamp, 0.0) +def KUKAiiwaFusionTrackMessageAddTimeEvent(builder, timeEvent): builder.PrependUOffsetTRelativeSlot(1, flatbuffers.number_types.UOffsetTFlags.py_type(timeEvent), 0) +def KUKAiiwaFusionTrackMessageAddDeviceStateType(builder, deviceStateType): builder.PrependUint8Slot(2, deviceStateType, 0) +def KUKAiiwaFusionTrackMessageAddDeviceState(builder, deviceState): builder.PrependUOffsetTRelativeSlot(3, flatbuffers.number_types.UOffsetTFlags.py_type(deviceState), 0) +def KUKAiiwaFusionTrackMessageEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaInterface.java b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaInterface.java new file mode 100644 index 0000000..2e85894 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaInterface.java @@ -0,0 +1,16 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +public final class KUKAiiwaInterface { + private KUKAiiwaInterface() { } + public static final byte Disabled = 0; + public static final byte SmartServo = 1; + public static final byte DirectServo = 2; + public static final byte FRI = 3; + + public static final String[] names = { "Disabled", "SmartServo", "DirectServo", "FRI", }; + + public static String name(int e) { return names[e]; } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaInterface.py b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaInterface.py new file mode 100644 index 0000000..64848e0 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaInterface.py @@ -0,0 +1,10 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +class KUKAiiwaInterface(object): + Disabled = 0 + SmartServo = 1 + DirectServo = 2 + FRI = 3 + diff --git a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaMonitorConfiguration.java b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaMonitorConfiguration.java new file mode 100644 index 0000000..edce424 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaMonitorConfiguration.java @@ -0,0 +1,63 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class KUKAiiwaMonitorConfiguration extends Table { + public static KUKAiiwaMonitorConfiguration getRootAsKUKAiiwaMonitorConfiguration(ByteBuffer _bb) { return getRootAsKUKAiiwaMonitorConfiguration(_bb, new KUKAiiwaMonitorConfiguration()); } + public static KUKAiiwaMonitorConfiguration getRootAsKUKAiiwaMonitorConfiguration(ByteBuffer _bb, KUKAiiwaMonitorConfiguration obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public KUKAiiwaMonitorConfiguration __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + public String hardwareVersion() { int o = __offset(4); return o != 0 ? __string(o + bb_pos) : null; } + public ByteBuffer hardwareVersionAsByteBuffer() { return __vector_as_bytebuffer(4, 1); } + public ByteBuffer hardwareVersionInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 4, 1); } + public double torqueSensorLimits(int j) { int o = __offset(6); return o != 0 ? bb.getDouble(__vector(o) + j * 8) : 0; } + public int torqueSensorLimitsLength() { int o = __offset(6); return o != 0 ? __vector_len(o) : 0; } + public ByteBuffer torqueSensorLimitsAsByteBuffer() { return __vector_as_bytebuffer(6, 8); } + public ByteBuffer torqueSensorLimitsInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 6, 8); } + public boolean isReadyToMove() { int o = __offset(8); return o != 0 ? 0!=bb.get(o + bb_pos) : false; } + public boolean isMastered() { int o = __offset(10); return o != 0 ? 0!=bb.get(o + bb_pos) : false; } + /** + * set kuka tablet "processData" panel UI config strings + */ + public ProcessData processData(int j) { return processData(new ProcessData(), j); } + public ProcessData processData(ProcessData obj, int j) { int o = __offset(12); return o != 0 ? obj.__assign(__indirect(__vector(o) + j * 4), bb) : null; } + public int processDataLength() { int o = __offset(12); return o != 0 ? __vector_len(o) : 0; } + + public static int createKUKAiiwaMonitorConfiguration(FlatBufferBuilder builder, + int hardwareVersionOffset, + int torqueSensorLimitsOffset, + boolean isReadyToMove, + boolean isMastered, + int processDataOffset) { + builder.startObject(5); + KUKAiiwaMonitorConfiguration.addProcessData(builder, processDataOffset); + KUKAiiwaMonitorConfiguration.addTorqueSensorLimits(builder, torqueSensorLimitsOffset); + KUKAiiwaMonitorConfiguration.addHardwareVersion(builder, hardwareVersionOffset); + KUKAiiwaMonitorConfiguration.addIsMastered(builder, isMastered); + KUKAiiwaMonitorConfiguration.addIsReadyToMove(builder, isReadyToMove); + return KUKAiiwaMonitorConfiguration.endKUKAiiwaMonitorConfiguration(builder); + } + + public static void startKUKAiiwaMonitorConfiguration(FlatBufferBuilder builder) { builder.startObject(5); } + public static void addHardwareVersion(FlatBufferBuilder builder, int hardwareVersionOffset) { builder.addOffset(0, hardwareVersionOffset, 0); } + public static void addTorqueSensorLimits(FlatBufferBuilder builder, int torqueSensorLimitsOffset) { builder.addOffset(1, torqueSensorLimitsOffset, 0); } + public static int createTorqueSensorLimitsVector(FlatBufferBuilder builder, double[] data) { builder.startVector(8, data.length, 8); for (int i = data.length - 1; i >= 0; i--) builder.addDouble(data[i]); return builder.endVector(); } + public static void startTorqueSensorLimitsVector(FlatBufferBuilder builder, int numElems) { builder.startVector(8, numElems, 8); } + public static void addIsReadyToMove(FlatBufferBuilder builder, boolean isReadyToMove) { builder.addBoolean(2, isReadyToMove, false); } + public static void addIsMastered(FlatBufferBuilder builder, boolean isMastered) { builder.addBoolean(3, isMastered, false); } + public static void addProcessData(FlatBufferBuilder builder, int processDataOffset) { builder.addOffset(4, processDataOffset, 0); } + public static int createProcessDataVector(FlatBufferBuilder builder, int[] data) { builder.startVector(4, data.length, 4); for (int i = data.length - 1; i >= 0; i--) builder.addOffset(data[i]); return builder.endVector(); } + public static void startProcessDataVector(FlatBufferBuilder builder, int numElems) { builder.startVector(4, numElems, 4); } + public static int endKUKAiiwaMonitorConfiguration(FlatBufferBuilder builder) { + int o = builder.endObject(); + return o; + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaMonitorConfiguration.py b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaMonitorConfiguration.py new file mode 100644 index 0000000..6c2201a --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaMonitorConfiguration.py @@ -0,0 +1,93 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class KUKAiiwaMonitorConfiguration(object): + __slots__ = ['_tab'] + + @classmethod + def GetRootAsKUKAiiwaMonitorConfiguration(cls, buf, offset): + n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) + x = KUKAiiwaMonitorConfiguration() + x.Init(buf, n + offset) + return x + + # KUKAiiwaMonitorConfiguration + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + + # KUKAiiwaMonitorConfiguration + def HardwareVersion(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + return self._tab.String(o + self._tab.Pos) + return bytes() + + # KUKAiiwaMonitorConfiguration + def TorqueSensorLimits(self, j): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) + if o != 0: + a = self._tab.Vector(o) + return self._tab.Get(flatbuffers.number_types.Float64Flags, a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 8)) + return 0 + + # KUKAiiwaMonitorConfiguration + def TorqueSensorLimitsAsNumpy(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) + if o != 0: + return self._tab.GetVectorAsNumpy(flatbuffers.number_types.Float64Flags, o) + return 0 + + # KUKAiiwaMonitorConfiguration + def TorqueSensorLimitsLength(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) + if o != 0: + return self._tab.VectorLen(o) + return 0 + + # KUKAiiwaMonitorConfiguration + def IsReadyToMove(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.BoolFlags, o + self._tab.Pos) + return 0 + + # KUKAiiwaMonitorConfiguration + def IsMastered(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.BoolFlags, o + self._tab.Pos) + return 0 + +# /// set kuka tablet "processData" panel UI config strings + # KUKAiiwaMonitorConfiguration + def ProcessData(self, j): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(12)) + if o != 0: + x = self._tab.Vector(o) + x += flatbuffers.number_types.UOffsetTFlags.py_type(j) * 4 + x = self._tab.Indirect(x) + from .ProcessData import ProcessData + obj = ProcessData() + obj.Init(self._tab.Bytes, x) + return obj + return None + + # KUKAiiwaMonitorConfiguration + def ProcessDataLength(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(12)) + if o != 0: + return self._tab.VectorLen(o) + return 0 + +def KUKAiiwaMonitorConfigurationStart(builder): builder.StartObject(5) +def KUKAiiwaMonitorConfigurationAddHardwareVersion(builder, hardwareVersion): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(hardwareVersion), 0) +def KUKAiiwaMonitorConfigurationAddTorqueSensorLimits(builder, torqueSensorLimits): builder.PrependUOffsetTRelativeSlot(1, flatbuffers.number_types.UOffsetTFlags.py_type(torqueSensorLimits), 0) +def KUKAiiwaMonitorConfigurationStartTorqueSensorLimitsVector(builder, numElems): return builder.StartVector(8, numElems, 8) +def KUKAiiwaMonitorConfigurationAddIsReadyToMove(builder, isReadyToMove): builder.PrependBoolSlot(2, isReadyToMove, 0) +def KUKAiiwaMonitorConfigurationAddIsMastered(builder, isMastered): builder.PrependBoolSlot(3, isMastered, 0) +def KUKAiiwaMonitorConfigurationAddProcessData(builder, processData): builder.PrependUOffsetTRelativeSlot(4, flatbuffers.number_types.UOffsetTFlags.py_type(processData), 0) +def KUKAiiwaMonitorConfigurationStartProcessDataVector(builder, numElems): return builder.StartVector(4, numElems, 4) +def KUKAiiwaMonitorConfigurationEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaMonitorState.java b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaMonitorState.java new file mode 100644 index 0000000..524ab79 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaMonitorState.java @@ -0,0 +1,57 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class KUKAiiwaMonitorState extends Table { + public static KUKAiiwaMonitorState getRootAsKUKAiiwaMonitorState(ByteBuffer _bb) { return getRootAsKUKAiiwaMonitorState(_bb, new KUKAiiwaMonitorState()); } + public static KUKAiiwaMonitorState getRootAsKUKAiiwaMonitorState(ByteBuffer _bb, KUKAiiwaMonitorState obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public KUKAiiwaMonitorState __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + public JointState measuredState() { return measuredState(new JointState()); } + public JointState measuredState(JointState obj) { int o = __offset(4); return o != 0 ? obj.__assign(__indirect(o + bb_pos), bb) : null; } + public Pose cartesianFlangePose() { return cartesianFlangePose(new Pose()); } + public Pose cartesianFlangePose(Pose obj) { int o = __offset(6); return o != 0 ? obj.__assign(o + bb_pos, bb) : null; } + public JointState jointStateReal() { return jointStateReal(new JointState()); } + public JointState jointStateReal(JointState obj) { int o = __offset(8); return o != 0 ? obj.__assign(__indirect(o + bb_pos), bb) : null; } + public JointState jointStateInterpolated() { return jointStateInterpolated(new JointState()); } + public JointState jointStateInterpolated(JointState obj) { int o = __offset(10); return o != 0 ? obj.__assign(__indirect(o + bb_pos), bb) : null; } + /** + * The state of the arm as calculated by kuka after + * subtracting the known weights of the arm + * and any attachments configured to be present. + * + * Most likely only contains torque. + * KukaState::ExternalTorque goes here + */ + public JointState externalState() { return externalState(new JointState()); } + public JointState externalState(JointState obj) { int o = __offset(12); return o != 0 ? obj.__assign(__indirect(o + bb_pos), bb) : null; } + /** + * KUKA::FRI::EOperationMode + */ + public byte operationMode() { int o = __offset(14); return o != 0 ? bb.get(o + bb_pos) : 0; } + public byte sessionState() { int o = __offset(16); return o != 0 ? bb.get(o + bb_pos) : 0; } + public Wrench CartesianWrench() { return CartesianWrench(new Wrench()); } + public Wrench CartesianWrench(Wrench obj) { int o = __offset(18); return o != 0 ? obj.__assign(o + bb_pos, bb) : null; } + + public static void startKUKAiiwaMonitorState(FlatBufferBuilder builder) { builder.startObject(8); } + public static void addMeasuredState(FlatBufferBuilder builder, int measuredStateOffset) { builder.addOffset(0, measuredStateOffset, 0); } + public static void addCartesianFlangePose(FlatBufferBuilder builder, int cartesianFlangePoseOffset) { builder.addStruct(1, cartesianFlangePoseOffset, 0); } + public static void addJointStateReal(FlatBufferBuilder builder, int jointStateRealOffset) { builder.addOffset(2, jointStateRealOffset, 0); } + public static void addJointStateInterpolated(FlatBufferBuilder builder, int jointStateInterpolatedOffset) { builder.addOffset(3, jointStateInterpolatedOffset, 0); } + public static void addExternalState(FlatBufferBuilder builder, int externalStateOffset) { builder.addOffset(4, externalStateOffset, 0); } + public static void addOperationMode(FlatBufferBuilder builder, byte operationMode) { builder.addByte(5, operationMode, 0); } + public static void addSessionState(FlatBufferBuilder builder, byte sessionState) { builder.addByte(6, sessionState, 0); } + public static void addCartesianWrench(FlatBufferBuilder builder, int CartesianWrenchOffset) { builder.addStruct(7, CartesianWrenchOffset, 0); } + public static int endKUKAiiwaMonitorState(FlatBufferBuilder builder) { + int o = builder.endObject(); + return o; + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaMonitorState.py b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaMonitorState.py new file mode 100644 index 0000000..601c1d1 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaMonitorState.py @@ -0,0 +1,117 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class KUKAiiwaMonitorState(object): + __slots__ = ['_tab'] + + @classmethod + def GetRootAsKUKAiiwaMonitorState(cls, buf, offset): + n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) + x = KUKAiiwaMonitorState() + x.Init(buf, n + offset) + return x + + # KUKAiiwaMonitorState + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + + # KUKAiiwaMonitorState + def MeasuredState(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + x = self._tab.Indirect(o + self._tab.Pos) + from .JointState import JointState + obj = JointState() + obj.Init(self._tab.Bytes, x) + return obj + return None + + # KUKAiiwaMonitorState + def CartesianFlangePose(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) + if o != 0: + x = o + self._tab.Pos + from .Pose import Pose + obj = Pose() + obj.Init(self._tab.Bytes, x) + return obj + return None + + # KUKAiiwaMonitorState + def JointStateReal(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) + if o != 0: + x = self._tab.Indirect(o + self._tab.Pos) + from .JointState import JointState + obj = JointState() + obj.Init(self._tab.Bytes, x) + return obj + return None + + # KUKAiiwaMonitorState + def JointStateInterpolated(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) + if o != 0: + x = self._tab.Indirect(o + self._tab.Pos) + from .JointState import JointState + obj = JointState() + obj.Init(self._tab.Bytes, x) + return obj + return None + +# /// The state of the arm as calculated by kuka after +# /// subtracting the known weights of the arm +# /// and any attachments configured to be present. +# /// +# /// Most likely only contains torque. +# /// KukaState::ExternalTorque goes here + # KUKAiiwaMonitorState + def ExternalState(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(12)) + if o != 0: + x = self._tab.Indirect(o + self._tab.Pos) + from .JointState import JointState + obj = JointState() + obj.Init(self._tab.Bytes, x) + return obj + return None + +# /// KUKA::FRI::EOperationMode + # KUKAiiwaMonitorState + def OperationMode(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(14)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Int8Flags, o + self._tab.Pos) + return 0 + + # KUKAiiwaMonitorState + def SessionState(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(16)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Int8Flags, o + self._tab.Pos) + return 0 + + # KUKAiiwaMonitorState + def CartesianWrench(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(18)) + if o != 0: + x = o + self._tab.Pos + from .Wrench import Wrench + obj = Wrench() + obj.Init(self._tab.Bytes, x) + return obj + return None + +def KUKAiiwaMonitorStateStart(builder): builder.StartObject(8) +def KUKAiiwaMonitorStateAddMeasuredState(builder, measuredState): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(measuredState), 0) +def KUKAiiwaMonitorStateAddCartesianFlangePose(builder, cartesianFlangePose): builder.PrependStructSlot(1, flatbuffers.number_types.UOffsetTFlags.py_type(cartesianFlangePose), 0) +def KUKAiiwaMonitorStateAddJointStateReal(builder, jointStateReal): builder.PrependUOffsetTRelativeSlot(2, flatbuffers.number_types.UOffsetTFlags.py_type(jointStateReal), 0) +def KUKAiiwaMonitorStateAddJointStateInterpolated(builder, jointStateInterpolated): builder.PrependUOffsetTRelativeSlot(3, flatbuffers.number_types.UOffsetTFlags.py_type(jointStateInterpolated), 0) +def KUKAiiwaMonitorStateAddExternalState(builder, externalState): builder.PrependUOffsetTRelativeSlot(4, flatbuffers.number_types.UOffsetTFlags.py_type(externalState), 0) +def KUKAiiwaMonitorStateAddOperationMode(builder, operationMode): builder.PrependInt8Slot(5, operationMode, 0) +def KUKAiiwaMonitorStateAddSessionState(builder, sessionState): builder.PrependInt8Slot(6, sessionState, 0) +def KUKAiiwaMonitorStateAddCartesianWrench(builder, CartesianWrench): builder.PrependStructSlot(7, flatbuffers.number_types.UOffsetTFlags.py_type(CartesianWrench), 0) +def KUKAiiwaMonitorStateEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaState.java b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaState.java new file mode 100644 index 0000000..5e9d7f3 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaState.java @@ -0,0 +1,93 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class KUKAiiwaState extends Table { + public static KUKAiiwaState getRootAsKUKAiiwaState(ByteBuffer _bb) { return getRootAsKUKAiiwaState(_bb, new KUKAiiwaState()); } + public static KUKAiiwaState getRootAsKUKAiiwaState(ByteBuffer _bb, KUKAiiwaState obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public KUKAiiwaState __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + public String name() { int o = __offset(4); return o != 0 ? __string(o + bb_pos) : null; } + public ByteBuffer nameAsByteBuffer() { return __vector_as_bytebuffer(4, 1); } + public ByteBuffer nameInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 4, 1); } + public String destination() { int o = __offset(6); return o != 0 ? __string(o + bb_pos) : null; } + public ByteBuffer destinationAsByteBuffer() { return __vector_as_bytebuffer(6, 1); } + public ByteBuffer destinationInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 6, 1); } + public String source() { int o = __offset(8); return o != 0 ? __string(o + bb_pos) : null; } + public ByteBuffer sourceAsByteBuffer() { return __vector_as_bytebuffer(8, 1); } + public ByteBuffer sourceInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 8, 1); } + public TimeEvent timeStamp() { return timeStamp(new TimeEvent()); } + public TimeEvent timeStamp(TimeEvent obj) { int o = __offset(10); return o != 0 ? obj.__assign(__indirect(o + bb_pos), bb) : null; } + public boolean setArmControlState() { int o = __offset(12); return o != 0 ? 0!=bb.get(o + bb_pos) : false; } + public ArmControlState armControlState() { return armControlState(new ArmControlState()); } + public ArmControlState armControlState(ArmControlState obj) { int o = __offset(14); return o != 0 ? obj.__assign(__indirect(o + bb_pos), bb) : null; } + public boolean setArmConfiguration() { int o = __offset(16); return o != 0 ? 0!=bb.get(o + bb_pos) : false; } + public KUKAiiwaArmConfiguration armConfiguration() { return armConfiguration(new KUKAiiwaArmConfiguration()); } + public KUKAiiwaArmConfiguration armConfiguration(KUKAiiwaArmConfiguration obj) { int o = __offset(18); return o != 0 ? obj.__assign(__indirect(o + bb_pos), bb) : null; } + public boolean hasMonitorState() { int o = __offset(20); return o != 0 ? 0!=bb.get(o + bb_pos) : false; } + public KUKAiiwaMonitorState monitorState() { return monitorState(new KUKAiiwaMonitorState()); } + public KUKAiiwaMonitorState monitorState(KUKAiiwaMonitorState obj) { int o = __offset(22); return o != 0 ? obj.__assign(__indirect(o + bb_pos), bb) : null; } + public boolean hasMonitorConfig() { int o = __offset(24); return o != 0 ? 0!=bb.get(o + bb_pos) : false; } + public KUKAiiwaMonitorConfiguration monitorConfig() { return monitorConfig(new KUKAiiwaMonitorConfiguration()); } + public KUKAiiwaMonitorConfiguration monitorConfig(KUKAiiwaMonitorConfiguration obj) { int o = __offset(26); return o != 0 ? obj.__assign(__indirect(o + bb_pos), bb) : null; } + public FRIMessageLog FRIMessage() { return FRIMessage(new FRIMessageLog()); } + public FRIMessageLog FRIMessage(FRIMessageLog obj) { int o = __offset(28); return o != 0 ? obj.__assign(__indirect(o + bb_pos), bb) : null; } + + public static int createKUKAiiwaState(FlatBufferBuilder builder, + int nameOffset, + int destinationOffset, + int sourceOffset, + int timeStampOffset, + boolean setArmControlState, + int armControlStateOffset, + boolean setArmConfiguration, + int armConfigurationOffset, + boolean hasMonitorState, + int monitorStateOffset, + boolean hasMonitorConfig, + int monitorConfigOffset, + int FRIMessageOffset) { + builder.startObject(13); + KUKAiiwaState.addFRIMessage(builder, FRIMessageOffset); + KUKAiiwaState.addMonitorConfig(builder, monitorConfigOffset); + KUKAiiwaState.addMonitorState(builder, monitorStateOffset); + KUKAiiwaState.addArmConfiguration(builder, armConfigurationOffset); + KUKAiiwaState.addArmControlState(builder, armControlStateOffset); + KUKAiiwaState.addTimeStamp(builder, timeStampOffset); + KUKAiiwaState.addSource(builder, sourceOffset); + KUKAiiwaState.addDestination(builder, destinationOffset); + KUKAiiwaState.addName(builder, nameOffset); + KUKAiiwaState.addHasMonitorConfig(builder, hasMonitorConfig); + KUKAiiwaState.addHasMonitorState(builder, hasMonitorState); + KUKAiiwaState.addSetArmConfiguration(builder, setArmConfiguration); + KUKAiiwaState.addSetArmControlState(builder, setArmControlState); + return KUKAiiwaState.endKUKAiiwaState(builder); + } + + public static void startKUKAiiwaState(FlatBufferBuilder builder) { builder.startObject(13); } + public static void addName(FlatBufferBuilder builder, int nameOffset) { builder.addOffset(0, nameOffset, 0); } + public static void addDestination(FlatBufferBuilder builder, int destinationOffset) { builder.addOffset(1, destinationOffset, 0); } + public static void addSource(FlatBufferBuilder builder, int sourceOffset) { builder.addOffset(2, sourceOffset, 0); } + public static void addTimeStamp(FlatBufferBuilder builder, int timeStampOffset) { builder.addOffset(3, timeStampOffset, 0); } + public static void addSetArmControlState(FlatBufferBuilder builder, boolean setArmControlState) { builder.addBoolean(4, setArmControlState, false); } + public static void addArmControlState(FlatBufferBuilder builder, int armControlStateOffset) { builder.addOffset(5, armControlStateOffset, 0); } + public static void addSetArmConfiguration(FlatBufferBuilder builder, boolean setArmConfiguration) { builder.addBoolean(6, setArmConfiguration, false); } + public static void addArmConfiguration(FlatBufferBuilder builder, int armConfigurationOffset) { builder.addOffset(7, armConfigurationOffset, 0); } + public static void addHasMonitorState(FlatBufferBuilder builder, boolean hasMonitorState) { builder.addBoolean(8, hasMonitorState, false); } + public static void addMonitorState(FlatBufferBuilder builder, int monitorStateOffset) { builder.addOffset(9, monitorStateOffset, 0); } + public static void addHasMonitorConfig(FlatBufferBuilder builder, boolean hasMonitorConfig) { builder.addBoolean(10, hasMonitorConfig, false); } + public static void addMonitorConfig(FlatBufferBuilder builder, int monitorConfigOffset) { builder.addOffset(11, monitorConfigOffset, 0); } + public static void addFRIMessage(FlatBufferBuilder builder, int FRIMessageOffset) { builder.addOffset(12, FRIMessageOffset, 0); } + public static int endKUKAiiwaState(FlatBufferBuilder builder) { + int o = builder.endObject(); + return o; + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaState.py b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaState.py new file mode 100644 index 0000000..69de57a --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaState.py @@ -0,0 +1,150 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class KUKAiiwaState(object): + __slots__ = ['_tab'] + + @classmethod + def GetRootAsKUKAiiwaState(cls, buf, offset): + n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) + x = KUKAiiwaState() + x.Init(buf, n + offset) + return x + + # KUKAiiwaState + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + + # KUKAiiwaState + def Name(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + return self._tab.String(o + self._tab.Pos) + return bytes() + + # KUKAiiwaState + def Destination(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) + if o != 0: + return self._tab.String(o + self._tab.Pos) + return bytes() + + # KUKAiiwaState + def Source(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) + if o != 0: + return self._tab.String(o + self._tab.Pos) + return bytes() + + # KUKAiiwaState + def TimeStamp(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) + if o != 0: + x = self._tab.Indirect(o + self._tab.Pos) + from .TimeEvent import TimeEvent + obj = TimeEvent() + obj.Init(self._tab.Bytes, x) + return obj + return None + + # KUKAiiwaState + def SetArmControlState(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(12)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.BoolFlags, o + self._tab.Pos) + return 0 + + # KUKAiiwaState + def ArmControlState(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(14)) + if o != 0: + x = self._tab.Indirect(o + self._tab.Pos) + from .ArmControlState import ArmControlState + obj = ArmControlState() + obj.Init(self._tab.Bytes, x) + return obj + return None + + # KUKAiiwaState + def SetArmConfiguration(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(16)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.BoolFlags, o + self._tab.Pos) + return 0 + + # KUKAiiwaState + def ArmConfiguration(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(18)) + if o != 0: + x = self._tab.Indirect(o + self._tab.Pos) + from .KUKAiiwaArmConfiguration import KUKAiiwaArmConfiguration + obj = KUKAiiwaArmConfiguration() + obj.Init(self._tab.Bytes, x) + return obj + return None + + # KUKAiiwaState + def HasMonitorState(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(20)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.BoolFlags, o + self._tab.Pos) + return 0 + + # KUKAiiwaState + def MonitorState(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(22)) + if o != 0: + x = self._tab.Indirect(o + self._tab.Pos) + from .KUKAiiwaMonitorState import KUKAiiwaMonitorState + obj = KUKAiiwaMonitorState() + obj.Init(self._tab.Bytes, x) + return obj + return None + + # KUKAiiwaState + def HasMonitorConfig(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(24)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.BoolFlags, o + self._tab.Pos) + return 0 + + # KUKAiiwaState + def MonitorConfig(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(26)) + if o != 0: + x = self._tab.Indirect(o + self._tab.Pos) + from .KUKAiiwaMonitorConfiguration import KUKAiiwaMonitorConfiguration + obj = KUKAiiwaMonitorConfiguration() + obj.Init(self._tab.Bytes, x) + return obj + return None + + # KUKAiiwaState + def FRIMessage(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(28)) + if o != 0: + x = self._tab.Indirect(o + self._tab.Pos) + from .FRIMessageLog import FRIMessageLog + obj = FRIMessageLog() + obj.Init(self._tab.Bytes, x) + return obj + return None + +def KUKAiiwaStateStart(builder): builder.StartObject(13) +def KUKAiiwaStateAddName(builder, name): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(name), 0) +def KUKAiiwaStateAddDestination(builder, destination): builder.PrependUOffsetTRelativeSlot(1, flatbuffers.number_types.UOffsetTFlags.py_type(destination), 0) +def KUKAiiwaStateAddSource(builder, source): builder.PrependUOffsetTRelativeSlot(2, flatbuffers.number_types.UOffsetTFlags.py_type(source), 0) +def KUKAiiwaStateAddTimeStamp(builder, timeStamp): builder.PrependUOffsetTRelativeSlot(3, flatbuffers.number_types.UOffsetTFlags.py_type(timeStamp), 0) +def KUKAiiwaStateAddSetArmControlState(builder, setArmControlState): builder.PrependBoolSlot(4, setArmControlState, 0) +def KUKAiiwaStateAddArmControlState(builder, armControlState): builder.PrependUOffsetTRelativeSlot(5, flatbuffers.number_types.UOffsetTFlags.py_type(armControlState), 0) +def KUKAiiwaStateAddSetArmConfiguration(builder, setArmConfiguration): builder.PrependBoolSlot(6, setArmConfiguration, 0) +def KUKAiiwaStateAddArmConfiguration(builder, armConfiguration): builder.PrependUOffsetTRelativeSlot(7, flatbuffers.number_types.UOffsetTFlags.py_type(armConfiguration), 0) +def KUKAiiwaStateAddHasMonitorState(builder, hasMonitorState): builder.PrependBoolSlot(8, hasMonitorState, 0) +def KUKAiiwaStateAddMonitorState(builder, monitorState): builder.PrependUOffsetTRelativeSlot(9, flatbuffers.number_types.UOffsetTFlags.py_type(monitorState), 0) +def KUKAiiwaStateAddHasMonitorConfig(builder, hasMonitorConfig): builder.PrependBoolSlot(10, hasMonitorConfig, 0) +def KUKAiiwaStateAddMonitorConfig(builder, monitorConfig): builder.PrependUOffsetTRelativeSlot(11, flatbuffers.number_types.UOffsetTFlags.py_type(monitorConfig), 0) +def KUKAiiwaStateAddFRIMessage(builder, FRIMessage): builder.PrependUOffsetTRelativeSlot(12, flatbuffers.number_types.UOffsetTFlags.py_type(FRIMessage), 0) +def KUKAiiwaStateEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaStates.java b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaStates.java new file mode 100644 index 0000000..8930bdf --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaStates.java @@ -0,0 +1,40 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class KUKAiiwaStates extends Table { + public static KUKAiiwaStates getRootAsKUKAiiwaStates(ByteBuffer _bb) { return getRootAsKUKAiiwaStates(_bb, new KUKAiiwaStates()); } + public static KUKAiiwaStates getRootAsKUKAiiwaStates(ByteBuffer _bb, KUKAiiwaStates obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } + public static boolean KUKAiiwaStatesBufferHasIdentifier(ByteBuffer _bb) { return __has_identifier(_bb, "iiwa"); } + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public KUKAiiwaStates __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + public KUKAiiwaState states(int j) { return states(new KUKAiiwaState(), j); } + public KUKAiiwaState states(KUKAiiwaState obj, int j) { int o = __offset(4); return o != 0 ? obj.__assign(__indirect(__vector(o) + j * 4), bb) : null; } + public int statesLength() { int o = __offset(4); return o != 0 ? __vector_len(o) : 0; } + + public static int createKUKAiiwaStates(FlatBufferBuilder builder, + int statesOffset) { + builder.startObject(1); + KUKAiiwaStates.addStates(builder, statesOffset); + return KUKAiiwaStates.endKUKAiiwaStates(builder); + } + + public static void startKUKAiiwaStates(FlatBufferBuilder builder) { builder.startObject(1); } + public static void addStates(FlatBufferBuilder builder, int statesOffset) { builder.addOffset(0, statesOffset, 0); } + public static int createStatesVector(FlatBufferBuilder builder, int[] data) { builder.startVector(4, data.length, 4); for (int i = data.length - 1; i >= 0; i--) builder.addOffset(data[i]); return builder.endVector(); } + public static void startStatesVector(FlatBufferBuilder builder, int numElems) { builder.startVector(4, numElems, 4); } + public static int endKUKAiiwaStates(FlatBufferBuilder builder) { + int o = builder.endObject(); + return o; + } + public static void finishKUKAiiwaStatesBuffer(FlatBufferBuilder builder, int offset) { builder.finish(offset, "iiwa"); } + public static void finishSizePrefixedKUKAiiwaStatesBuffer(FlatBufferBuilder builder, int offset) { builder.finishSizePrefixed(offset, "iiwa"); } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaStates.py b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaStates.py new file mode 100644 index 0000000..dfeab00 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaStates.py @@ -0,0 +1,44 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class KUKAiiwaStates(object): + __slots__ = ['_tab'] + + @classmethod + def GetRootAsKUKAiiwaStates(cls, buf, offset): + n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) + x = KUKAiiwaStates() + x.Init(buf, n + offset) + return x + + # KUKAiiwaStates + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + + # KUKAiiwaStates + def States(self, j): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + x = self._tab.Vector(o) + x += flatbuffers.number_types.UOffsetTFlags.py_type(j) * 4 + x = self._tab.Indirect(x) + from .KUKAiiwaState import KUKAiiwaState + obj = KUKAiiwaState() + obj.Init(self._tab.Bytes, x) + return obj + return None + + # KUKAiiwaStates + def StatesLength(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + return self._tab.VectorLen(o) + return 0 + +def KUKAiiwaStatesStart(builder): builder.StartObject(1) +def KUKAiiwaStatesAddStates(builder, states): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(states), 0) +def KUKAiiwaStatesStartStatesVector(builder, numElems): return builder.StartVector(4, numElems, 4) +def KUKAiiwaStatesEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/LinkObject.java b/include/grl/flatbuffer/grl/flatbuffer/LinkObject.java new file mode 100644 index 0000000..6332388 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/LinkObject.java @@ -0,0 +1,38 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class LinkObject extends Table { + public static LinkObject getRootAsLinkObject(ByteBuffer _bb) { return getRootAsLinkObject(_bb, new LinkObject()); } + public static LinkObject getRootAsLinkObject(ByteBuffer _bb, LinkObject obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public LinkObject __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + public String name() { int o = __offset(4); return o != 0 ? __string(o + bb_pos) : null; } + public ByteBuffer nameAsByteBuffer() { return __vector_as_bytebuffer(4, 1); } + public ByteBuffer nameInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 4, 1); } + public String parent() { int o = __offset(6); return o != 0 ? __string(o + bb_pos) : null; } + public ByteBuffer parentAsByteBuffer() { return __vector_as_bytebuffer(6, 1); } + public ByteBuffer parentInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 6, 1); } + public Pose pose() { return pose(new Pose()); } + public Pose pose(Pose obj) { int o = __offset(8); return o != 0 ? obj.__assign(o + bb_pos, bb) : null; } + public Inertia inertia() { return inertia(new Inertia()); } + public Inertia inertia(Inertia obj) { int o = __offset(10); return o != 0 ? obj.__assign(o + bb_pos, bb) : null; } + + public static void startLinkObject(FlatBufferBuilder builder) { builder.startObject(4); } + public static void addName(FlatBufferBuilder builder, int nameOffset) { builder.addOffset(0, nameOffset, 0); } + public static void addParent(FlatBufferBuilder builder, int parentOffset) { builder.addOffset(1, parentOffset, 0); } + public static void addPose(FlatBufferBuilder builder, int poseOffset) { builder.addStruct(2, poseOffset, 0); } + public static void addInertia(FlatBufferBuilder builder, int inertiaOffset) { builder.addStruct(3, inertiaOffset, 0); } + public static int endLinkObject(FlatBufferBuilder builder) { + int o = builder.endObject(); + return o; + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/LinkObject.py b/include/grl/flatbuffer/grl/flatbuffer/LinkObject.py new file mode 100644 index 0000000..e61d17e --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/LinkObject.py @@ -0,0 +1,62 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class LinkObject(object): + __slots__ = ['_tab'] + + @classmethod + def GetRootAsLinkObject(cls, buf, offset): + n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) + x = LinkObject() + x.Init(buf, n + offset) + return x + + # LinkObject + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + + # LinkObject + def Name(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + return self._tab.String(o + self._tab.Pos) + return bytes() + + # LinkObject + def Parent(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) + if o != 0: + return self._tab.String(o + self._tab.Pos) + return bytes() + + # LinkObject + def Pose(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) + if o != 0: + x = o + self._tab.Pos + from .Pose import Pose + obj = Pose() + obj.Init(self._tab.Bytes, x) + return obj + return None + + # LinkObject + def Inertia(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) + if o != 0: + x = o + self._tab.Pos + from .Inertia import Inertia + obj = Inertia() + obj.Init(self._tab.Bytes, x) + return obj + return None + +def LinkObjectStart(builder): builder.StartObject(4) +def LinkObjectAddName(builder, name): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(name), 0) +def LinkObjectAddParent(builder, parent): builder.PrependUOffsetTRelativeSlot(1, flatbuffers.number_types.UOffsetTFlags.py_type(parent), 0) +def LinkObjectAddPose(builder, pose): builder.PrependStructSlot(2, flatbuffers.number_types.UOffsetTFlags.py_type(pose), 0) +def LinkObjectAddInertia(builder, inertia): builder.PrependStructSlot(3, flatbuffers.number_types.UOffsetTFlags.py_type(inertia), 0) +def LinkObjectEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/LogKUKAiiwaFusionTrack.java b/include/grl/flatbuffer/grl/flatbuffer/LogKUKAiiwaFusionTrack.java new file mode 100644 index 0000000..ace360d --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/LogKUKAiiwaFusionTrack.java @@ -0,0 +1,40 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class LogKUKAiiwaFusionTrack extends Table { + public static LogKUKAiiwaFusionTrack getRootAsLogKUKAiiwaFusionTrack(ByteBuffer _bb) { return getRootAsLogKUKAiiwaFusionTrack(_bb, new LogKUKAiiwaFusionTrack()); } + public static LogKUKAiiwaFusionTrack getRootAsLogKUKAiiwaFusionTrack(ByteBuffer _bb, LogKUKAiiwaFusionTrack obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } + public static boolean LogKUKAiiwaFusionTrackBufferHasIdentifier(ByteBuffer _bb) { return __has_identifier(_bb, "flik"); } + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public LogKUKAiiwaFusionTrack __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + public KUKAiiwaFusionTrackMessage states(int j) { return states(new KUKAiiwaFusionTrackMessage(), j); } + public KUKAiiwaFusionTrackMessage states(KUKAiiwaFusionTrackMessage obj, int j) { int o = __offset(4); return o != 0 ? obj.__assign(__indirect(__vector(o) + j * 4), bb) : null; } + public int statesLength() { int o = __offset(4); return o != 0 ? __vector_len(o) : 0; } + + public static int createLogKUKAiiwaFusionTrack(FlatBufferBuilder builder, + int statesOffset) { + builder.startObject(1); + LogKUKAiiwaFusionTrack.addStates(builder, statesOffset); + return LogKUKAiiwaFusionTrack.endLogKUKAiiwaFusionTrack(builder); + } + + public static void startLogKUKAiiwaFusionTrack(FlatBufferBuilder builder) { builder.startObject(1); } + public static void addStates(FlatBufferBuilder builder, int statesOffset) { builder.addOffset(0, statesOffset, 0); } + public static int createStatesVector(FlatBufferBuilder builder, int[] data) { builder.startVector(4, data.length, 4); for (int i = data.length - 1; i >= 0; i--) builder.addOffset(data[i]); return builder.endVector(); } + public static void startStatesVector(FlatBufferBuilder builder, int numElems) { builder.startVector(4, numElems, 4); } + public static int endLogKUKAiiwaFusionTrack(FlatBufferBuilder builder) { + int o = builder.endObject(); + return o; + } + public static void finishLogKUKAiiwaFusionTrackBuffer(FlatBufferBuilder builder, int offset) { builder.finish(offset, "flik"); } + public static void finishSizePrefixedLogKUKAiiwaFusionTrackBuffer(FlatBufferBuilder builder, int offset) { builder.finishSizePrefixed(offset, "flik"); } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/LogKUKAiiwaFusionTrack.py b/include/grl/flatbuffer/grl/flatbuffer/LogKUKAiiwaFusionTrack.py new file mode 100644 index 0000000..5c223a6 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/LogKUKAiiwaFusionTrack.py @@ -0,0 +1,44 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class LogKUKAiiwaFusionTrack(object): + __slots__ = ['_tab'] + + @classmethod + def GetRootAsLogKUKAiiwaFusionTrack(cls, buf, offset): + n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) + x = LogKUKAiiwaFusionTrack() + x.Init(buf, n + offset) + return x + + # LogKUKAiiwaFusionTrack + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + + # LogKUKAiiwaFusionTrack + def States(self, j): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + x = self._tab.Vector(o) + x += flatbuffers.number_types.UOffsetTFlags.py_type(j) * 4 + x = self._tab.Indirect(x) + from .KUKAiiwaFusionTrackMessage import KUKAiiwaFusionTrackMessage + obj = KUKAiiwaFusionTrackMessage() + obj.Init(self._tab.Bytes, x) + return obj + return None + + # LogKUKAiiwaFusionTrack + def StatesLength(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + return self._tab.VectorLen(o) + return 0 + +def LogKUKAiiwaFusionTrackStart(builder): builder.StartObject(1) +def LogKUKAiiwaFusionTrackAddStates(builder, states): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(states), 0) +def LogKUKAiiwaFusionTrackStartStatesVector(builder, numElems): return builder.StartVector(4, numElems, 4) +def LogKUKAiiwaFusionTrackEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/MoveArmCartesianServo.java b/include/grl/flatbuffer/grl/flatbuffer/MoveArmCartesianServo.java new file mode 100644 index 0000000..3e0a78f --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/MoveArmCartesianServo.java @@ -0,0 +1,31 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class MoveArmCartesianServo extends Table { + public static MoveArmCartesianServo getRootAsMoveArmCartesianServo(ByteBuffer _bb) { return getRootAsMoveArmCartesianServo(_bb, new MoveArmCartesianServo()); } + public static MoveArmCartesianServo getRootAsMoveArmCartesianServo(ByteBuffer _bb, MoveArmCartesianServo obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public MoveArmCartesianServo __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + public String parent() { int o = __offset(4); return o != 0 ? __string(o + bb_pos) : null; } + public ByteBuffer parentAsByteBuffer() { return __vector_as_bytebuffer(4, 1); } + public ByteBuffer parentInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 4, 1); } + public Pose goal() { return goal(new Pose()); } + public Pose goal(Pose obj) { int o = __offset(6); return o != 0 ? obj.__assign(o + bb_pos, bb) : null; } + + public static void startMoveArmCartesianServo(FlatBufferBuilder builder) { builder.startObject(2); } + public static void addParent(FlatBufferBuilder builder, int parentOffset) { builder.addOffset(0, parentOffset, 0); } + public static void addGoal(FlatBufferBuilder builder, int goalOffset) { builder.addStruct(1, goalOffset, 0); } + public static int endMoveArmCartesianServo(FlatBufferBuilder builder) { + int o = builder.endObject(); + return o; + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/MoveArmCartesianServo.py b/include/grl/flatbuffer/grl/flatbuffer/MoveArmCartesianServo.py new file mode 100644 index 0000000..1f8b622 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/MoveArmCartesianServo.py @@ -0,0 +1,42 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class MoveArmCartesianServo(object): + __slots__ = ['_tab'] + + @classmethod + def GetRootAsMoveArmCartesianServo(cls, buf, offset): + n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) + x = MoveArmCartesianServo() + x.Init(buf, n + offset) + return x + + # MoveArmCartesianServo + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + + # MoveArmCartesianServo + def Parent(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + return self._tab.String(o + self._tab.Pos) + return bytes() + + # MoveArmCartesianServo + def Goal(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) + if o != 0: + x = o + self._tab.Pos + from .Pose import Pose + obj = Pose() + obj.Init(self._tab.Bytes, x) + return obj + return None + +def MoveArmCartesianServoStart(builder): builder.StartObject(2) +def MoveArmCartesianServoAddParent(builder, parent): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(parent), 0) +def MoveArmCartesianServoAddGoal(builder, goal): builder.PrependStructSlot(1, flatbuffers.number_types.UOffsetTFlags.py_type(goal), 0) +def MoveArmCartesianServoEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/MoveArmJointServo.java b/include/grl/flatbuffer/grl/flatbuffer/MoveArmJointServo.java new file mode 100644 index 0000000..92f91a2 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/MoveArmJointServo.java @@ -0,0 +1,34 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class MoveArmJointServo extends Table { + public static MoveArmJointServo getRootAsMoveArmJointServo(ByteBuffer _bb) { return getRootAsMoveArmJointServo(_bb, new MoveArmJointServo()); } + public static MoveArmJointServo getRootAsMoveArmJointServo(ByteBuffer _bb, MoveArmJointServo obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public MoveArmJointServo __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + public JointState goal() { return goal(new JointState()); } + public JointState goal(JointState obj) { int o = __offset(4); return o != 0 ? obj.__assign(__indirect(o + bb_pos), bb) : null; } + + public static int createMoveArmJointServo(FlatBufferBuilder builder, + int goalOffset) { + builder.startObject(1); + MoveArmJointServo.addGoal(builder, goalOffset); + return MoveArmJointServo.endMoveArmJointServo(builder); + } + + public static void startMoveArmJointServo(FlatBufferBuilder builder) { builder.startObject(1); } + public static void addGoal(FlatBufferBuilder builder, int goalOffset) { builder.addOffset(0, goalOffset, 0); } + public static int endMoveArmJointServo(FlatBufferBuilder builder) { + int o = builder.endObject(); + return o; + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/MoveArmJointServo.py b/include/grl/flatbuffer/grl/flatbuffer/MoveArmJointServo.py new file mode 100644 index 0000000..fb233fc --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/MoveArmJointServo.py @@ -0,0 +1,34 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class MoveArmJointServo(object): + __slots__ = ['_tab'] + + @classmethod + def GetRootAsMoveArmJointServo(cls, buf, offset): + n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) + x = MoveArmJointServo() + x.Init(buf, n + offset) + return x + + # MoveArmJointServo + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + + # MoveArmJointServo + def Goal(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + x = self._tab.Indirect(o + self._tab.Pos) + from .JointState import JointState + obj = JointState() + obj.Init(self._tab.Bytes, x) + return obj + return None + +def MoveArmJointServoStart(builder): builder.StartObject(1) +def MoveArmJointServoAddGoal(builder, goal): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(goal), 0) +def MoveArmJointServoEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/MoveArmTrajectory.java b/include/grl/flatbuffer/grl/flatbuffer/MoveArmTrajectory.java new file mode 100644 index 0000000..5e4709c --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/MoveArmTrajectory.java @@ -0,0 +1,37 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class MoveArmTrajectory extends Table { + public static MoveArmTrajectory getRootAsMoveArmTrajectory(ByteBuffer _bb) { return getRootAsMoveArmTrajectory(_bb, new MoveArmTrajectory()); } + public static MoveArmTrajectory getRootAsMoveArmTrajectory(ByteBuffer _bb, MoveArmTrajectory obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public MoveArmTrajectory __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + public JointState traj(int j) { return traj(new JointState(), j); } + public JointState traj(JointState obj, int j) { int o = __offset(4); return o != 0 ? obj.__assign(__indirect(__vector(o) + j * 4), bb) : null; } + public int trajLength() { int o = __offset(4); return o != 0 ? __vector_len(o) : 0; } + + public static int createMoveArmTrajectory(FlatBufferBuilder builder, + int trajOffset) { + builder.startObject(1); + MoveArmTrajectory.addTraj(builder, trajOffset); + return MoveArmTrajectory.endMoveArmTrajectory(builder); + } + + public static void startMoveArmTrajectory(FlatBufferBuilder builder) { builder.startObject(1); } + public static void addTraj(FlatBufferBuilder builder, int trajOffset) { builder.addOffset(0, trajOffset, 0); } + public static int createTrajVector(FlatBufferBuilder builder, int[] data) { builder.startVector(4, data.length, 4); for (int i = data.length - 1; i >= 0; i--) builder.addOffset(data[i]); return builder.endVector(); } + public static void startTrajVector(FlatBufferBuilder builder, int numElems) { builder.startVector(4, numElems, 4); } + public static int endMoveArmTrajectory(FlatBufferBuilder builder) { + int o = builder.endObject(); + return o; + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/MoveArmTrajectory.py b/include/grl/flatbuffer/grl/flatbuffer/MoveArmTrajectory.py new file mode 100644 index 0000000..1464f45 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/MoveArmTrajectory.py @@ -0,0 +1,44 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class MoveArmTrajectory(object): + __slots__ = ['_tab'] + + @classmethod + def GetRootAsMoveArmTrajectory(cls, buf, offset): + n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) + x = MoveArmTrajectory() + x.Init(buf, n + offset) + return x + + # MoveArmTrajectory + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + + # MoveArmTrajectory + def Traj(self, j): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + x = self._tab.Vector(o) + x += flatbuffers.number_types.UOffsetTFlags.py_type(j) * 4 + x = self._tab.Indirect(x) + from .JointState import JointState + obj = JointState() + obj.Init(self._tab.Bytes, x) + return obj + return None + + # MoveArmTrajectory + def TrajLength(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + return self._tab.VectorLen(o) + return 0 + +def MoveArmTrajectoryStart(builder): builder.StartObject(1) +def MoveArmTrajectoryAddTraj(builder, traj): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(traj), 0) +def MoveArmTrajectoryStartTrajVector(builder, numElems): return builder.StartVector(4, numElems, 4) +def MoveArmTrajectoryEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/PauseArm.java b/include/grl/flatbuffer/grl/flatbuffer/PauseArm.java new file mode 100644 index 0000000..ccec622 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/PauseArm.java @@ -0,0 +1,24 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class PauseArm extends Table { + public static PauseArm getRootAsPauseArm(ByteBuffer _bb) { return getRootAsPauseArm(_bb, new PauseArm()); } + public static PauseArm getRootAsPauseArm(ByteBuffer _bb, PauseArm obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public PauseArm __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + + public static void startPauseArm(FlatBufferBuilder builder) { builder.startObject(0); } + public static int endPauseArm(FlatBufferBuilder builder) { + int o = builder.endObject(); + return o; + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/PauseArm.py b/include/grl/flatbuffer/grl/flatbuffer/PauseArm.py new file mode 100644 index 0000000..a3415d1 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/PauseArm.py @@ -0,0 +1,22 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class PauseArm(object): + __slots__ = ['_tab'] + + @classmethod + def GetRootAsPauseArm(cls, buf, offset): + n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) + x = PauseArm() + x.Init(buf, n + offset) + return x + + # PauseArm + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + +def PauseArmStart(builder): builder.StartObject(0) +def PauseArmEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/Pose.java b/include/grl/flatbuffer/grl/flatbuffer/Pose.java new file mode 100644 index 0000000..90f2736 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/Pose.java @@ -0,0 +1,34 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class Pose extends Struct { + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public Pose __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + public Vector3d position() { return position(new Vector3d()); } + public Vector3d position(Vector3d obj) { return obj.__assign(bb_pos + 0, bb); } + public Quaternion orientation() { return orientation(new Quaternion()); } + public Quaternion orientation(Quaternion obj) { return obj.__assign(bb_pos + 24, bb); } + + public static int createPose(FlatBufferBuilder builder, double position_x, double position_y, double position_z, double orientation_x, double orientation_y, double orientation_z, double orientation_w) { + builder.prep(8, 56); + builder.prep(8, 32); + builder.putDouble(orientation_w); + builder.putDouble(orientation_z); + builder.putDouble(orientation_y); + builder.putDouble(orientation_x); + builder.prep(8, 24); + builder.putDouble(position_z); + builder.putDouble(position_y); + builder.putDouble(position_x); + return builder.offset(); + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/Pose.py b/include/grl/flatbuffer/grl/flatbuffer/Pose.py new file mode 100644 index 0000000..10e7e23 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/Pose.py @@ -0,0 +1,36 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class Pose(object): + __slots__ = ['_tab'] + + # Pose + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + + # Pose + def Position(self, obj): + obj.Init(self._tab.Bytes, self._tab.Pos + 0) + return obj + + # Pose + def Orientation(self, obj): + obj.Init(self._tab.Bytes, self._tab.Pos + 24) + return obj + + +def CreatePose(builder, position_x, position_y, position_z, orientation_x, orientation_y, orientation_z, orientation_w): + builder.Prep(8, 56) + builder.Prep(8, 32) + builder.PrependFloat64(orientation_w) + builder.PrependFloat64(orientation_z) + builder.PrependFloat64(orientation_y) + builder.PrependFloat64(orientation_x) + builder.Prep(8, 24) + builder.PrependFloat64(position_z) + builder.PrependFloat64(position_y) + builder.PrependFloat64(position_x) + return builder.Offset() diff --git a/include/grl/flatbuffer/grl/flatbuffer/ProcessData.java b/include/grl/flatbuffer/grl/flatbuffer/ProcessData.java new file mode 100644 index 0000000..ae0bb67 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/ProcessData.java @@ -0,0 +1,97 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +/** + * "ProcessData" is a field that appears + * on your physical kuka tablet. + * This message allows you to update these + * fields on the tablet yourself. + */ +public final class ProcessData extends Table { + public static ProcessData getRootAsProcessData(ByteBuffer _bb) { return getRootAsProcessData(_bb, new ProcessData()); } + public static ProcessData getRootAsProcessData(ByteBuffer _bb, ProcessData obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public ProcessData __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + public String dataType() { int o = __offset(4); return o != 0 ? __string(o + bb_pos) : null; } + public ByteBuffer dataTypeAsByteBuffer() { return __vector_as_bytebuffer(4, 1); } + public ByteBuffer dataTypeInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 4, 1); } + public String defaultValue() { int o = __offset(6); return o != 0 ? __string(o + bb_pos) : null; } + public ByteBuffer defaultValueAsByteBuffer() { return __vector_as_bytebuffer(6, 1); } + public ByteBuffer defaultValueInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 6, 1); } + public String displayName() { int o = __offset(8); return o != 0 ? __string(o + bb_pos) : null; } + public ByteBuffer displayNameAsByteBuffer() { return __vector_as_bytebuffer(8, 1); } + public ByteBuffer displayNameInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 8, 1); } + public String id() { int o = __offset(10); return o != 0 ? __string(o + bb_pos) : null; } + public ByteBuffer idAsByteBuffer() { return __vector_as_bytebuffer(10, 1); } + public ByteBuffer idInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 10, 1); } + public String min() { int o = __offset(12); return o != 0 ? __string(o + bb_pos) : null; } + public ByteBuffer minAsByteBuffer() { return __vector_as_bytebuffer(12, 1); } + public ByteBuffer minInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 12, 1); } + public String max() { int o = __offset(14); return o != 0 ? __string(o + bb_pos) : null; } + public ByteBuffer maxAsByteBuffer() { return __vector_as_bytebuffer(14, 1); } + public ByteBuffer maxInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 14, 1); } + public String unit() { int o = __offset(16); return o != 0 ? __string(o + bb_pos) : null; } + public ByteBuffer unitAsByteBuffer() { return __vector_as_bytebuffer(16, 1); } + public ByteBuffer unitInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 16, 1); } + public String value() { int o = __offset(18); return o != 0 ? __string(o + bb_pos) : null; } + public ByteBuffer valueAsByteBuffer() { return __vector_as_bytebuffer(18, 1); } + public ByteBuffer valueInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 18, 1); } + /** + * should the data be removed completely? + */ + public boolean shouldRemove() { int o = __offset(20); return o != 0 ? 0!=bb.get(o + bb_pos) : false; } + /** + * should the data be updated to these values? + */ + public boolean shouldUpdate() { int o = __offset(22); return o != 0 ? 0!=bb.get(o + bb_pos) : false; } + + public static int createProcessData(FlatBufferBuilder builder, + int dataTypeOffset, + int defaultValueOffset, + int displayNameOffset, + int idOffset, + int minOffset, + int maxOffset, + int unitOffset, + int valueOffset, + boolean shouldRemove, + boolean shouldUpdate) { + builder.startObject(10); + ProcessData.addValue(builder, valueOffset); + ProcessData.addUnit(builder, unitOffset); + ProcessData.addMax(builder, maxOffset); + ProcessData.addMin(builder, minOffset); + ProcessData.addId(builder, idOffset); + ProcessData.addDisplayName(builder, displayNameOffset); + ProcessData.addDefaultValue(builder, defaultValueOffset); + ProcessData.addDataType(builder, dataTypeOffset); + ProcessData.addShouldUpdate(builder, shouldUpdate); + ProcessData.addShouldRemove(builder, shouldRemove); + return ProcessData.endProcessData(builder); + } + + public static void startProcessData(FlatBufferBuilder builder) { builder.startObject(10); } + public static void addDataType(FlatBufferBuilder builder, int dataTypeOffset) { builder.addOffset(0, dataTypeOffset, 0); } + public static void addDefaultValue(FlatBufferBuilder builder, int defaultValueOffset) { builder.addOffset(1, defaultValueOffset, 0); } + public static void addDisplayName(FlatBufferBuilder builder, int displayNameOffset) { builder.addOffset(2, displayNameOffset, 0); } + public static void addId(FlatBufferBuilder builder, int idOffset) { builder.addOffset(3, idOffset, 0); } + public static void addMin(FlatBufferBuilder builder, int minOffset) { builder.addOffset(4, minOffset, 0); } + public static void addMax(FlatBufferBuilder builder, int maxOffset) { builder.addOffset(5, maxOffset, 0); } + public static void addUnit(FlatBufferBuilder builder, int unitOffset) { builder.addOffset(6, unitOffset, 0); } + public static void addValue(FlatBufferBuilder builder, int valueOffset) { builder.addOffset(7, valueOffset, 0); } + public static void addShouldRemove(FlatBufferBuilder builder, boolean shouldRemove) { builder.addBoolean(8, shouldRemove, false); } + public static void addShouldUpdate(FlatBufferBuilder builder, boolean shouldUpdate) { builder.addBoolean(9, shouldUpdate, false); } + public static int endProcessData(FlatBufferBuilder builder) { + int o = builder.endObject(); + return o; + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/ProcessData.py b/include/grl/flatbuffer/grl/flatbuffer/ProcessData.py new file mode 100644 index 0000000..3101a94 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/ProcessData.py @@ -0,0 +1,108 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +# /// "ProcessData" is a field that appears +# /// on your physical kuka tablet. +# /// This message allows you to update these +# /// fields on the tablet yourself. +class ProcessData(object): + __slots__ = ['_tab'] + + @classmethod + def GetRootAsProcessData(cls, buf, offset): + n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) + x = ProcessData() + x.Init(buf, n + offset) + return x + + # ProcessData + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + + # ProcessData + def DataType(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + return self._tab.String(o + self._tab.Pos) + return bytes() + + # ProcessData + def DefaultValue(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) + if o != 0: + return self._tab.String(o + self._tab.Pos) + return bytes() + + # ProcessData + def DisplayName(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) + if o != 0: + return self._tab.String(o + self._tab.Pos) + return bytes() + + # ProcessData + def Id(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) + if o != 0: + return self._tab.String(o + self._tab.Pos) + return bytes() + + # ProcessData + def Min(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(12)) + if o != 0: + return self._tab.String(o + self._tab.Pos) + return bytes() + + # ProcessData + def Max(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(14)) + if o != 0: + return self._tab.String(o + self._tab.Pos) + return bytes() + + # ProcessData + def Unit(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(16)) + if o != 0: + return self._tab.String(o + self._tab.Pos) + return bytes() + + # ProcessData + def Value(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(18)) + if o != 0: + return self._tab.String(o + self._tab.Pos) + return bytes() + +# /// should the data be removed completely? + # ProcessData + def ShouldRemove(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(20)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.BoolFlags, o + self._tab.Pos) + return 0 + +# /// should the data be updated to these values? + # ProcessData + def ShouldUpdate(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(22)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.BoolFlags, o + self._tab.Pos) + return 0 + +def ProcessDataStart(builder): builder.StartObject(10) +def ProcessDataAddDataType(builder, dataType): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(dataType), 0) +def ProcessDataAddDefaultValue(builder, defaultValue): builder.PrependUOffsetTRelativeSlot(1, flatbuffers.number_types.UOffsetTFlags.py_type(defaultValue), 0) +def ProcessDataAddDisplayName(builder, displayName): builder.PrependUOffsetTRelativeSlot(2, flatbuffers.number_types.UOffsetTFlags.py_type(displayName), 0) +def ProcessDataAddId(builder, id): builder.PrependUOffsetTRelativeSlot(3, flatbuffers.number_types.UOffsetTFlags.py_type(id), 0) +def ProcessDataAddMin(builder, min): builder.PrependUOffsetTRelativeSlot(4, flatbuffers.number_types.UOffsetTFlags.py_type(min), 0) +def ProcessDataAddMax(builder, max): builder.PrependUOffsetTRelativeSlot(5, flatbuffers.number_types.UOffsetTFlags.py_type(max), 0) +def ProcessDataAddUnit(builder, unit): builder.PrependUOffsetTRelativeSlot(6, flatbuffers.number_types.UOffsetTFlags.py_type(unit), 0) +def ProcessDataAddValue(builder, value): builder.PrependUOffsetTRelativeSlot(7, flatbuffers.number_types.UOffsetTFlags.py_type(value), 0) +def ProcessDataAddShouldRemove(builder, shouldRemove): builder.PrependBoolSlot(8, shouldRemove, 0) +def ProcessDataAddShouldUpdate(builder, shouldUpdate): builder.PrependBoolSlot(9, shouldUpdate, 0) +def ProcessDataEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/Quaternion.java b/include/grl/flatbuffer/grl/flatbuffer/Quaternion.java new file mode 100644 index 0000000..d6b4be0 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/Quaternion.java @@ -0,0 +1,29 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class Quaternion extends Struct { + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public Quaternion __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + public double x() { return bb.getDouble(bb_pos + 0); } + public double y() { return bb.getDouble(bb_pos + 8); } + public double z() { return bb.getDouble(bb_pos + 16); } + public double w() { return bb.getDouble(bb_pos + 24); } + + public static int createQuaternion(FlatBufferBuilder builder, double x, double y, double z, double w) { + builder.prep(8, 32); + builder.putDouble(w); + builder.putDouble(z); + builder.putDouble(y); + builder.putDouble(x); + return builder.offset(); + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/Quaternion.py b/include/grl/flatbuffer/grl/flatbuffer/Quaternion.py new file mode 100644 index 0000000..54f6b1f --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/Quaternion.py @@ -0,0 +1,29 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class Quaternion(object): + __slots__ = ['_tab'] + + # Quaternion + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + + # Quaternion + def X(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(0)) + # Quaternion + def Y(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(8)) + # Quaternion + def Z(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(16)) + # Quaternion + def W(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(24)) + +def CreateQuaternion(builder, x, y, z, w): + builder.Prep(8, 32) + builder.PrependFloat64(w) + builder.PrependFloat64(z) + builder.PrependFloat64(y) + builder.PrependFloat64(x) + return builder.Offset() diff --git a/include/grl/flatbuffer/grl/flatbuffer/ShutdownArm.java b/include/grl/flatbuffer/grl/flatbuffer/ShutdownArm.java new file mode 100644 index 0000000..675fecd --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/ShutdownArm.java @@ -0,0 +1,24 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class ShutdownArm extends Table { + public static ShutdownArm getRootAsShutdownArm(ByteBuffer _bb) { return getRootAsShutdownArm(_bb, new ShutdownArm()); } + public static ShutdownArm getRootAsShutdownArm(ByteBuffer _bb, ShutdownArm obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public ShutdownArm __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + + public static void startShutdownArm(FlatBufferBuilder builder) { builder.startObject(0); } + public static int endShutdownArm(FlatBufferBuilder builder) { + int o = builder.endObject(); + return o; + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/ShutdownArm.py b/include/grl/flatbuffer/grl/flatbuffer/ShutdownArm.py new file mode 100644 index 0000000..c96dbd1 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/ShutdownArm.py @@ -0,0 +1,22 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class ShutdownArm(object): + __slots__ = ['_tab'] + + @classmethod + def GetRootAsShutdownArm(cls, buf, offset): + n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) + x = ShutdownArm() + x.Init(buf, n + offset) + return x + + # ShutdownArm + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + +def ShutdownArmStart(builder): builder.StartObject(0) +def ShutdownArmEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/SmartServo.java b/include/grl/flatbuffer/grl/flatbuffer/SmartServo.java new file mode 100644 index 0000000..e0204c6 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/SmartServo.java @@ -0,0 +1,61 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class SmartServo extends Table { + public static SmartServo getRootAsSmartServo(ByteBuffer _bb) { return getRootAsSmartServo(_bb, new SmartServo()); } + public static SmartServo getRootAsSmartServo(ByteBuffer _bb, SmartServo obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public SmartServo __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + /** + * normalized joint accelerations from 0 to 1 relative to system capabilities + */ + public double jointAccelerationRel(int j) { int o = __offset(4); return o != 0 ? bb.getDouble(__vector(o) + j * 8) : 0; } + public int jointAccelerationRelLength() { int o = __offset(4); return o != 0 ? __vector_len(o) : 0; } + public ByteBuffer jointAccelerationRelAsByteBuffer() { return __vector_as_bytebuffer(4, 8); } + public ByteBuffer jointAccelerationRelInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 4, 8); } + /** + * normalized joint velocity from 0 to 1 relative to system capabilities + */ + public double jointVelocityRel(int j) { int o = __offset(6); return o != 0 ? bb.getDouble(__vector(o) + j * 8) : 0; } + public int jointVelocityRelLength() { int o = __offset(6); return o != 0 ? __vector_len(o) : 0; } + public ByteBuffer jointVelocityRelAsByteBuffer() { return __vector_as_bytebuffer(6, 8); } + public ByteBuffer jointVelocityRelInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 6, 8); } + public boolean updateMinimumTrajectoryExecutionTime() { int o = __offset(8); return o != 0 ? 0!=bb.get(o + bb_pos) : false; } + public double minimumTrajectoryExecutionTime() { int o = __offset(10); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } + + public static int createSmartServo(FlatBufferBuilder builder, + int jointAccelerationRelOffset, + int jointVelocityRelOffset, + boolean updateMinimumTrajectoryExecutionTime, + double minimumTrajectoryExecutionTime) { + builder.startObject(4); + SmartServo.addMinimumTrajectoryExecutionTime(builder, minimumTrajectoryExecutionTime); + SmartServo.addJointVelocityRel(builder, jointVelocityRelOffset); + SmartServo.addJointAccelerationRel(builder, jointAccelerationRelOffset); + SmartServo.addUpdateMinimumTrajectoryExecutionTime(builder, updateMinimumTrajectoryExecutionTime); + return SmartServo.endSmartServo(builder); + } + + public static void startSmartServo(FlatBufferBuilder builder) { builder.startObject(4); } + public static void addJointAccelerationRel(FlatBufferBuilder builder, int jointAccelerationRelOffset) { builder.addOffset(0, jointAccelerationRelOffset, 0); } + public static int createJointAccelerationRelVector(FlatBufferBuilder builder, double[] data) { builder.startVector(8, data.length, 8); for (int i = data.length - 1; i >= 0; i--) builder.addDouble(data[i]); return builder.endVector(); } + public static void startJointAccelerationRelVector(FlatBufferBuilder builder, int numElems) { builder.startVector(8, numElems, 8); } + public static void addJointVelocityRel(FlatBufferBuilder builder, int jointVelocityRelOffset) { builder.addOffset(1, jointVelocityRelOffset, 0); } + public static int createJointVelocityRelVector(FlatBufferBuilder builder, double[] data) { builder.startVector(8, data.length, 8); for (int i = data.length - 1; i >= 0; i--) builder.addDouble(data[i]); return builder.endVector(); } + public static void startJointVelocityRelVector(FlatBufferBuilder builder, int numElems) { builder.startVector(8, numElems, 8); } + public static void addUpdateMinimumTrajectoryExecutionTime(FlatBufferBuilder builder, boolean updateMinimumTrajectoryExecutionTime) { builder.addBoolean(2, updateMinimumTrajectoryExecutionTime, false); } + public static void addMinimumTrajectoryExecutionTime(FlatBufferBuilder builder, double minimumTrajectoryExecutionTime) { builder.addDouble(3, minimumTrajectoryExecutionTime, 0.0); } + public static int endSmartServo(FlatBufferBuilder builder) { + int o = builder.endObject(); + return o; + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/SmartServo.py b/include/grl/flatbuffer/grl/flatbuffer/SmartServo.py new file mode 100644 index 0000000..6c832d2 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/SmartServo.py @@ -0,0 +1,88 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class SmartServo(object): + __slots__ = ['_tab'] + + @classmethod + def GetRootAsSmartServo(cls, buf, offset): + n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) + x = SmartServo() + x.Init(buf, n + offset) + return x + + # SmartServo + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + +# /// normalized joint accelerations from 0 to 1 relative to system capabilities + # SmartServo + def JointAccelerationRel(self, j): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + a = self._tab.Vector(o) + return self._tab.Get(flatbuffers.number_types.Float64Flags, a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 8)) + return 0 + + # SmartServo + def JointAccelerationRelAsNumpy(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + return self._tab.GetVectorAsNumpy(flatbuffers.number_types.Float64Flags, o) + return 0 + + # SmartServo + def JointAccelerationRelLength(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + return self._tab.VectorLen(o) + return 0 + +# /// normalized joint velocity from 0 to 1 relative to system capabilities + # SmartServo + def JointVelocityRel(self, j): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) + if o != 0: + a = self._tab.Vector(o) + return self._tab.Get(flatbuffers.number_types.Float64Flags, a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 8)) + return 0 + + # SmartServo + def JointVelocityRelAsNumpy(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) + if o != 0: + return self._tab.GetVectorAsNumpy(flatbuffers.number_types.Float64Flags, o) + return 0 + + # SmartServo + def JointVelocityRelLength(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) + if o != 0: + return self._tab.VectorLen(o) + return 0 + + # SmartServo + def UpdateMinimumTrajectoryExecutionTime(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.BoolFlags, o + self._tab.Pos) + return 0 + + # SmartServo + def MinimumTrajectoryExecutionTime(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) + return 0.0 + +def SmartServoStart(builder): builder.StartObject(4) +def SmartServoAddJointAccelerationRel(builder, jointAccelerationRel): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(jointAccelerationRel), 0) +def SmartServoStartJointAccelerationRelVector(builder, numElems): return builder.StartVector(8, numElems, 8) +def SmartServoAddJointVelocityRel(builder, jointVelocityRel): builder.PrependUOffsetTRelativeSlot(1, flatbuffers.number_types.UOffsetTFlags.py_type(jointVelocityRel), 0) +def SmartServoStartJointVelocityRelVector(builder, numElems): return builder.StartVector(8, numElems, 8) +def SmartServoAddUpdateMinimumTrajectoryExecutionTime(builder, updateMinimumTrajectoryExecutionTime): builder.PrependBoolSlot(2, updateMinimumTrajectoryExecutionTime, 0) +def SmartServoAddMinimumTrajectoryExecutionTime(builder, minimumTrajectoryExecutionTime): builder.PrependFloat64Slot(3, minimumTrajectoryExecutionTime, 0.0) +def SmartServoEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/StartArm.java b/include/grl/flatbuffer/grl/flatbuffer/StartArm.java new file mode 100644 index 0000000..2a0b60b --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/StartArm.java @@ -0,0 +1,24 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class StartArm extends Table { + public static StartArm getRootAsStartArm(ByteBuffer _bb) { return getRootAsStartArm(_bb, new StartArm()); } + public static StartArm getRootAsStartArm(ByteBuffer _bb, StartArm obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public StartArm __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + + public static void startStartArm(FlatBufferBuilder builder) { builder.startObject(0); } + public static int endStartArm(FlatBufferBuilder builder) { + int o = builder.endObject(); + return o; + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/StartArm.py b/include/grl/flatbuffer/grl/flatbuffer/StartArm.py new file mode 100644 index 0000000..23446e8 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/StartArm.py @@ -0,0 +1,22 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class StartArm(object): + __slots__ = ['_tab'] + + @classmethod + def GetRootAsStartArm(cls, buf, offset): + n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) + x = StartArm() + x.Init(buf, n + offset) + return x + + # StartArm + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + +def StartArmStart(builder): builder.StartObject(0) +def StartArmEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/StopArm.java b/include/grl/flatbuffer/grl/flatbuffer/StopArm.java new file mode 100644 index 0000000..ad15226 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/StopArm.java @@ -0,0 +1,24 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class StopArm extends Table { + public static StopArm getRootAsStopArm(ByteBuffer _bb) { return getRootAsStopArm(_bb, new StopArm()); } + public static StopArm getRootAsStopArm(ByteBuffer _bb, StopArm obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public StopArm __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + + public static void startStopArm(FlatBufferBuilder builder) { builder.startObject(0); } + public static int endStopArm(FlatBufferBuilder builder) { + int o = builder.endObject(); + return o; + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/StopArm.py b/include/grl/flatbuffer/grl/flatbuffer/StopArm.py new file mode 100644 index 0000000..8f7d1e4 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/StopArm.py @@ -0,0 +1,22 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class StopArm(object): + __slots__ = ['_tab'] + + @classmethod + def GetRootAsStopArm(cls, buf, offset): + n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) + x = StopArm() + x.Init(buf, n + offset) + return x + + # StopArm + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + +def StopArmStart(builder): builder.StartObject(0) +def StopArmEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/TeachArm.java b/include/grl/flatbuffer/grl/flatbuffer/TeachArm.java new file mode 100644 index 0000000..0c75807 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/TeachArm.java @@ -0,0 +1,24 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class TeachArm extends Table { + public static TeachArm getRootAsTeachArm(ByteBuffer _bb) { return getRootAsTeachArm(_bb, new TeachArm()); } + public static TeachArm getRootAsTeachArm(ByteBuffer _bb, TeachArm obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public TeachArm __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + + public static void startTeachArm(FlatBufferBuilder builder) { builder.startObject(0); } + public static int endTeachArm(FlatBufferBuilder builder) { + int o = builder.endObject(); + return o; + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/TeachArm.py b/include/grl/flatbuffer/grl/flatbuffer/TeachArm.py new file mode 100644 index 0000000..a8818fd --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/TeachArm.py @@ -0,0 +1,22 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class TeachArm(object): + __slots__ = ['_tab'] + + @classmethod + def GetRootAsTeachArm(cls, buf, offset): + n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) + x = TeachArm() + x.Init(buf, n + offset) + return x + + # TeachArm + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + +def TeachArmStart(builder): builder.StartObject(0) +def TeachArmEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/Time.java b/include/grl/flatbuffer/grl/flatbuffer/Time.java new file mode 100644 index 0000000..d92b522 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/Time.java @@ -0,0 +1,23 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class Time extends Struct { + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public Time __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + public double sec() { return bb.getDouble(bb_pos + 0); } + + public static int createTime(FlatBufferBuilder builder, double sec) { + builder.prep(8, 8); + builder.putDouble(sec); + return builder.offset(); + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/Time.py b/include/grl/flatbuffer/grl/flatbuffer/Time.py new file mode 100644 index 0000000..46b015c --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/Time.py @@ -0,0 +1,20 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class Time(object): + __slots__ = ['_tab'] + + # Time + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + + # Time + def Sec(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(0)) + +def CreateTime(builder, sec): + builder.Prep(8, 8) + builder.PrependFloat64(sec) + return builder.Offset() diff --git a/include/grl/flatbuffer/grl/flatbuffer/TimeEvent.java b/include/grl/flatbuffer/grl/flatbuffer/TimeEvent.java new file mode 100644 index 0000000..b76eedd --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/TimeEvent.java @@ -0,0 +1,112 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +/** + * Note that all of these time entries are + * longs with a minimum step of 100ns, + * see google cartographer's cartographer::common::time + */ +public final class TimeEvent extends Table { + public static TimeEvent getRootAsTimeEvent(ByteBuffer _bb) { return getRootAsTimeEvent(_bb, new TimeEvent()); } + public static TimeEvent getRootAsTimeEvent(ByteBuffer _bb, TimeEvent obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public TimeEvent __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + /** + * Identifying string for this time stamped data topic + * something like "/opticaltracker/00000000/frame" where + * 00000000 is the serial number of the optical tracker. + */ + public String eventName() { int o = __offset(4); return o != 0 ? __string(o + bb_pos) : null; } + public ByteBuffer eventNameAsByteBuffer() { return __vector_as_bytebuffer(4, 1); } + public ByteBuffer eventNameInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 4, 1); } + /** + * The time just before a data update request is made + */ + public long localRequestTime() { int o = __offset(6); return o != 0 ? bb.getLong(o + bb_pos) : 0L; } + /** + * Identifying string for the clock used to drive the device + * something like "/opticaltracker/00000000/clock" + * if it is the clock internal to a sensor like an optical tracker + */ + public String deviceClockId() { int o = __offset(8); return o != 0 ? __string(o + bb_pos) : null; } + public ByteBuffer deviceClockIdAsByteBuffer() { return __vector_as_bytebuffer(8, 1); } + public ByteBuffer deviceClockIdInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 8, 1); } + /** + * The time provided by the device specified by device_clock_id + */ + public long deviceTime() { int o = __offset(10); return o != 0 ? bb.getLong(o + bb_pos) : 0L; } + /** + * Identifying string for the clock used to drive the device + * or "/control_computer/clock/steady" if the device has no clock + * and the time is the desktop computer + * running the steady clock (vs clocks which might change time) + */ + public String localClockId() { int o = __offset(12); return o != 0 ? __string(o + bb_pos) : null; } + public ByteBuffer localClockIdAsByteBuffer() { return __vector_as_bytebuffer(12, 1); } + public ByteBuffer localClockIdInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 12, 1); } + /** + * The time at which the data was received + */ + public long localReceiveTime() { int o = __offset(14); return o != 0 ? bb.getLong(o + bb_pos) : 0L; } + /** + * The corrected local time which represents when the sensor + * data was actually captured. + */ + public long correctedLocalTime() { int o = __offset(16); return o != 0 ? bb.getLong(o + bb_pos) : 0L; } + /** + * Estimated duration of the skew between the device clock + * and the local time clock + */ + public long clockSkew() { int o = __offset(18); return o != 0 ? bb.getLong(o + bb_pos) : 0L; } + /** + * The minimum expected delay in transporting the data request + */ + public long minTransportDelay() { int o = __offset(20); return o != 0 ? bb.getLong(o + bb_pos) : 0L; } + + public static int createTimeEvent(FlatBufferBuilder builder, + int event_nameOffset, + long local_request_time, + int device_clock_idOffset, + long device_time, + int local_clock_idOffset, + long local_receive_time, + long corrected_local_time, + long clock_skew, + long min_transport_delay) { + builder.startObject(9); + TimeEvent.addMinTransportDelay(builder, min_transport_delay); + TimeEvent.addClockSkew(builder, clock_skew); + TimeEvent.addCorrectedLocalTime(builder, corrected_local_time); + TimeEvent.addLocalReceiveTime(builder, local_receive_time); + TimeEvent.addDeviceTime(builder, device_time); + TimeEvent.addLocalRequestTime(builder, local_request_time); + TimeEvent.addLocalClockId(builder, local_clock_idOffset); + TimeEvent.addDeviceClockId(builder, device_clock_idOffset); + TimeEvent.addEventName(builder, event_nameOffset); + return TimeEvent.endTimeEvent(builder); + } + + public static void startTimeEvent(FlatBufferBuilder builder) { builder.startObject(9); } + public static void addEventName(FlatBufferBuilder builder, int eventNameOffset) { builder.addOffset(0, eventNameOffset, 0); } + public static void addLocalRequestTime(FlatBufferBuilder builder, long localRequestTime) { builder.addLong(1, localRequestTime, 0L); } + public static void addDeviceClockId(FlatBufferBuilder builder, int deviceClockIdOffset) { builder.addOffset(2, deviceClockIdOffset, 0); } + public static void addDeviceTime(FlatBufferBuilder builder, long deviceTime) { builder.addLong(3, deviceTime, 0L); } + public static void addLocalClockId(FlatBufferBuilder builder, int localClockIdOffset) { builder.addOffset(4, localClockIdOffset, 0); } + public static void addLocalReceiveTime(FlatBufferBuilder builder, long localReceiveTime) { builder.addLong(5, localReceiveTime, 0L); } + public static void addCorrectedLocalTime(FlatBufferBuilder builder, long correctedLocalTime) { builder.addLong(6, correctedLocalTime, 0L); } + public static void addClockSkew(FlatBufferBuilder builder, long clockSkew) { builder.addLong(7, clockSkew, 0L); } + public static void addMinTransportDelay(FlatBufferBuilder builder, long minTransportDelay) { builder.addLong(8, minTransportDelay, 0L); } + public static int endTimeEvent(FlatBufferBuilder builder) { + int o = builder.endObject(); + return o; + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/TimeEvent.py b/include/grl/flatbuffer/grl/flatbuffer/TimeEvent.py new file mode 100644 index 0000000..0a764dc --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/TimeEvent.py @@ -0,0 +1,115 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +# /// Note that all of these time entries are +# /// longs with a minimum step of 100ns, +# /// see google cartographer's cartographer::common::time +class TimeEvent(object): + __slots__ = ['_tab'] + + @classmethod + def GetRootAsTimeEvent(cls, buf, offset): + n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) + x = TimeEvent() + x.Init(buf, n + offset) + return x + + # TimeEvent + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + +# /// Identifying string for this time stamped data topic +# /// something like "/opticaltracker/00000000/frame" where +# /// 00000000 is the serial number of the optical tracker. + # TimeEvent + def EventName(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + return self._tab.String(o + self._tab.Pos) + return bytes() + +# /// The time just before a data update request is made + # TimeEvent + def LocalRequestTime(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Int64Flags, o + self._tab.Pos) + return 0 + +# /// Identifying string for the clock used to drive the device +# /// something like "/opticaltracker/00000000/clock" +# /// if it is the clock internal to a sensor like an optical tracker + # TimeEvent + def DeviceClockId(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) + if o != 0: + return self._tab.String(o + self._tab.Pos) + return bytes() + +# /// The time provided by the device specified by device_clock_id + # TimeEvent + def DeviceTime(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Int64Flags, o + self._tab.Pos) + return 0 + +# /// Identifying string for the clock used to drive the device +# /// or "/control_computer/clock/steady" if the device has no clock +# /// and the time is the desktop computer +# /// running the steady clock (vs clocks which might change time) + # TimeEvent + def LocalClockId(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(12)) + if o != 0: + return self._tab.String(o + self._tab.Pos) + return bytes() + +# /// The time at which the data was received + # TimeEvent + def LocalReceiveTime(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(14)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Int64Flags, o + self._tab.Pos) + return 0 + +# /// The corrected local time which represents when the sensor +# /// data was actually captured. + # TimeEvent + def CorrectedLocalTime(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(16)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Int64Flags, o + self._tab.Pos) + return 0 + +# /// Estimated duration of the skew between the device clock +# /// and the local time clock + # TimeEvent + def ClockSkew(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(18)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Int64Flags, o + self._tab.Pos) + return 0 + +# /// The minimum expected delay in transporting the data request + # TimeEvent + def MinTransportDelay(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(20)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Int64Flags, o + self._tab.Pos) + return 0 + +def TimeEventStart(builder): builder.StartObject(9) +def TimeEventAddEventName(builder, eventName): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(eventName), 0) +def TimeEventAddLocalRequestTime(builder, localRequestTime): builder.PrependInt64Slot(1, localRequestTime, 0) +def TimeEventAddDeviceClockId(builder, deviceClockId): builder.PrependUOffsetTRelativeSlot(2, flatbuffers.number_types.UOffsetTFlags.py_type(deviceClockId), 0) +def TimeEventAddDeviceTime(builder, deviceTime): builder.PrependInt64Slot(3, deviceTime, 0) +def TimeEventAddLocalClockId(builder, localClockId): builder.PrependUOffsetTRelativeSlot(4, flatbuffers.number_types.UOffsetTFlags.py_type(localClockId), 0) +def TimeEventAddLocalReceiveTime(builder, localReceiveTime): builder.PrependInt64Slot(5, localReceiveTime, 0) +def TimeEventAddCorrectedLocalTime(builder, correctedLocalTime): builder.PrependInt64Slot(6, correctedLocalTime, 0) +def TimeEventAddClockSkew(builder, clockSkew): builder.PrependInt64Slot(7, clockSkew, 0) +def TimeEventAddMinTransportDelay(builder, minTransportDelay): builder.PrependInt64Slot(8, minTransportDelay, 0) +def TimeEventEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/Vector3d.java b/include/grl/flatbuffer/grl/flatbuffer/Vector3d.java new file mode 100644 index 0000000..8217e81 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/Vector3d.java @@ -0,0 +1,27 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class Vector3d extends Struct { + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public Vector3d __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + public double x() { return bb.getDouble(bb_pos + 0); } + public double y() { return bb.getDouble(bb_pos + 8); } + public double z() { return bb.getDouble(bb_pos + 16); } + + public static int createVector3d(FlatBufferBuilder builder, double x, double y, double z) { + builder.prep(8, 24); + builder.putDouble(z); + builder.putDouble(y); + builder.putDouble(x); + return builder.offset(); + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/Vector3d.py b/include/grl/flatbuffer/grl/flatbuffer/Vector3d.py new file mode 100644 index 0000000..ffd7bf0 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/Vector3d.py @@ -0,0 +1,26 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class Vector3d(object): + __slots__ = ['_tab'] + + # Vector3d + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + + # Vector3d + def X(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(0)) + # Vector3d + def Y(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(8)) + # Vector3d + def Z(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(16)) + +def CreateVector3d(builder, x, y, z): + builder.Prep(8, 24) + builder.PrependFloat64(z) + builder.PrependFloat64(y) + builder.PrependFloat64(x) + return builder.Offset() diff --git a/include/grl/flatbuffer/grl/flatbuffer/VrepControlPoint.java b/include/grl/flatbuffer/grl/flatbuffer/VrepControlPoint.java new file mode 100644 index 0000000..7fbbe7a --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/VrepControlPoint.java @@ -0,0 +1,52 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class VrepControlPoint extends Table { + public static VrepControlPoint getRootAsVrepControlPoint(ByteBuffer _bb) { return getRootAsVrepControlPoint(_bb, new VrepControlPoint()); } + public static VrepControlPoint getRootAsVrepControlPoint(ByteBuffer _bb, VrepControlPoint obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public VrepControlPoint __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + public Vector3d position() { return position(new Vector3d()); } + public Vector3d position(Vector3d obj) { int o = __offset(4); return o != 0 ? obj.__assign(o + bb_pos, bb) : null; } + public EulerXYZd rotation() { return rotation(new EulerXYZd()); } + public EulerXYZd rotation(EulerXYZd obj) { int o = __offset(6); return o != 0 ? obj.__assign(o + bb_pos, bb) : null; } + public double relativeVelocity() { int o = __offset(8); return o != 0 ? bb.getDouble(o + bb_pos) : 1.0; } + public int bezierPointCount() { int o = __offset(10); return o != 0 ? bb.getInt(o + bb_pos) : 1; } + public double interpolationFactor1() { int o = __offset(12); return o != 0 ? bb.getDouble(o + bb_pos) : 0.5; } + public double interpolationFactor2() { int o = __offset(14); return o != 0 ? bb.getDouble(o + bb_pos) : 0.5; } + public double virtualDistance() { int o = __offset(16); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } + public int auxiliaryFlags() { int o = __offset(18); return o != 0 ? bb.getInt(o + bb_pos) : 0; } + public double auxiliaryChannel1() { int o = __offset(20); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } + public double auxiliaryChannel2() { int o = __offset(22); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } + public double auxiliaryChannel3() { int o = __offset(24); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } + public double auxiliaryChannel4() { int o = __offset(26); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } + + public static void startVrepControlPoint(FlatBufferBuilder builder) { builder.startObject(12); } + public static void addPosition(FlatBufferBuilder builder, int positionOffset) { builder.addStruct(0, positionOffset, 0); } + public static void addRotation(FlatBufferBuilder builder, int rotationOffset) { builder.addStruct(1, rotationOffset, 0); } + public static void addRelativeVelocity(FlatBufferBuilder builder, double relativeVelocity) { builder.addDouble(2, relativeVelocity, 1.0); } + public static void addBezierPointCount(FlatBufferBuilder builder, int bezierPointCount) { builder.addInt(3, bezierPointCount, 1); } + public static void addInterpolationFactor1(FlatBufferBuilder builder, double interpolationFactor1) { builder.addDouble(4, interpolationFactor1, 0.5); } + public static void addInterpolationFactor2(FlatBufferBuilder builder, double interpolationFactor2) { builder.addDouble(5, interpolationFactor2, 0.5); } + public static void addVirtualDistance(FlatBufferBuilder builder, double virtualDistance) { builder.addDouble(6, virtualDistance, 0.0); } + public static void addAuxiliaryFlags(FlatBufferBuilder builder, int auxiliaryFlags) { builder.addInt(7, auxiliaryFlags, 0); } + public static void addAuxiliaryChannel1(FlatBufferBuilder builder, double auxiliaryChannel1) { builder.addDouble(8, auxiliaryChannel1, 0.0); } + public static void addAuxiliaryChannel2(FlatBufferBuilder builder, double auxiliaryChannel2) { builder.addDouble(9, auxiliaryChannel2, 0.0); } + public static void addAuxiliaryChannel3(FlatBufferBuilder builder, double auxiliaryChannel3) { builder.addDouble(10, auxiliaryChannel3, 0.0); } + public static void addAuxiliaryChannel4(FlatBufferBuilder builder, double auxiliaryChannel4) { builder.addDouble(11, auxiliaryChannel4, 0.0); } + public static int endVrepControlPoint(FlatBufferBuilder builder) { + int o = builder.endObject(); + return o; + } + public static void finishVrepControlPointBuffer(FlatBufferBuilder builder, int offset) { builder.finish(offset); } + public static void finishSizePrefixedVrepControlPointBuffer(FlatBufferBuilder builder, int offset) { builder.finishSizePrefixed(offset); } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/VrepControlPoint.py b/include/grl/flatbuffer/grl/flatbuffer/VrepControlPoint.py new file mode 100644 index 0000000..58cfa1b --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/VrepControlPoint.py @@ -0,0 +1,126 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class VrepControlPoint(object): + __slots__ = ['_tab'] + + @classmethod + def GetRootAsVrepControlPoint(cls, buf, offset): + n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) + x = VrepControlPoint() + x.Init(buf, n + offset) + return x + + # VrepControlPoint + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + + # VrepControlPoint + def Position(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + x = o + self._tab.Pos + from .Vector3d import Vector3d + obj = Vector3d() + obj.Init(self._tab.Bytes, x) + return obj + return None + + # VrepControlPoint + def Rotation(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) + if o != 0: + x = o + self._tab.Pos + from .EulerXYZd import EulerXYZd + obj = EulerXYZd() + obj.Init(self._tab.Bytes, x) + return obj + return None + + # VrepControlPoint + def RelativeVelocity(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) + return 1.0 + + # VrepControlPoint + def BezierPointCount(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Int32Flags, o + self._tab.Pos) + return 1 + + # VrepControlPoint + def InterpolationFactor1(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(12)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) + return 0.5 + + # VrepControlPoint + def InterpolationFactor2(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(14)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) + return 0.5 + + # VrepControlPoint + def VirtualDistance(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(16)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) + return 0.0 + + # VrepControlPoint + def AuxiliaryFlags(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(18)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Int32Flags, o + self._tab.Pos) + return 0 + + # VrepControlPoint + def AuxiliaryChannel1(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(20)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) + return 0.0 + + # VrepControlPoint + def AuxiliaryChannel2(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(22)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) + return 0.0 + + # VrepControlPoint + def AuxiliaryChannel3(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(24)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) + return 0.0 + + # VrepControlPoint + def AuxiliaryChannel4(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(26)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) + return 0.0 + +def VrepControlPointStart(builder): builder.StartObject(12) +def VrepControlPointAddPosition(builder, position): builder.PrependStructSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(position), 0) +def VrepControlPointAddRotation(builder, rotation): builder.PrependStructSlot(1, flatbuffers.number_types.UOffsetTFlags.py_type(rotation), 0) +def VrepControlPointAddRelativeVelocity(builder, relativeVelocity): builder.PrependFloat64Slot(2, relativeVelocity, 1.0) +def VrepControlPointAddBezierPointCount(builder, bezierPointCount): builder.PrependInt32Slot(3, bezierPointCount, 1) +def VrepControlPointAddInterpolationFactor1(builder, interpolationFactor1): builder.PrependFloat64Slot(4, interpolationFactor1, 0.5) +def VrepControlPointAddInterpolationFactor2(builder, interpolationFactor2): builder.PrependFloat64Slot(5, interpolationFactor2, 0.5) +def VrepControlPointAddVirtualDistance(builder, virtualDistance): builder.PrependFloat64Slot(6, virtualDistance, 0.0) +def VrepControlPointAddAuxiliaryFlags(builder, auxiliaryFlags): builder.PrependInt32Slot(7, auxiliaryFlags, 0) +def VrepControlPointAddAuxiliaryChannel1(builder, auxiliaryChannel1): builder.PrependFloat64Slot(8, auxiliaryChannel1, 0.0) +def VrepControlPointAddAuxiliaryChannel2(builder, auxiliaryChannel2): builder.PrependFloat64Slot(9, auxiliaryChannel2, 0.0) +def VrepControlPointAddAuxiliaryChannel3(builder, auxiliaryChannel3): builder.PrependFloat64Slot(10, auxiliaryChannel3, 0.0) +def VrepControlPointAddAuxiliaryChannel4(builder, auxiliaryChannel4): builder.PrependFloat64Slot(11, auxiliaryChannel4, 0.0) +def VrepControlPointEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/VrepPath.java b/include/grl/flatbuffer/grl/flatbuffer/VrepPath.java new file mode 100644 index 0000000..ae3a459 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/VrepPath.java @@ -0,0 +1,39 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class VrepPath extends Table { + public static VrepPath getRootAsVrepPath(ByteBuffer _bb) { return getRootAsVrepPath(_bb, new VrepPath()); } + public static VrepPath getRootAsVrepPath(ByteBuffer _bb, VrepPath obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public VrepPath __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + public VrepControlPoint controlPoints(int j) { return controlPoints(new VrepControlPoint(), j); } + public VrepControlPoint controlPoints(VrepControlPoint obj, int j) { int o = __offset(4); return o != 0 ? obj.__assign(__indirect(__vector(o) + j * 4), bb) : null; } + public int controlPointsLength() { int o = __offset(4); return o != 0 ? __vector_len(o) : 0; } + + public static int createVrepPath(FlatBufferBuilder builder, + int controlPointsOffset) { + builder.startObject(1); + VrepPath.addControlPoints(builder, controlPointsOffset); + return VrepPath.endVrepPath(builder); + } + + public static void startVrepPath(FlatBufferBuilder builder) { builder.startObject(1); } + public static void addControlPoints(FlatBufferBuilder builder, int controlPointsOffset) { builder.addOffset(0, controlPointsOffset, 0); } + public static int createControlPointsVector(FlatBufferBuilder builder, int[] data) { builder.startVector(4, data.length, 4); for (int i = data.length - 1; i >= 0; i--) builder.addOffset(data[i]); return builder.endVector(); } + public static void startControlPointsVector(FlatBufferBuilder builder, int numElems) { builder.startVector(4, numElems, 4); } + public static int endVrepPath(FlatBufferBuilder builder) { + int o = builder.endObject(); + return o; + } + public static void finishVrepPathBuffer(FlatBufferBuilder builder, int offset) { builder.finish(offset); } + public static void finishSizePrefixedVrepPathBuffer(FlatBufferBuilder builder, int offset) { builder.finishSizePrefixed(offset); } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/VrepPath.py b/include/grl/flatbuffer/grl/flatbuffer/VrepPath.py new file mode 100644 index 0000000..4fb6567 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/VrepPath.py @@ -0,0 +1,44 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class VrepPath(object): + __slots__ = ['_tab'] + + @classmethod + def GetRootAsVrepPath(cls, buf, offset): + n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) + x = VrepPath() + x.Init(buf, n + offset) + return x + + # VrepPath + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + + # VrepPath + def ControlPoints(self, j): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + x = self._tab.Vector(o) + x += flatbuffers.number_types.UOffsetTFlags.py_type(j) * 4 + x = self._tab.Indirect(x) + from .VrepControlPoint import VrepControlPoint + obj = VrepControlPoint() + obj.Init(self._tab.Bytes, x) + return obj + return None + + # VrepPath + def ControlPointsLength(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + return self._tab.VectorLen(o) + return 0 + +def VrepPathStart(builder): builder.StartObject(1) +def VrepPathAddControlPoints(builder, controlPoints): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(controlPoints), 0) +def VrepPathStartControlPointsVector(builder, numElems): return builder.StartVector(4, numElems, 4) +def VrepPathEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/Wrench.java b/include/grl/flatbuffer/grl/flatbuffer/Wrench.java new file mode 100644 index 0000000..1d499d2 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/Wrench.java @@ -0,0 +1,39 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class Wrench extends Struct { + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public Wrench __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + public Vector3d force() { return force(new Vector3d()); } + public Vector3d force(Vector3d obj) { return obj.__assign(bb_pos + 0, bb); } + public Vector3d torque() { return torque(new Vector3d()); } + public Vector3d torque(Vector3d obj) { return obj.__assign(bb_pos + 24, bb); } + public Vector3d forceOffset() { return forceOffset(new Vector3d()); } + public Vector3d forceOffset(Vector3d obj) { return obj.__assign(bb_pos + 48, bb); } + + public static int createWrench(FlatBufferBuilder builder, double force_x, double force_y, double force_z, double torque_x, double torque_y, double torque_z, double force_offset_x, double force_offset_y, double force_offset_z) { + builder.prep(8, 72); + builder.prep(8, 24); + builder.putDouble(force_offset_z); + builder.putDouble(force_offset_y); + builder.putDouble(force_offset_x); + builder.prep(8, 24); + builder.putDouble(torque_z); + builder.putDouble(torque_y); + builder.putDouble(torque_x); + builder.prep(8, 24); + builder.putDouble(force_z); + builder.putDouble(force_y); + builder.putDouble(force_x); + return builder.offset(); + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/Wrench.py b/include/grl/flatbuffer/grl/flatbuffer/Wrench.py new file mode 100644 index 0000000..9d94d48 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/Wrench.py @@ -0,0 +1,44 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class Wrench(object): + __slots__ = ['_tab'] + + # Wrench + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + + # Wrench + def Force(self, obj): + obj.Init(self._tab.Bytes, self._tab.Pos + 0) + return obj + + # Wrench + def Torque(self, obj): + obj.Init(self._tab.Bytes, self._tab.Pos + 24) + return obj + + # Wrench + def ForceOffset(self, obj): + obj.Init(self._tab.Bytes, self._tab.Pos + 48) + return obj + + +def CreateWrench(builder, force_x, force_y, force_z, torque_x, torque_y, torque_z, force_offset_x, force_offset_y, force_offset_z): + builder.Prep(8, 72) + builder.Prep(8, 24) + builder.PrependFloat64(force_offset_z) + builder.PrependFloat64(force_offset_y) + builder.PrependFloat64(force_offset_x) + builder.Prep(8, 24) + builder.PrependFloat64(torque_z) + builder.PrependFloat64(torque_y) + builder.PrependFloat64(torque_x) + builder.Prep(8, 24) + builder.PrependFloat64(force_z) + builder.PrependFloat64(force_y) + builder.PrependFloat64(force_x) + return builder.Offset() diff --git a/include/grl/flatbuffer/grl/flatbuffer/__init__.py b/include/grl/flatbuffer/grl/flatbuffer/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/include/grl/flatbuffer/grl/flatbuffer/ftk3DFiducial.java b/include/grl/flatbuffer/grl/flatbuffer/ftk3DFiducial.java new file mode 100644 index 0000000..a20ab92 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/ftk3DFiducial.java @@ -0,0 +1,39 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class ftk3DFiducial extends Table { + public static ftk3DFiducial getRootAsftk3DFiducial(ByteBuffer _bb) { return getRootAsftk3DFiducial(_bb, new ftk3DFiducial()); } + public static ftk3DFiducial getRootAsftk3DFiducial(ByteBuffer _bb, ftk3DFiducial obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public ftk3DFiducial __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + public long markerID() { int o = __offset(4); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } + public long leftIndex() { int o = __offset(6); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } + public long rightIndex() { int o = __offset(8); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } + public Vector3d position() { return position(new Vector3d()); } + public Vector3d position(Vector3d obj) { int o = __offset(10); return o != 0 ? obj.__assign(o + bb_pos, bb) : null; } + public double epipolarErrorPixels() { int o = __offset(12); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } + public double triangulationError() { int o = __offset(14); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } + public double probability() { int o = __offset(16); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } + + public static void startftk3DFiducial(FlatBufferBuilder builder) { builder.startObject(7); } + public static void addMarkerID(FlatBufferBuilder builder, long markerID) { builder.addInt(0, (int)markerID, (int)0L); } + public static void addLeftIndex(FlatBufferBuilder builder, long leftIndex) { builder.addInt(1, (int)leftIndex, (int)0L); } + public static void addRightIndex(FlatBufferBuilder builder, long rightIndex) { builder.addInt(2, (int)rightIndex, (int)0L); } + public static void addPosition(FlatBufferBuilder builder, int positionOffset) { builder.addStruct(3, positionOffset, 0); } + public static void addEpipolarErrorPixels(FlatBufferBuilder builder, double epipolarErrorPixels) { builder.addDouble(4, epipolarErrorPixels, 0.0); } + public static void addTriangulationError(FlatBufferBuilder builder, double triangulationError) { builder.addDouble(5, triangulationError, 0.0); } + public static void addProbability(FlatBufferBuilder builder, double probability) { builder.addDouble(6, probability, 0.0); } + public static int endftk3DFiducial(FlatBufferBuilder builder) { + int o = builder.endObject(); + return o; + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/ftk3DFiducial.py b/include/grl/flatbuffer/grl/flatbuffer/ftk3DFiducial.py new file mode 100644 index 0000000..7644659 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/ftk3DFiducial.py @@ -0,0 +1,82 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class ftk3DFiducial(object): + __slots__ = ['_tab'] + + @classmethod + def GetRootAsftk3DFiducial(cls, buf, offset): + n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) + x = ftk3DFiducial() + x.Init(buf, n + offset) + return x + + # ftk3DFiducial + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + + # ftk3DFiducial + def MarkerID(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) + return 0 + + # ftk3DFiducial + def LeftIndex(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) + return 0 + + # ftk3DFiducial + def RightIndex(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) + return 0 + + # ftk3DFiducial + def Position(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) + if o != 0: + x = o + self._tab.Pos + from .Vector3d import Vector3d + obj = Vector3d() + obj.Init(self._tab.Bytes, x) + return obj + return None + + # ftk3DFiducial + def EpipolarErrorPixels(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(12)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) + return 0.0 + + # ftk3DFiducial + def TriangulationError(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(14)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) + return 0.0 + + # ftk3DFiducial + def Probability(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(16)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) + return 0.0 + +def ftk3DFiducialStart(builder): builder.StartObject(7) +def ftk3DFiducialAddMarkerID(builder, markerID): builder.PrependUint32Slot(0, markerID, 0) +def ftk3DFiducialAddLeftIndex(builder, leftIndex): builder.PrependUint32Slot(1, leftIndex, 0) +def ftk3DFiducialAddRightIndex(builder, rightIndex): builder.PrependUint32Slot(2, rightIndex, 0) +def ftk3DFiducialAddPosition(builder, position): builder.PrependStructSlot(3, flatbuffers.number_types.UOffsetTFlags.py_type(position), 0) +def ftk3DFiducialAddEpipolarErrorPixels(builder, epipolarErrorPixels): builder.PrependFloat64Slot(4, epipolarErrorPixels, 0.0) +def ftk3DFiducialAddTriangulationError(builder, triangulationError): builder.PrependFloat64Slot(5, triangulationError, 0.0) +def ftk3DFiducialAddProbability(builder, probability): builder.PrependFloat64Slot(6, probability, 0.0) +def ftk3DFiducialEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/ftkDeviceType.java b/include/grl/flatbuffer/grl/flatbuffer/ftkDeviceType.java new file mode 100644 index 0000000..d4c2a51 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/ftkDeviceType.java @@ -0,0 +1,13 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +public final class ftkDeviceType { + private ftkDeviceType() { } + public static final int DEV_SIMULATOR = 0; + public static final int DEV_INFINITRACK = 1; + public static final int DEV_FUSIONTRACK_500 = 2; + public static final int DEV_FUSIONTRACK_250 = 3; + public static final int DEV_UNKNOWN_DEVICE = 127; +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/ftkDeviceType.py b/include/grl/flatbuffer/grl/flatbuffer/ftkDeviceType.py new file mode 100644 index 0000000..6c94614 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/ftkDeviceType.py @@ -0,0 +1,11 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +class ftkDeviceType(object): + DEV_SIMULATOR = 0 + DEV_INFINITRACK = 1 + DEV_FUSIONTRACK_500 = 2 + DEV_FUSIONTRACK_250 = 3 + DEV_UNKNOWN_DEVICE = 127 + diff --git a/include/grl/flatbuffer/grl/flatbuffer/ftkGeometry.java b/include/grl/flatbuffer/grl/flatbuffer/ftkGeometry.java new file mode 100644 index 0000000..780b533 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/ftkGeometry.java @@ -0,0 +1,50 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class ftkGeometry extends Table { + public static ftkGeometry getRootAsftkGeometry(ByteBuffer _bb) { return getRootAsftkGeometry(_bb, new ftkGeometry()); } + public static ftkGeometry getRootAsftkGeometry(ByteBuffer _bb, ftkGeometry obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public ftkGeometry __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + public String name() { int o = __offset(4); return o != 0 ? __string(o + bb_pos) : null; } + public ByteBuffer nameAsByteBuffer() { return __vector_as_bytebuffer(4, 1); } + public ByteBuffer nameInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 4, 1); } + public long geometryID() { int o = __offset(6); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } + public long version() { int o = __offset(8); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } + public Vector3d positions(int j) { return positions(new Vector3d(), j); } + public Vector3d positions(Vector3d obj, int j) { int o = __offset(10); return o != 0 ? obj.__assign(__vector(o) + j * 24, bb) : null; } + public int positionsLength() { int o = __offset(10); return o != 0 ? __vector_len(o) : 0; } + + public static int createftkGeometry(FlatBufferBuilder builder, + int nameOffset, + long geometryID, + long version, + int positionsOffset) { + builder.startObject(4); + ftkGeometry.addPositions(builder, positionsOffset); + ftkGeometry.addVersion(builder, version); + ftkGeometry.addGeometryID(builder, geometryID); + ftkGeometry.addName(builder, nameOffset); + return ftkGeometry.endftkGeometry(builder); + } + + public static void startftkGeometry(FlatBufferBuilder builder) { builder.startObject(4); } + public static void addName(FlatBufferBuilder builder, int nameOffset) { builder.addOffset(0, nameOffset, 0); } + public static void addGeometryID(FlatBufferBuilder builder, long geometryID) { builder.addInt(1, (int)geometryID, (int)0L); } + public static void addVersion(FlatBufferBuilder builder, long version) { builder.addInt(2, (int)version, (int)0L); } + public static void addPositions(FlatBufferBuilder builder, int positionsOffset) { builder.addOffset(3, positionsOffset, 0); } + public static void startPositionsVector(FlatBufferBuilder builder, int numElems) { builder.startVector(24, numElems, 8); } + public static int endftkGeometry(FlatBufferBuilder builder) { + int o = builder.endObject(); + return o; + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/ftkGeometry.py b/include/grl/flatbuffer/grl/flatbuffer/ftkGeometry.py new file mode 100644 index 0000000..45db60a --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/ftkGeometry.py @@ -0,0 +1,67 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class ftkGeometry(object): + __slots__ = ['_tab'] + + @classmethod + def GetRootAsftkGeometry(cls, buf, offset): + n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) + x = ftkGeometry() + x.Init(buf, n + offset) + return x + + # ftkGeometry + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + + # ftkGeometry + def Name(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + return self._tab.String(o + self._tab.Pos) + return bytes() + + # ftkGeometry + def GeometryID(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) + return 0 + + # ftkGeometry + def Version(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) + return 0 + + # ftkGeometry + def Positions(self, j): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) + if o != 0: + x = self._tab.Vector(o) + x += flatbuffers.number_types.UOffsetTFlags.py_type(j) * 24 + from .Vector3d import Vector3d + obj = Vector3d() + obj.Init(self._tab.Bytes, x) + return obj + return None + + # ftkGeometry + def PositionsLength(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) + if o != 0: + return self._tab.VectorLen(o) + return 0 + +def ftkGeometryStart(builder): builder.StartObject(4) +def ftkGeometryAddName(builder, name): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(name), 0) +def ftkGeometryAddGeometryID(builder, geometryID): builder.PrependUint32Slot(1, geometryID, 0) +def ftkGeometryAddVersion(builder, version): builder.PrependUint32Slot(2, version, 0) +def ftkGeometryAddPositions(builder, positions): builder.PrependUOffsetTRelativeSlot(3, flatbuffers.number_types.UOffsetTFlags.py_type(positions), 0) +def ftkGeometryStartPositionsVector(builder, numElems): return builder.StartVector(24, numElems, 8) +def ftkGeometryEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/ftkMarker.java b/include/grl/flatbuffer/grl/flatbuffer/ftkMarker.java new file mode 100644 index 0000000..796e077 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/ftkMarker.java @@ -0,0 +1,42 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class ftkMarker extends Table { + public static ftkMarker getRootAsftkMarker(ByteBuffer _bb) { return getRootAsftkMarker(_bb, new ftkMarker()); } + public static ftkMarker getRootAsftkMarker(ByteBuffer _bb, ftkMarker obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public ftkMarker __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + public String name() { int o = __offset(4); return o != 0 ? __string(o + bb_pos) : null; } + public ByteBuffer nameAsByteBuffer() { return __vector_as_bytebuffer(4, 1); } + public ByteBuffer nameInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 4, 1); } + public long ID() { int o = __offset(6); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } + public long geometryID() { int o = __offset(8); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } + public long geometryPresenceMask(int j) { int o = __offset(10); return o != 0 ? (long)bb.getInt(__vector(o) + j * 4) & 0xFFFFFFFFL : 0; } + public int geometryPresenceMaskLength() { int o = __offset(10); return o != 0 ? __vector_len(o) : 0; } + public ByteBuffer geometryPresenceMaskAsByteBuffer() { return __vector_as_bytebuffer(10, 4); } + public ByteBuffer geometryPresenceMaskInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 10, 4); } + public Pose transform() { return transform(new Pose()); } + public Pose transform(Pose obj) { int o = __offset(12); return o != 0 ? obj.__assign(o + bb_pos, bb) : null; } + + public static void startftkMarker(FlatBufferBuilder builder) { builder.startObject(5); } + public static void addName(FlatBufferBuilder builder, int nameOffset) { builder.addOffset(0, nameOffset, 0); } + public static void addID(FlatBufferBuilder builder, long ID) { builder.addInt(1, (int)ID, (int)0L); } + public static void addGeometryID(FlatBufferBuilder builder, long geometryID) { builder.addInt(2, (int)geometryID, (int)0L); } + public static void addGeometryPresenceMask(FlatBufferBuilder builder, int geometryPresenceMaskOffset) { builder.addOffset(3, geometryPresenceMaskOffset, 0); } + public static int createGeometryPresenceMaskVector(FlatBufferBuilder builder, int[] data) { builder.startVector(4, data.length, 4); for (int i = data.length - 1; i >= 0; i--) builder.addInt(data[i]); return builder.endVector(); } + public static void startGeometryPresenceMaskVector(FlatBufferBuilder builder, int numElems) { builder.startVector(4, numElems, 4); } + public static void addTransform(FlatBufferBuilder builder, int transformOffset) { builder.addStruct(4, transformOffset, 0); } + public static int endftkMarker(FlatBufferBuilder builder) { + int o = builder.endObject(); + return o; + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/ftkMarker.py b/include/grl/flatbuffer/grl/flatbuffer/ftkMarker.py new file mode 100644 index 0000000..81700b2 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/ftkMarker.py @@ -0,0 +1,82 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class ftkMarker(object): + __slots__ = ['_tab'] + + @classmethod + def GetRootAsftkMarker(cls, buf, offset): + n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) + x = ftkMarker() + x.Init(buf, n + offset) + return x + + # ftkMarker + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + + # ftkMarker + def Name(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + return self._tab.String(o + self._tab.Pos) + return bytes() + + # ftkMarker + def ID(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) + return 0 + + # ftkMarker + def GeometryID(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) + return 0 + + # ftkMarker + def GeometryPresenceMask(self, j): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) + if o != 0: + a = self._tab.Vector(o) + return self._tab.Get(flatbuffers.number_types.Uint32Flags, a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 4)) + return 0 + + # ftkMarker + def GeometryPresenceMaskAsNumpy(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) + if o != 0: + return self._tab.GetVectorAsNumpy(flatbuffers.number_types.Uint32Flags, o) + return 0 + + # ftkMarker + def GeometryPresenceMaskLength(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) + if o != 0: + return self._tab.VectorLen(o) + return 0 + + # ftkMarker + def Transform(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(12)) + if o != 0: + x = o + self._tab.Pos + from .Pose import Pose + obj = Pose() + obj.Init(self._tab.Bytes, x) + return obj + return None + +def ftkMarkerStart(builder): builder.StartObject(5) +def ftkMarkerAddName(builder, name): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(name), 0) +def ftkMarkerAddID(builder, ID): builder.PrependUint32Slot(1, ID, 0) +def ftkMarkerAddGeometryID(builder, geometryID): builder.PrependUint32Slot(2, geometryID, 0) +def ftkMarkerAddGeometryPresenceMask(builder, geometryPresenceMask): builder.PrependUOffsetTRelativeSlot(3, flatbuffers.number_types.UOffsetTFlags.py_type(geometryPresenceMask), 0) +def ftkMarkerStartGeometryPresenceMaskVector(builder, numElems): return builder.StartVector(4, numElems, 4) +def ftkMarkerAddTransform(builder, transform): builder.PrependStructSlot(4, flatbuffers.number_types.UOffsetTFlags.py_type(transform), 0) +def ftkMarkerEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/ftkQueryStatus.java b/include/grl/flatbuffer/grl/flatbuffer/ftkQueryStatus.java new file mode 100644 index 0000000..c10ee6e --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/ftkQueryStatus.java @@ -0,0 +1,17 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +public final class ftkQueryStatus { + private ftkQueryStatus() { } + public static final int QS_WAR_SKIPPED = -1; + public static final int QS_OK = 0; + public static final int QS_ERR_OVERFLOW = 1; + public static final int QS_ERR_INVALID_RESERVED_SIZE = 2; + public static final int QS_REPROCESS = 10; + + public static final String[] names = { "QS_WAR_SKIPPED", "QS_OK", "QS_ERR_OVERFLOW", "QS_ERR_INVALID_RESERVED_SIZE", "", "", "", "", "", "", "", "QS_REPROCESS", }; + + public static String name(int e) { return names[e - QS_WAR_SKIPPED]; } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/ftkQueryStatus.py b/include/grl/flatbuffer/grl/flatbuffer/ftkQueryStatus.py new file mode 100644 index 0000000..cae8d81 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/ftkQueryStatus.py @@ -0,0 +1,11 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +class ftkQueryStatus(object): + QS_WAR_SKIPPED = -1 + QS_OK = 0 + QS_ERR_OVERFLOW = 1 + QS_ERR_INVALID_RESERVED_SIZE = 2 + QS_REPROCESS = 10 + diff --git a/include/grl/flatbuffer/grl/flatbuffer/ftkRegionOfInterest.java b/include/grl/flatbuffer/grl/flatbuffer/ftkRegionOfInterest.java new file mode 100644 index 0000000..2b09da6 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/ftkRegionOfInterest.java @@ -0,0 +1,61 @@ +// automatically generated by the FlatBuffers compiler, do not modify + +package grl.flatbuffer; + +import java.nio.*; +import java.lang.*; +import java.util.*; +import com.google.flatbuffers.*; + +@SuppressWarnings("unused") +public final class ftkRegionOfInterest extends Table { + public static ftkRegionOfInterest getRootAsftkRegionOfInterest(ByteBuffer _bb) { return getRootAsftkRegionOfInterest(_bb, new ftkRegionOfInterest()); } + public static ftkRegionOfInterest getRootAsftkRegionOfInterest(ByteBuffer _bb, ftkRegionOfInterest obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } + public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } + public ftkRegionOfInterest __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } + + public double centerXPixels() { int o = __offset(4); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } + public double centerYPixels() { int o = __offset(6); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } + public long RightEdge() { int o = __offset(8); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } + public long BottomEdge() { int o = __offset(10); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } + public long LeftEdge() { int o = __offset(12); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } + public long TopEdge() { int o = __offset(14); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } + public long pixelsCount() { int o = __offset(16); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } + public double probability() { int o = __offset(18); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } + + public static int createftkRegionOfInterest(FlatBufferBuilder builder, + double centerXPixels, + double centerYPixels, + long RightEdge, + long BottomEdge, + long LeftEdge, + long TopEdge, + long pixelsCount, + double probability) { + builder.startObject(8); + ftkRegionOfInterest.addProbability(builder, probability); + ftkRegionOfInterest.addCenterYPixels(builder, centerYPixels); + ftkRegionOfInterest.addCenterXPixels(builder, centerXPixels); + ftkRegionOfInterest.addPixelsCount(builder, pixelsCount); + ftkRegionOfInterest.addTopEdge(builder, TopEdge); + ftkRegionOfInterest.addLeftEdge(builder, LeftEdge); + ftkRegionOfInterest.addBottomEdge(builder, BottomEdge); + ftkRegionOfInterest.addRightEdge(builder, RightEdge); + return ftkRegionOfInterest.endftkRegionOfInterest(builder); + } + + public static void startftkRegionOfInterest(FlatBufferBuilder builder) { builder.startObject(8); } + public static void addCenterXPixels(FlatBufferBuilder builder, double centerXPixels) { builder.addDouble(0, centerXPixels, 0.0); } + public static void addCenterYPixels(FlatBufferBuilder builder, double centerYPixels) { builder.addDouble(1, centerYPixels, 0.0); } + public static void addRightEdge(FlatBufferBuilder builder, long RightEdge) { builder.addInt(2, (int)RightEdge, (int)0L); } + public static void addBottomEdge(FlatBufferBuilder builder, long BottomEdge) { builder.addInt(3, (int)BottomEdge, (int)0L); } + public static void addLeftEdge(FlatBufferBuilder builder, long LeftEdge) { builder.addInt(4, (int)LeftEdge, (int)0L); } + public static void addTopEdge(FlatBufferBuilder builder, long TopEdge) { builder.addInt(5, (int)TopEdge, (int)0L); } + public static void addPixelsCount(FlatBufferBuilder builder, long pixelsCount) { builder.addInt(6, (int)pixelsCount, (int)0L); } + public static void addProbability(FlatBufferBuilder builder, double probability) { builder.addDouble(7, probability, 0.0); } + public static int endftkRegionOfInterest(FlatBufferBuilder builder) { + int o = builder.endObject(); + return o; + } +} + diff --git a/include/grl/flatbuffer/grl/flatbuffer/ftkRegionOfInterest.py b/include/grl/flatbuffer/grl/flatbuffer/ftkRegionOfInterest.py new file mode 100644 index 0000000..88dcf74 --- /dev/null +++ b/include/grl/flatbuffer/grl/flatbuffer/ftkRegionOfInterest.py @@ -0,0 +1,86 @@ +# automatically generated by the FlatBuffers compiler, do not modify + +# namespace: flatbuffer + +import flatbuffers + +class ftkRegionOfInterest(object): + __slots__ = ['_tab'] + + @classmethod + def GetRootAsftkRegionOfInterest(cls, buf, offset): + n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) + x = ftkRegionOfInterest() + x.Init(buf, n + offset) + return x + + # ftkRegionOfInterest + def Init(self, buf, pos): + self._tab = flatbuffers.table.Table(buf, pos) + + # ftkRegionOfInterest + def CenterXPixels(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) + return 0.0 + + # ftkRegionOfInterest + def CenterYPixels(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) + return 0.0 + + # ftkRegionOfInterest + def RightEdge(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) + return 0 + + # ftkRegionOfInterest + def BottomEdge(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) + return 0 + + # ftkRegionOfInterest + def LeftEdge(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(12)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) + return 0 + + # ftkRegionOfInterest + def TopEdge(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(14)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) + return 0 + + # ftkRegionOfInterest + def PixelsCount(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(16)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) + return 0 + + # ftkRegionOfInterest + def Probability(self): + o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(18)) + if o != 0: + return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) + return 0.0 + +def ftkRegionOfInterestStart(builder): builder.StartObject(8) +def ftkRegionOfInterestAddCenterXPixels(builder, centerXPixels): builder.PrependFloat64Slot(0, centerXPixels, 0.0) +def ftkRegionOfInterestAddCenterYPixels(builder, centerYPixels): builder.PrependFloat64Slot(1, centerYPixels, 0.0) +def ftkRegionOfInterestAddRightEdge(builder, RightEdge): builder.PrependUint32Slot(2, RightEdge, 0) +def ftkRegionOfInterestAddBottomEdge(builder, BottomEdge): builder.PrependUint32Slot(3, BottomEdge, 0) +def ftkRegionOfInterestAddLeftEdge(builder, LeftEdge): builder.PrependUint32Slot(4, LeftEdge, 0) +def ftkRegionOfInterestAddTopEdge(builder, TopEdge): builder.PrependUint32Slot(5, TopEdge, 0) +def ftkRegionOfInterestAddPixelsCount(builder, pixelsCount): builder.PrependUint32Slot(6, pixelsCount, 0) +def ftkRegionOfInterestAddProbability(builder, probability): builder.PrependFloat64Slot(7, probability, 0.0) +def ftkRegionOfInterestEnd(builder): return builder.EndObject() From 741787e4f167967c3c5b9671c529f03001da81fc Mon Sep 17 00:00:00 2001 From: Chunting Date: Tue, 10 Apr 2018 18:48:45 -0400 Subject: [PATCH 090/102] Update the document about data based on the review. --- .../{Data.rst => DataRecordingAndPlayback.rst} | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) rename doc/howto/{Data.rst => DataRecordingAndPlayback.rst} (83%) diff --git a/doc/howto/Data.rst b/doc/howto/DataRecordingAndPlayback.rst similarity index 83% rename from doc/howto/Data.rst rename to doc/howto/DataRecordingAndPlayback.rst index 06e8b3b..c2eb9de 100644 --- a/doc/howto/Data.rst +++ b/doc/howto/DataRecordingAndPlayback.rst @@ -1,5 +1,5 @@ ========== -Data Setup +Data Analysis ========== .. note:: I will interpret the data files, including the relative location and the main content. @@ -7,10 +7,16 @@ Data Setup Flatbuffer fbs file ================================== -First install `VREP `__ and grl. +'FlatBuffers ' __ is an efficient cross platform serialization library for C++, C#, C, Go, Java, JavaScript, TypeScript, PHP, and Python. +It was originally created at Google for game development and other performance-critical applications. + +In this project, the messages are communicated by flatbuffer between PC and Kuka workstation, which makes reading and writing data efficient. + + +First install `VREP `__ and 'grl '__. The location of the 'fbs file '__. -They define the data structure we collect from Kuka and Atracsys. +fbs files define the binary format we use for high performance data collection from devices, such as the Kuka LBR iiwa 14kg and Atracsys FusionTrack. The binary files are put in VREP data folder (i.e. ~/src/V-REP_PRO_EDU_V3_4_0_Linux/data/), and are named by the time stamp (i.e. 2018_03_26_19_06_21_FusionTrack.flik, 2018_03_26_19_06_21_Kukaiiwa.iiwa). @@ -22,7 +28,7 @@ flatc -I . --json KUKAiiwa.fbs -- 2018_03_26_19_06_21_Kukaiiwa.iiwa CSV file ================================== -When Parsing the flatbuffer file to CSV, you need to follow the instructions below: +When exporting the flatbuffer file to CSV, you need to follow the instructions below: 1. Keep the binary files in the VREP data folder (i.e. ~/src/V-REP_PRO_EDU_V3_4_0_Linux/data/); From 7dcf90e73c341a911d4b09b65ad44b4d952ac2f0 Mon Sep 17 00:00:00 2001 From: Chunting Date: Wed, 11 Apr 2018 13:27:10 -0400 Subject: [PATCH 091/102] Undo the commit, removed those meaningless files --- config/FindFlatBuffers.cmake | 10 +- config/FindFlatBuffers.cmake.deleteme | 160 + doc/howto/Data.rst | 66 - .../flatbuffer/ArmControlState_generated.h | 1447 --------- include/grl/flatbuffer/Euler_generated.h | 529 --- .../grl/flatbuffer/FusionTrack_generated.h | 1803 ---------- include/grl/flatbuffer/Geometry_generated.h | 200 -- include/grl/flatbuffer/JointState_generated.h | 163 - include/grl/flatbuffer/KUKAiiwa_generated.h | 2885 ----------------- include/grl/flatbuffer/LinkObject_generated.h | 163 - .../LogKUKAiiwaFusionTrack_generated.h | 504 --- include/grl/flatbuffer/Time_generated.h | 291 -- .../flatbuffer/VrepControlPoint_generated.h | 306 -- include/grl/flatbuffer/VrepPath_generated.h | 153 - include/grl/flatbuffer/grl/__init__.py | 0 .../grl/flatbuffer/ArmControlSeries.java | 40 - .../grl/flatbuffer/ArmControlSeries.py | 44 - .../grl/flatbuffer/ArmControlState.java | 51 - .../grl/flatbuffer/ArmControlState.py | 65 - .../flatbuffer/grl/flatbuffer/ArmState.java | 21 - .../grl/flatbuffer/grl/flatbuffer/ArmState.py | 15 - .../CartesianImpedenceControlMode.java | 69 - .../CartesianImpedenceControlMode.py | 114 - .../grl/flatbuffer/DeviceState.java | 15 - .../flatbuffer/grl/flatbuffer/DeviceState.py | 9 - .../flatbuffer/grl/flatbuffer/Disabled.java | 24 - .../grl/flatbuffer/grl/flatbuffer/Disabled.py | 22 - .../grl/flatbuffer/EClientCommandMode.java | 19 - .../grl/flatbuffer/EClientCommandMode.py | 11 - .../grl/flatbuffer/EConnectionQuality.java | 16 - .../grl/flatbuffer/EConnectionQuality.py | 10 - .../grl/flatbuffer/EControlMode.java | 16 - .../flatbuffer/grl/flatbuffer/EControlMode.py | 10 - .../grl/flatbuffer/EDriveState.java | 24 - .../flatbuffer/grl/flatbuffer/EDriveState.py | 12 - .../grl/flatbuffer/EOperationMode.java | 15 - .../grl/flatbuffer/EOperationMode.py | 9 - .../grl/flatbuffer/EOverlayType.java | 15 - .../flatbuffer/grl/flatbuffer/EOverlayType.py | 9 - .../grl/flatbuffer/ESafetyState.java | 16 - .../flatbuffer/grl/flatbuffer/ESafetyState.py | 10 - .../grl/flatbuffer/ESessionState.java | 32 - .../grl/flatbuffer/ESessionState.py | 16 - .../flatbuffer/grl/flatbuffer/EulerOrder.java | 23 - .../flatbuffer/grl/flatbuffer/EulerOrder.py | 17 - .../flatbuffer/grl/flatbuffer/EulerPose.java | 35 - .../flatbuffer/grl/flatbuffer/EulerPose.py | 37 - .../grl/flatbuffer/EulerPoseParams.java | 30 - .../grl/flatbuffer/EulerPoseParams.py | 46 - .../grl/flatbuffer/EulerRotation.java | 30 - .../grl/flatbuffer/EulerRotation.py | 30 - .../grl/flatbuffer/EulerRotationParams.java | 45 - .../grl/flatbuffer/EulerRotationParams.py | 54 - .../flatbuffer/EulerTranslationParams.java | 41 - .../grl/flatbuffer/EulerTranslationParams.py | 46 - .../flatbuffer/grl/flatbuffer/EulerXYZd.java | 27 - .../flatbuffer/grl/flatbuffer/EulerXYZd.py | 26 - .../grl/flatbuffer/grl/flatbuffer/FRI.java | 97 - include/grl/flatbuffer/grl/flatbuffer/FRI.py | 110 - .../grl/flatbuffer/FRIMessageLog.java | 112 - .../grl/flatbuffer/FRIMessageLog.py | 226 -- .../grl/flatbuffer/FRITimeStamp.java | 37 - .../flatbuffer/grl/flatbuffer/FRITimeStamp.py | 38 - .../grl/flatbuffer/FusionTrackFrame.java | 179 - .../grl/flatbuffer/FusionTrackFrame.py | 330 -- .../grl/flatbuffer/FusionTrackMessage.java | 48 - .../grl/flatbuffer/FusionTrackMessage.py | 66 - .../grl/flatbuffer/FusionTrackParameters.java | 145 - .../grl/flatbuffer/FusionTrackParameters.py | 222 -- .../flatbuffer/grl/flatbuffer/Inertia.java | 47 - .../grl/flatbuffer/grl/flatbuffer/Inertia.py | 53 - .../flatbuffer/JointImpedenceControlMode.java | 47 - .../flatbuffer/JointImpedenceControlMode.py | 70 - .../flatbuffer/grl/flatbuffer/JointState.java | 65 - .../flatbuffer/grl/flatbuffer/JointState.py | 118 - .../flatbuffer/KUKAiiwaArmConfiguration.java | 119 - .../flatbuffer/KUKAiiwaArmConfiguration.py | 184 -- .../KUKAiiwaFusionTrackMessage.java | 46 - .../flatbuffer/KUKAiiwaFusionTrackMessage.py | 61 - .../grl/flatbuffer/KUKAiiwaInterface.java | 16 - .../grl/flatbuffer/KUKAiiwaInterface.py | 10 - .../KUKAiiwaMonitorConfiguration.java | 63 - .../KUKAiiwaMonitorConfiguration.py | 93 - .../grl/flatbuffer/KUKAiiwaMonitorState.java | 57 - .../grl/flatbuffer/KUKAiiwaMonitorState.py | 117 - .../grl/flatbuffer/KUKAiiwaState.java | 93 - .../grl/flatbuffer/KUKAiiwaState.py | 150 - .../grl/flatbuffer/KUKAiiwaStates.java | 40 - .../grl/flatbuffer/KUKAiiwaStates.py | 44 - .../flatbuffer/grl/flatbuffer/LinkObject.java | 38 - .../flatbuffer/grl/flatbuffer/LinkObject.py | 62 - .../flatbuffer/LogKUKAiiwaFusionTrack.java | 40 - .../grl/flatbuffer/LogKUKAiiwaFusionTrack.py | 44 - .../grl/flatbuffer/MoveArmCartesianServo.java | 31 - .../grl/flatbuffer/MoveArmCartesianServo.py | 42 - .../grl/flatbuffer/MoveArmJointServo.java | 34 - .../grl/flatbuffer/MoveArmJointServo.py | 34 - .../grl/flatbuffer/MoveArmTrajectory.java | 37 - .../grl/flatbuffer/MoveArmTrajectory.py | 44 - .../flatbuffer/grl/flatbuffer/PauseArm.java | 24 - .../grl/flatbuffer/grl/flatbuffer/PauseArm.py | 22 - .../grl/flatbuffer/grl/flatbuffer/Pose.java | 34 - include/grl/flatbuffer/grl/flatbuffer/Pose.py | 36 - .../grl/flatbuffer/ProcessData.java | 97 - .../flatbuffer/grl/flatbuffer/ProcessData.py | 108 - .../flatbuffer/grl/flatbuffer/Quaternion.java | 29 - .../flatbuffer/grl/flatbuffer/Quaternion.py | 29 - .../grl/flatbuffer/ShutdownArm.java | 24 - .../flatbuffer/grl/flatbuffer/ShutdownArm.py | 22 - .../flatbuffer/grl/flatbuffer/SmartServo.java | 61 - .../flatbuffer/grl/flatbuffer/SmartServo.py | 88 - .../flatbuffer/grl/flatbuffer/StartArm.java | 24 - .../grl/flatbuffer/grl/flatbuffer/StartArm.py | 22 - .../flatbuffer/grl/flatbuffer/StopArm.java | 24 - .../grl/flatbuffer/grl/flatbuffer/StopArm.py | 22 - .../flatbuffer/grl/flatbuffer/TeachArm.java | 24 - .../grl/flatbuffer/grl/flatbuffer/TeachArm.py | 22 - .../grl/flatbuffer/grl/flatbuffer/Time.java | 23 - include/grl/flatbuffer/grl/flatbuffer/Time.py | 20 - .../flatbuffer/grl/flatbuffer/TimeEvent.java | 112 - .../flatbuffer/grl/flatbuffer/TimeEvent.py | 115 - .../flatbuffer/grl/flatbuffer/Vector3d.java | 27 - .../grl/flatbuffer/grl/flatbuffer/Vector3d.py | 26 - .../grl/flatbuffer/VrepControlPoint.java | 52 - .../grl/flatbuffer/VrepControlPoint.py | 126 - .../flatbuffer/grl/flatbuffer/VrepPath.java | 39 - .../grl/flatbuffer/grl/flatbuffer/VrepPath.py | 44 - .../grl/flatbuffer/grl/flatbuffer/Wrench.java | 39 - .../grl/flatbuffer/grl/flatbuffer/Wrench.py | 44 - .../grl/flatbuffer/grl/flatbuffer/__init__.py | 0 .../grl/flatbuffer/ftk3DFiducial.java | 39 - .../grl/flatbuffer/ftk3DFiducial.py | 82 - .../grl/flatbuffer/ftkDeviceType.java | 13 - .../grl/flatbuffer/ftkDeviceType.py | 11 - .../grl/flatbuffer/ftkGeometry.java | 50 - .../flatbuffer/grl/flatbuffer/ftkGeometry.py | 67 - .../flatbuffer/grl/flatbuffer/ftkMarker.java | 42 - .../flatbuffer/grl/flatbuffer/ftkMarker.py | 82 - .../grl/flatbuffer/ftkQueryStatus.java | 17 - .../grl/flatbuffer/ftkQueryStatus.py | 11 - .../grl/flatbuffer/ftkRegionOfInterest.java | 61 - .../grl/flatbuffer/ftkRegionOfInterest.py | 86 - 142 files changed, 162 insertions(+), 15183 deletions(-) create mode 100644 config/FindFlatBuffers.cmake.deleteme delete mode 100644 doc/howto/Data.rst delete mode 100644 include/grl/flatbuffer/ArmControlState_generated.h delete mode 100644 include/grl/flatbuffer/Euler_generated.h delete mode 100644 include/grl/flatbuffer/FusionTrack_generated.h delete mode 100644 include/grl/flatbuffer/Geometry_generated.h delete mode 100644 include/grl/flatbuffer/JointState_generated.h delete mode 100644 include/grl/flatbuffer/KUKAiiwa_generated.h delete mode 100644 include/grl/flatbuffer/LinkObject_generated.h delete mode 100644 include/grl/flatbuffer/LogKUKAiiwaFusionTrack_generated.h delete mode 100644 include/grl/flatbuffer/Time_generated.h delete mode 100644 include/grl/flatbuffer/VrepControlPoint_generated.h delete mode 100644 include/grl/flatbuffer/VrepPath_generated.h delete mode 100644 include/grl/flatbuffer/grl/__init__.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/ArmControlSeries.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/ArmControlSeries.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/ArmControlState.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/ArmControlState.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/ArmState.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/ArmState.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/CartesianImpedenceControlMode.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/CartesianImpedenceControlMode.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/DeviceState.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/DeviceState.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/Disabled.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/Disabled.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/EClientCommandMode.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/EClientCommandMode.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/EConnectionQuality.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/EConnectionQuality.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/EControlMode.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/EControlMode.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/EDriveState.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/EDriveState.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/EOperationMode.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/EOperationMode.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/EOverlayType.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/EOverlayType.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/ESafetyState.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/ESafetyState.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/ESessionState.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/ESessionState.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/EulerOrder.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/EulerOrder.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/EulerPose.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/EulerPose.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/EulerPoseParams.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/EulerPoseParams.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/EulerRotation.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/EulerRotation.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/EulerRotationParams.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/EulerRotationParams.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/EulerTranslationParams.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/EulerTranslationParams.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/EulerXYZd.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/EulerXYZd.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/FRI.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/FRI.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/FRIMessageLog.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/FRIMessageLog.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/FRITimeStamp.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/FRITimeStamp.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/FusionTrackFrame.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/FusionTrackFrame.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/FusionTrackMessage.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/FusionTrackMessage.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/FusionTrackParameters.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/FusionTrackParameters.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/Inertia.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/Inertia.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/JointImpedenceControlMode.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/JointImpedenceControlMode.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/JointState.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/JointState.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaArmConfiguration.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaArmConfiguration.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaFusionTrackMessage.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaFusionTrackMessage.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaInterface.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaInterface.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaMonitorConfiguration.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaMonitorConfiguration.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaMonitorState.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaMonitorState.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaState.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaState.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaStates.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaStates.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/LinkObject.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/LinkObject.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/LogKUKAiiwaFusionTrack.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/LogKUKAiiwaFusionTrack.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/MoveArmCartesianServo.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/MoveArmCartesianServo.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/MoveArmJointServo.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/MoveArmJointServo.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/MoveArmTrajectory.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/MoveArmTrajectory.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/PauseArm.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/PauseArm.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/Pose.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/Pose.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/ProcessData.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/ProcessData.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/Quaternion.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/Quaternion.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/ShutdownArm.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/ShutdownArm.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/SmartServo.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/SmartServo.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/StartArm.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/StartArm.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/StopArm.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/StopArm.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/TeachArm.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/TeachArm.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/Time.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/Time.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/TimeEvent.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/TimeEvent.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/Vector3d.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/Vector3d.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/VrepControlPoint.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/VrepControlPoint.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/VrepPath.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/VrepPath.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/Wrench.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/Wrench.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/__init__.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/ftk3DFiducial.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/ftk3DFiducial.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/ftkDeviceType.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/ftkDeviceType.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/ftkGeometry.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/ftkGeometry.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/ftkMarker.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/ftkMarker.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/ftkQueryStatus.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/ftkQueryStatus.py delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/ftkRegionOfInterest.java delete mode 100644 include/grl/flatbuffer/grl/flatbuffer/ftkRegionOfInterest.py diff --git a/config/FindFlatBuffers.cmake b/config/FindFlatBuffers.cmake index f405e46..1b884aa 100644 --- a/config/FindFlatBuffers.cmake +++ b/config/FindFlatBuffers.cmake @@ -97,7 +97,6 @@ FLATBUFFERS_COMPILER if(FLATBUFFERS_FOUND) function(FLATBUFFERS_GENERATE_C_HEADERS Name FLATBUFFERS_DIR OUTPUT_DIR) - # Name is the name of the user defined variable that will be created by this function # Another variable that will be set is ${NAME}_OUTPUTS which will be set to the names # of all output files that have been generated. @@ -115,20 +114,15 @@ if(FLATBUFFERS_FOUND) list(APPEND FLATC_OUTPUTS ${FLATC_OUTPUT} ${FBS_FILE_COPY_INCLUDE} ${FBS_FILE_COPY_BIN}) # this is the absolute path to the actual filename.fbs file - set(ABSOLUTE_FBS_FILE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/${FLATBUFFERS_DIR}${FILE}) - + set(ABSOLUTE_FBS_FILE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/${FLATBUFFERS_DIR}/${FILE}) add_custom_command(OUTPUT ${FLATC_OUTPUT} - COMMAND ${FLATBUFFERS_COMPILER} # Note: We are setting several custom parameters here to make life easier. # see flatbuffers documentation for details. # flatc --gen-name-strings --scoped-enums --gen-object-api -c -j -p -o # see https://google.github.io/flatbuffers/flatbuffers_guide_using_schema_compiler.html - - - #ARGS --gen-name-strings --scoped-enums --gen-object-api -c -j -p -o "${OUTPUT_DIR}" ${ABSOLUTE_FBS_FILE_PATH} - ARGS --gen-name-strings --scoped-enums --gen-object-api -c -j -p -o "./" ${ABSOLUTE_FBS_FILE_PATH} + ARGS --gen-name-strings --scoped-enums --gen-object-api -c -j -p -o "${OUTPUT_DIR}" ${ABSOLUTE_FBS_FILE_PATH} MAIN_DEPENDENCY ${ABSOLUTE_FBS_FILE_PATH} COMMENT "Building C++, Java, and Python header for ${FILE}" WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/${FLATBUFFERS_DIR}) diff --git a/config/FindFlatBuffers.cmake.deleteme b/config/FindFlatBuffers.cmake.deleteme new file mode 100644 index 0000000..6f3a326 --- /dev/null +++ b/config/FindFlatBuffers.cmake.deleteme @@ -0,0 +1,160 @@ +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# Tries to find Flatbuffers headers and libraries. +# +# Usage of this module as follows: +# +# find_package(Flatbuffers) +# +# Variables used by this module, they can change the default behaviour and need +# to be set before calling find_package: +# +# Flatbuffers_HOME - +# When set, this path is inspected instead of standard library locations as +# the root of the Flatbuffers installation. The environment variable +# FLATBUFFERS_HOME overrides this veriable. +# +# This module defines +# FLATBUFFERS_INCLUDE_DIR, directory containing headers +# FLATBUFFERS_LIBS, directory containing flatbuffers libraries +# FLATBUFFERS_STATIC_LIB, path to libflatbuffers.a +# FLATBUFFERS_FOUND, whether flatbuffers has been found + +if( NOT "$ENV{FLATBUFFERS_HOME}" STREQUAL "") +file( TO_CMAKE_PATH "$ENV{FLATBUFFERS_HOME}" _native_path ) +list( APPEND _flatbuffers_roots ${_native_path} ) +elseif ( Flatbuffers_HOME ) +list( APPEND _flatbuffers_roots ${Flatbuffers_HOME} ) +endif() + +# Try the parameterized roots, if they exist +if ( _flatbuffers_roots ) +find_path( FLATBUFFERS_INCLUDE_DIR NAMES flatbuffers/flatbuffers.h + PATHS ${_flatbuffers_roots} NO_DEFAULT_PATH + PATH_SUFFIXES "include" ) +find_library( FLATBUFFERS_LIBRARIES NAMES flatbuffers + PATHS ${_flatbuffers_roots} NO_DEFAULT_PATH + PATH_SUFFIXES "lib" ) +else () +find_path( FLATBUFFERS_INCLUDE_DIR NAMES flatbuffers/flatbuffers.h ) +find_library( FLATBUFFERS_LIBRARIES NAMES flatbufferFLATBUFFERS_FLATC_EXECUTABLEs ) +endif () + +find_program(FLATBUFFERS_COMPILER flatc +PATH +$ENV{FLATBUFFERS_HOME}/bin +${_flatbuffers_roots}/bin +/usr/local/bin +/usr/bin +NO_DEFAULT_PATH +) + +if (FLATBUFFERS_INCLUDE_DIR AND FLATBUFFERS_LIBRARIES) +set(FLATBUFFERS_FOUND TRUE) +get_filename_component( FLATBUFFERS_LIBS ${FLATBUFFERS_LIBRARIES} PATH ) +set(FLATBUFFERS_LIB_NAME libflatbuffers) +set(FLATBUFFERS_STATIC_LIB ${FLATBUFFERS_LIBS}/${FLATBUFFERS_LIB_NAME}.a) +else () +set(FLATBUFFERS_FOUND FALSE) +endif () + +if (FLATBUFFERS_FOUND) +if (NOT Flatbuffers_FIND_QUIETLY) +message(STATUS "Found the Flatbuffers library: ${FLATBUFFERS_LIBRARIES}") +endif ()FLATBUFFERS_COMPILER +else () +if (NOT Flatbuffers_FIND_QUIETLY) +set(FLATBUFFERS_ERR_MSG "Could not find the Flatbuffers library. Looked in ") +if ( _flatbuffers_roots ) + set(FLATBUFFERS_ERR_MSG "${FLATBUFFERS_ERR_MSG} in ${_flatbuffers_roots}.") +else () + set(FLATBUFFERS_ERR_MSG "${FLATBUFFERS_ERR_MSG} system search paths.") +endif () +if (Flatbuffers_FIND_REQUIRED) + message(FATAL_ERROR "${FLATBUFFERS_ERR_MSG}") +else (Flatbuffers_FIND_REQUIRED) + message(STATUS "${FLATBUFFERS_ERR_MSG}") +endif (Flatbuffers_FIND_REQUIRED) +endif () +endif () + +mark_as_advanced( +FLATBUFFERS_INCLUDE_DIR +FLATBUFFERS_LIBS +FLATBUFFERS_STATIC_LIB +FLATBUFFERS_COMPILER +) + +if(FLATBUFFERS_FOUND) + function(FLATBUFFERS_GENERATE_C_HEADERS Name FLATBUFFERS_DIR OUTPUT_DIR) + # Name is the name of the user defined variable that will be created by this function + # Another variable that will be set is ${NAME}_OUTPUTS which will be set to the names + # of all output files that have been generated.FLATBUFFERS_COMPILER + # FLATBUFFERS_DIR is the directory in which to look for the .fbs files + # OUTPUT_DIR is the directory in which all output files should be placed + set(FLATC_OUTPUTS) + foreach(FILE ${ARGN}) + get_filename_component(FLATC_OUTPUT ${FILE} NAME_WE) + # create a target for the specific flatbuffers file + set(FBS_FILE_COPY_INCLUDE copy_${FLATC_OUTPUT}_to_include) + set(FBS_FILE_COPY_BIN copy_${FLATC_OUTPUT}_to_bin) + # create a target for the generated output cpp file + set(FLATC_OUTPUT + "${OUTPUT_DIR}/${FLATC_OUTPUT}_generated.h") + list(APPEND FLATC_OUTPUTS ${FLATC_OUTPUT} ${FBS_FILE_COPY_INCLUDE} ${FBS_FILE_COPY_BIN}) + + # this is the absolute path to the actual filename.fbs file + set(ABSOLUTE_FBS_FILE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/${FLATBUFFERS_DIR}/${FILE}) + + add_custom_command(OUTPUT ${FLATC_OUTPUT} + COMMAND ${FLATBUFFERS_COMPILER} + # Note: We are setting several custom parameters here to make life easier. + # see flatbuffers documentation for details. + # flatc --gen-name-strings --scoped-enums --gen-object-api -c -j -p -o + # see https://google.github.io/flatbuffers/flatbuffers_guide_using_schema_compiler.html + ARGS --gen-name-strings --scoped-enums --gen-object-api -c -j -p -o "${OUTPUT_DIR}" ${FILE} + MAIN_DEPENDENCY ${ABSOLUTE_FBS_FILE_PATH} + COMMENT "Building C++, Java, and Python header for ${FILE}" + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/${FLATBUFFERS_DIR}) + + # need to copy the flatbuffers schema so config files can be loaded + # http://stackoverflow.com/a/13429998/99379 + # CMAKE_CURRENT_SOURCE_DIR + # this is the directory where the currently processed CMakeLists.txt is located in + # terminal copy commands change between OS versions, so we use CMake's built in file + # copy command which runs with "cmake -E copy file_to_copy file_destination" + # we use some variables here so the path is reproducible. + add_custom_command(OUTPUT ${FBS_FILE_COPY_INCLUDE} + COMMAND ${CMAKE_COMMAND} ARGS -E copy ${ABSOLUTE_FBS_FILE_PATH} ${OUTPUT_DIR} + MAIN_DEPENDENCY ${CMAKE_CURRENT_SOURCE_DIR}/${FLATBUFFERS_DIR}/${FILE} + COMMENT "Copying fbs file ${FILE} to ${OUTPUT_DIR}" + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/${FLATBUFFERS_DIR} + ) + + add_custom_command(OUTPUT ${FBS_FILE_COPY_BIN} + # TODO(ahundt) remove hacky /bin manually set path, this will break for some IDEs + COMMAND ${CMAKE_COMMAND} ARGS -E copy ${ABSOLUTE_FBS_FILE_PATH} ${CMAKE_BINARY_DIR}/bin + MAIN_DEPENDENCY ${CMAKE_CURRENT_SOURCE_DIR}/${FLATBUFFERS_DIR}/${FILE} + COMMENT "Copying fbs file ${FILE} to ${CMAKE_BINARY_DIR}" + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/${FLATBUFFERS_DIR} + ) + endforeach() + set(${Name}_OUTPUTS ${FLATC_OUTPUTS} PARENT_SCOPE) + endfunction() + + set(FLATBUFFERS_INCLUDE_DIRS ${FLATBUFFERS_INCLUDE_DIR}) + include_directories(${CMAKE_BINARY_DIR}) +else() + set(FLATBUFFERS_INCLUDE_DIR) +endif() diff --git a/doc/howto/Data.rst b/doc/howto/Data.rst deleted file mode 100644 index 939f633..0000000 --- a/doc/howto/Data.rst +++ /dev/null @@ -1,66 +0,0 @@ -========== -Data Setup -========== - -.. note:: Here I will interpret the data file, including the relative location and the main content. - -Flatbuffer fbs file -================================== - -First install `VREP `__ and grl. - -The location of the 'fbs file '__. -They define the data we collect from Kuka and Atracsys. - -The binary files are put in VREP folder (i.e. ~/src/V-REP_PRO_EDU_V3_4_0_Linux), and are named by the time stamp (i.e. 2018_03_26_19_06_21_FusionTrack.flik, 2018_03_26_19_06_21_Kukaiiwa.iiwa). - -The command to generate json file, the binary file and the fbs file should be put in the same folder: -flatc -I . --json LogKUKAiiwaFusionTrack.fbs -- 2018_03_26_19_06_21_FusionTrack.flik -flatc -I . --json KUKAiiwa.fbs -- 2018_03_26_19_06_21_Kukaiiwa.iiwa - -CSV file -================================== - -When Parsing the flatbuffer file to CSV, you need to follow the instructions below: - -1. Copy the binary files to the location of the actual 'readFlatbufferTest '__executable; - -2. Run 'readFlatbufferTest '__ with the arguments of the name of binary file. - .. code-block:: bash - ./readFlatbufferTest 2018_03_26_19_06_21_Kukaiiwa.iiwa 2018_03_26_19_06_21_FusionTrack.flik - # You can also pass the arguments manually to the main() function in readFlatbufferTest.cpp. -3. The generated csv files will be put into a folder in the name of time stamp. - Label explanation: - .. code-block:: bash - local_request_time_offset: PC time of sending a requesting command to devices; - local_receive_time_offset: PC time of receiving measured data from devices; - device_time_offset: the time from device; - time_Y: time driftting, device_time_offset - local_request_time_offset; - counter: the identifier of message, defined by devices; - X Y Z A B C: the cartesian postion and oritation in Plucker coordinate system; - M_Pos_Ji: measured joint position of joint i from kuka; - C_Pos_J*: command joint position of joint i to kuka; - M_Tor_J*: measured joint torque of joint i form kuka; - C_Tor_J*: comand joint torque of joint i to kuka; - E_Tor_J*: external torque of joint i exerted on kuka; - - # on the first message save local_request_time as the initial_local_request_time. Both kuka and Atracsys share the same initial_local_request_time, which means they have the same time axis. - # on the first message save device_time as the initial_device_time - X = local_request_time - local_request_offset = (local_request_time - initial_local_request_time) - device_offset = (device_time - initial_device_time) - time_Y = device_offset - local_request_offset - CSV file explanation: - FTKUKA_TimeEvent.csv has the information from both Atracsys and kuka. It can help analysis time event. - FT_Pose_Marker*.csv have the time event and pose (in Plucker coordinate) of the specific marker in Atracsys space; - FT_TimeEvent.csv gives more detail information about time event from Atracsys, such as the time step; - KUKA_FRIMessage.csv includes all the FRI message from robot; - KUKA_Command_Joint.csv has the commanding joint angle sent to robot, which should use local_request_time_offset as time axis when plotting; - KUKA_Measured_Joint.csv has the measured joint angles received from robt, which should use local_receive_time_offset as time axis when plotting. - KUKA_TimeEvent.csv gives more detail information about time event from kuka, such as the time step; - - All the CSV files above are generated from binary files. To make it convenient, all the files have time event information, which can be used as X-axis when plotting. - - ForwardKinematics_Pose.csv is created by the replay process(at this moment, it's in the InverseKinematicsVrepPlug.cpp. - In future, It's better to creat a replay plugin to handl this process). It will read the measured joint angles from KUKA_Measured_Joint.csv, excute forward kinematics based on vrep model, - then write the cartesian pose (in Atracsys space) into ForwardKinematics_Pose.csv, which should be in ~/src/V-REP_PRO_EDU_V3_4_0_Linux/. diff --git a/include/grl/flatbuffer/ArmControlState_generated.h b/include/grl/flatbuffer/ArmControlState_generated.h deleted file mode 100644 index 18f323c..0000000 --- a/include/grl/flatbuffer/ArmControlState_generated.h +++ /dev/null @@ -1,1447 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - - -#ifndef FLATBUFFERS_GENERATED_ARMCONTROLSTATE_GRL_FLATBUFFER_H_ -#define FLATBUFFERS_GENERATED_ARMCONTROLSTATE_GRL_FLATBUFFER_H_ - -#include "flatbuffers/flatbuffers.h" - -#include "Geometry_generated.h" -#include "JointState_generated.h" - -namespace grl { -namespace flatbuffer { - -struct StartArm; -struct StartArmT; - -struct StopArm; -struct StopArmT; - -struct PauseArm; -struct PauseArmT; - -struct TeachArm; -struct TeachArmT; - -struct ShutdownArm; -struct ShutdownArmT; - -struct MoveArmTrajectory; -struct MoveArmTrajectoryT; - -struct MoveArmJointServo; -struct MoveArmJointServoT; - -struct MoveArmCartesianServo; -struct MoveArmCartesianServoT; - -struct ArmControlState; -struct ArmControlStateT; - -struct ArmControlSeries; -struct ArmControlSeriesT; - -enum class ArmState : uint8_t { - NONE = 0, - StartArm = 1, - StopArm = 2, - PauseArm = 3, - ShutdownArm = 4, - TeachArm = 5, - MoveArmTrajectory = 6, - MoveArmJointServo = 7, - MoveArmCartesianServo = 8, - MIN = NONE, - MAX = MoveArmCartesianServo -}; - -inline const ArmState (&EnumValuesArmState())[9] { - static const ArmState values[] = { - ArmState::NONE, - ArmState::StartArm, - ArmState::StopArm, - ArmState::PauseArm, - ArmState::ShutdownArm, - ArmState::TeachArm, - ArmState::MoveArmTrajectory, - ArmState::MoveArmJointServo, - ArmState::MoveArmCartesianServo - }; - return values; -} - -inline const char * const *EnumNamesArmState() { - static const char * const names[] = { - "NONE", - "StartArm", - "StopArm", - "PauseArm", - "ShutdownArm", - "TeachArm", - "MoveArmTrajectory", - "MoveArmJointServo", - "MoveArmCartesianServo", - nullptr - }; - return names; -} - -inline const char *EnumNameArmState(ArmState e) { - const size_t index = static_cast(e); - return EnumNamesArmState()[index]; -} - -template struct ArmStateTraits { - static const ArmState enum_value = ArmState::NONE; -}; - -template<> struct ArmStateTraits { - static const ArmState enum_value = ArmState::StartArm; -}; - -template<> struct ArmStateTraits { - static const ArmState enum_value = ArmState::StopArm; -}; - -template<> struct ArmStateTraits { - static const ArmState enum_value = ArmState::PauseArm; -}; - -template<> struct ArmStateTraits { - static const ArmState enum_value = ArmState::ShutdownArm; -}; - -template<> struct ArmStateTraits { - static const ArmState enum_value = ArmState::TeachArm; -}; - -template<> struct ArmStateTraits { - static const ArmState enum_value = ArmState::MoveArmTrajectory; -}; - -template<> struct ArmStateTraits { - static const ArmState enum_value = ArmState::MoveArmJointServo; -}; - -template<> struct ArmStateTraits { - static const ArmState enum_value = ArmState::MoveArmCartesianServo; -}; - -struct ArmStateUnion { - ArmState type; - void *value; - - ArmStateUnion() : type(ArmState::NONE), value(nullptr) {} - ArmStateUnion(ArmStateUnion&& u) FLATBUFFERS_NOEXCEPT : - type(ArmState::NONE), value(nullptr) - { std::swap(type, u.type); std::swap(value, u.value); } - ArmStateUnion(const ArmStateUnion &) FLATBUFFERS_NOEXCEPT; - ArmStateUnion &operator=(const ArmStateUnion &u) FLATBUFFERS_NOEXCEPT - { ArmStateUnion t(u); std::swap(type, t.type); std::swap(value, t.value); return *this; } - ArmStateUnion &operator=(ArmStateUnion &&u) FLATBUFFERS_NOEXCEPT - { std::swap(type, u.type); std::swap(value, u.value); return *this; } - ~ArmStateUnion() { Reset(); } - - void Reset(); - -#ifndef FLATBUFFERS_CPP98_STL - template - void Set(T&& val) { - Reset(); - type = ArmStateTraits::enum_value; - if (type != ArmState::NONE) { - value = new T(std::forward(val)); - } - } -#endif // FLATBUFFERS_CPP98_STL - - static void *UnPack(const void *obj, ArmState type, const flatbuffers::resolver_function_t *resolver); - flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const flatbuffers::rehasher_function_t *_rehasher = nullptr) const; - - StartArmT *AsStartArm() { - return type == ArmState::StartArm ? - reinterpret_cast(value) : nullptr; - } - const StartArmT *AsStartArm() const { - return type == ArmState::StartArm ? - reinterpret_cast(value) : nullptr; - } - StopArmT *AsStopArm() { - return type == ArmState::StopArm ? - reinterpret_cast(value) : nullptr; - } - const StopArmT *AsStopArm() const { - return type == ArmState::StopArm ? - reinterpret_cast(value) : nullptr; - } - PauseArmT *AsPauseArm() { - return type == ArmState::PauseArm ? - reinterpret_cast(value) : nullptr; - } - const PauseArmT *AsPauseArm() const { - return type == ArmState::PauseArm ? - reinterpret_cast(value) : nullptr; - } - ShutdownArmT *AsShutdownArm() { - return type == ArmState::ShutdownArm ? - reinterpret_cast(value) : nullptr; - } - const ShutdownArmT *AsShutdownArm() const { - return type == ArmState::ShutdownArm ? - reinterpret_cast(value) : nullptr; - } - TeachArmT *AsTeachArm() { - return type == ArmState::TeachArm ? - reinterpret_cast(value) : nullptr; - } - const TeachArmT *AsTeachArm() const { - return type == ArmState::TeachArm ? - reinterpret_cast(value) : nullptr; - } - MoveArmTrajectoryT *AsMoveArmTrajectory() { - return type == ArmState::MoveArmTrajectory ? - reinterpret_cast(value) : nullptr; - } - const MoveArmTrajectoryT *AsMoveArmTrajectory() const { - return type == ArmState::MoveArmTrajectory ? - reinterpret_cast(value) : nullptr; - } - MoveArmJointServoT *AsMoveArmJointServo() { - return type == ArmState::MoveArmJointServo ? - reinterpret_cast(value) : nullptr; - } - const MoveArmJointServoT *AsMoveArmJointServo() const { - return type == ArmState::MoveArmJointServo ? - reinterpret_cast(value) : nullptr; - } - MoveArmCartesianServoT *AsMoveArmCartesianServo() { - return type == ArmState::MoveArmCartesianServo ? - reinterpret_cast(value) : nullptr; - } - const MoveArmCartesianServoT *AsMoveArmCartesianServo() const { - return type == ArmState::MoveArmCartesianServo ? - reinterpret_cast(value) : nullptr; - } -}; - -bool VerifyArmState(flatbuffers::Verifier &verifier, const void *obj, ArmState type); -bool VerifyArmStateVector(flatbuffers::Verifier &verifier, const flatbuffers::Vector> *values, const flatbuffers::Vector *types); - -struct StartArmT : public flatbuffers::NativeTable { - typedef StartArm TableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.StartArmT"; - } - StartArmT() { - } -}; - -struct StartArm FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { - typedef StartArmT NativeTableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.StartArm"; - } - bool Verify(flatbuffers::Verifier &verifier) const { - return VerifyTableStart(verifier) && - verifier.EndTable(); - } - StartArmT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; - void UnPackTo(StartArmT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; - static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const StartArmT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); -}; - -struct StartArmBuilder { - flatbuffers::FlatBufferBuilder &fbb_; - flatbuffers::uoffset_t start_; - explicit StartArmBuilder(flatbuffers::FlatBufferBuilder &_fbb) - : fbb_(_fbb) { - start_ = fbb_.StartTable(); - } - StartArmBuilder &operator=(const StartArmBuilder &); - flatbuffers::Offset Finish() { - const auto end = fbb_.EndTable(start_); - auto o = flatbuffers::Offset(end); - return o; - } -}; - -inline flatbuffers::Offset CreateStartArm( - flatbuffers::FlatBufferBuilder &_fbb) { - StartArmBuilder builder_(_fbb); - return builder_.Finish(); -} - -flatbuffers::Offset CreateStartArm(flatbuffers::FlatBufferBuilder &_fbb, const StartArmT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); - -struct StopArmT : public flatbuffers::NativeTable { - typedef StopArm TableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.StopArmT"; - } - StopArmT() { - } -}; - -struct StopArm FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { - typedef StopArmT NativeTableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.StopArm"; - } - bool Verify(flatbuffers::Verifier &verifier) const { - return VerifyTableStart(verifier) && - verifier.EndTable(); - } - StopArmT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; - void UnPackTo(StopArmT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; - static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const StopArmT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); -}; - -struct StopArmBuilder { - flatbuffers::FlatBufferBuilder &fbb_; - flatbuffers::uoffset_t start_; - explicit StopArmBuilder(flatbuffers::FlatBufferBuilder &_fbb) - : fbb_(_fbb) { - start_ = fbb_.StartTable(); - } - StopArmBuilder &operator=(const StopArmBuilder &); - flatbuffers::Offset Finish() { - const auto end = fbb_.EndTable(start_); - auto o = flatbuffers::Offset(end); - return o; - } -}; - -inline flatbuffers::Offset CreateStopArm( - flatbuffers::FlatBufferBuilder &_fbb) { - StopArmBuilder builder_(_fbb); - return builder_.Finish(); -} - -flatbuffers::Offset CreateStopArm(flatbuffers::FlatBufferBuilder &_fbb, const StopArmT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); - -struct PauseArmT : public flatbuffers::NativeTable { - typedef PauseArm TableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.PauseArmT"; - } - PauseArmT() { - } -}; - -struct PauseArm FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { - typedef PauseArmT NativeTableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.PauseArm"; - } - bool Verify(flatbuffers::Verifier &verifier) const { - return VerifyTableStart(verifier) && - verifier.EndTable(); - } - PauseArmT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; - void UnPackTo(PauseArmT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; - static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const PauseArmT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); -}; - -struct PauseArmBuilder { - flatbuffers::FlatBufferBuilder &fbb_; - flatbuffers::uoffset_t start_; - explicit PauseArmBuilder(flatbuffers::FlatBufferBuilder &_fbb) - : fbb_(_fbb) { - start_ = fbb_.StartTable(); - } - PauseArmBuilder &operator=(const PauseArmBuilder &); - flatbuffers::Offset Finish() { - const auto end = fbb_.EndTable(start_); - auto o = flatbuffers::Offset(end); - return o; - } -}; - -inline flatbuffers::Offset CreatePauseArm( - flatbuffers::FlatBufferBuilder &_fbb) { - PauseArmBuilder builder_(_fbb); - return builder_.Finish(); -} - -flatbuffers::Offset CreatePauseArm(flatbuffers::FlatBufferBuilder &_fbb, const PauseArmT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); - -struct TeachArmT : public flatbuffers::NativeTable { - typedef TeachArm TableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.TeachArmT"; - } - TeachArmT() { - } -}; - -struct TeachArm FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { - typedef TeachArmT NativeTableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.TeachArm"; - } - bool Verify(flatbuffers::Verifier &verifier) const { - return VerifyTableStart(verifier) && - verifier.EndTable(); - } - TeachArmT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; - void UnPackTo(TeachArmT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; - static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const TeachArmT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); -}; - -struct TeachArmBuilder { - flatbuffers::FlatBufferBuilder &fbb_; - flatbuffers::uoffset_t start_; - explicit TeachArmBuilder(flatbuffers::FlatBufferBuilder &_fbb) - : fbb_(_fbb) { - start_ = fbb_.StartTable(); - } - TeachArmBuilder &operator=(const TeachArmBuilder &); - flatbuffers::Offset Finish() { - const auto end = fbb_.EndTable(start_); - auto o = flatbuffers::Offset(end); - return o; - } -}; - -inline flatbuffers::Offset CreateTeachArm( - flatbuffers::FlatBufferBuilder &_fbb) { - TeachArmBuilder builder_(_fbb); - return builder_.Finish(); -} - -flatbuffers::Offset CreateTeachArm(flatbuffers::FlatBufferBuilder &_fbb, const TeachArmT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); - -struct ShutdownArmT : public flatbuffers::NativeTable { - typedef ShutdownArm TableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.ShutdownArmT"; - } - ShutdownArmT() { - } -}; - -struct ShutdownArm FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { - typedef ShutdownArmT NativeTableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.ShutdownArm"; - } - bool Verify(flatbuffers::Verifier &verifier) const { - return VerifyTableStart(verifier) && - verifier.EndTable(); - } - ShutdownArmT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; - void UnPackTo(ShutdownArmT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; - static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const ShutdownArmT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); -}; - -struct ShutdownArmBuilder { - flatbuffers::FlatBufferBuilder &fbb_; - flatbuffers::uoffset_t start_; - explicit ShutdownArmBuilder(flatbuffers::FlatBufferBuilder &_fbb) - : fbb_(_fbb) { - start_ = fbb_.StartTable(); - } - ShutdownArmBuilder &operator=(const ShutdownArmBuilder &); - flatbuffers::Offset Finish() { - const auto end = fbb_.EndTable(start_); - auto o = flatbuffers::Offset(end); - return o; - } -}; - -inline flatbuffers::Offset CreateShutdownArm( - flatbuffers::FlatBufferBuilder &_fbb) { - ShutdownArmBuilder builder_(_fbb); - return builder_.Finish(); -} - -flatbuffers::Offset CreateShutdownArm(flatbuffers::FlatBufferBuilder &_fbb, const ShutdownArmT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); - -struct MoveArmTrajectoryT : public flatbuffers::NativeTable { - typedef MoveArmTrajectory TableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.MoveArmTrajectoryT"; - } - std::vector> traj; - MoveArmTrajectoryT() { - } -}; - -struct MoveArmTrajectory FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { - typedef MoveArmTrajectoryT NativeTableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.MoveArmTrajectory"; - } - enum { - VT_TRAJ = 4 - }; - const flatbuffers::Vector> *traj() const { - return GetPointer> *>(VT_TRAJ); - } - bool Verify(flatbuffers::Verifier &verifier) const { - return VerifyTableStart(verifier) && - VerifyOffset(verifier, VT_TRAJ) && - verifier.Verify(traj()) && - verifier.VerifyVectorOfTables(traj()) && - verifier.EndTable(); - } - MoveArmTrajectoryT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; - void UnPackTo(MoveArmTrajectoryT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; - static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const MoveArmTrajectoryT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); -}; - -struct MoveArmTrajectoryBuilder { - flatbuffers::FlatBufferBuilder &fbb_; - flatbuffers::uoffset_t start_; - void add_traj(flatbuffers::Offset>> traj) { - fbb_.AddOffset(MoveArmTrajectory::VT_TRAJ, traj); - } - explicit MoveArmTrajectoryBuilder(flatbuffers::FlatBufferBuilder &_fbb) - : fbb_(_fbb) { - start_ = fbb_.StartTable(); - } - MoveArmTrajectoryBuilder &operator=(const MoveArmTrajectoryBuilder &); - flatbuffers::Offset Finish() { - const auto end = fbb_.EndTable(start_); - auto o = flatbuffers::Offset(end); - return o; - } -}; - -inline flatbuffers::Offset CreateMoveArmTrajectory( - flatbuffers::FlatBufferBuilder &_fbb, - flatbuffers::Offset>> traj = 0) { - MoveArmTrajectoryBuilder builder_(_fbb); - builder_.add_traj(traj); - return builder_.Finish(); -} - -inline flatbuffers::Offset CreateMoveArmTrajectoryDirect( - flatbuffers::FlatBufferBuilder &_fbb, - const std::vector> *traj = nullptr) { - return grl::flatbuffer::CreateMoveArmTrajectory( - _fbb, - traj ? _fbb.CreateVector>(*traj) : 0); -} - -flatbuffers::Offset CreateMoveArmTrajectory(flatbuffers::FlatBufferBuilder &_fbb, const MoveArmTrajectoryT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); - -struct MoveArmJointServoT : public flatbuffers::NativeTable { - typedef MoveArmJointServo TableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.MoveArmJointServoT"; - } - std::unique_ptr goal; - MoveArmJointServoT() { - } -}; - -struct MoveArmJointServo FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { - typedef MoveArmJointServoT NativeTableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.MoveArmJointServo"; - } - enum { - VT_GOAL = 4 - }; - const JointState *goal() const { - return GetPointer(VT_GOAL); - } - bool Verify(flatbuffers::Verifier &verifier) const { - return VerifyTableStart(verifier) && - VerifyOffset(verifier, VT_GOAL) && - verifier.VerifyTable(goal()) && - verifier.EndTable(); - } - MoveArmJointServoT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; - void UnPackTo(MoveArmJointServoT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; - static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const MoveArmJointServoT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); -}; - -struct MoveArmJointServoBuilder { - flatbuffers::FlatBufferBuilder &fbb_; - flatbuffers::uoffset_t start_; - void add_goal(flatbuffers::Offset goal) { - fbb_.AddOffset(MoveArmJointServo::VT_GOAL, goal); - } - explicit MoveArmJointServoBuilder(flatbuffers::FlatBufferBuilder &_fbb) - : fbb_(_fbb) { - start_ = fbb_.StartTable(); - } - MoveArmJointServoBuilder &operator=(const MoveArmJointServoBuilder &); - flatbuffers::Offset Finish() { - const auto end = fbb_.EndTable(start_); - auto o = flatbuffers::Offset(end); - return o; - } -}; - -inline flatbuffers::Offset CreateMoveArmJointServo( - flatbuffers::FlatBufferBuilder &_fbb, - flatbuffers::Offset goal = 0) { - MoveArmJointServoBuilder builder_(_fbb); - builder_.add_goal(goal); - return builder_.Finish(); -} - -flatbuffers::Offset CreateMoveArmJointServo(flatbuffers::FlatBufferBuilder &_fbb, const MoveArmJointServoT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); - -struct MoveArmCartesianServoT : public flatbuffers::NativeTable { - typedef MoveArmCartesianServo TableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.MoveArmCartesianServoT"; - } - std::string parent; - std::unique_ptr goal; - MoveArmCartesianServoT() { - } -}; - -struct MoveArmCartesianServo FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { - typedef MoveArmCartesianServoT NativeTableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.MoveArmCartesianServo"; - } - enum { - VT_PARENT = 4, - VT_GOAL = 6 - }; - const flatbuffers::String *parent() const { - return GetPointer(VT_PARENT); - } - const Pose *goal() const { - return GetStruct(VT_GOAL); - } - bool Verify(flatbuffers::Verifier &verifier) const { - return VerifyTableStart(verifier) && - VerifyOffset(verifier, VT_PARENT) && - verifier.Verify(parent()) && - VerifyField(verifier, VT_GOAL) && - verifier.EndTable(); - } - MoveArmCartesianServoT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; - void UnPackTo(MoveArmCartesianServoT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; - static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const MoveArmCartesianServoT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); -}; - -struct MoveArmCartesianServoBuilder { - flatbuffers::FlatBufferBuilder &fbb_; - flatbuffers::uoffset_t start_; - void add_parent(flatbuffers::Offset parent) { - fbb_.AddOffset(MoveArmCartesianServo::VT_PARENT, parent); - } - void add_goal(const Pose *goal) { - fbb_.AddStruct(MoveArmCartesianServo::VT_GOAL, goal); - } - explicit MoveArmCartesianServoBuilder(flatbuffers::FlatBufferBuilder &_fbb) - : fbb_(_fbb) { - start_ = fbb_.StartTable(); - } - MoveArmCartesianServoBuilder &operator=(const MoveArmCartesianServoBuilder &); - flatbuffers::Offset Finish() { - const auto end = fbb_.EndTable(start_); - auto o = flatbuffers::Offset(end); - return o; - } -}; - -inline flatbuffers::Offset CreateMoveArmCartesianServo( - flatbuffers::FlatBufferBuilder &_fbb, - flatbuffers::Offset parent = 0, - const Pose *goal = 0) { - MoveArmCartesianServoBuilder builder_(_fbb); - builder_.add_goal(goal); - builder_.add_parent(parent); - return builder_.Finish(); -} - -inline flatbuffers::Offset CreateMoveArmCartesianServoDirect( - flatbuffers::FlatBufferBuilder &_fbb, - const char *parent = nullptr, - const Pose *goal = 0) { - return grl::flatbuffer::CreateMoveArmCartesianServo( - _fbb, - parent ? _fbb.CreateString(parent) : 0, - goal); -} - -flatbuffers::Offset CreateMoveArmCartesianServo(flatbuffers::FlatBufferBuilder &_fbb, const MoveArmCartesianServoT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); - -struct ArmControlStateT : public flatbuffers::NativeTable { - typedef ArmControlState TableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.ArmControlStateT"; - } - std::string name; - int64_t sequenceNumber; - double timeStamp; - ArmStateUnion state; - ArmControlStateT() - : sequenceNumber(0), - timeStamp(0.0) { - } -}; - -struct ArmControlState FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { - typedef ArmControlStateT NativeTableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.ArmControlState"; - } - enum { - VT_NAME = 4, - VT_SEQUENCENUMBER = 6, - VT_TIMESTAMP = 8, - VT_STATE_TYPE = 10, - VT_STATE = 12 - }; - const flatbuffers::String *name() const { - return GetPointer(VT_NAME); - } - int64_t sequenceNumber() const { - return GetField(VT_SEQUENCENUMBER, 0); - } - double timeStamp() const { - return GetField(VT_TIMESTAMP, 0.0); - } - ArmState state_type() const { - return static_cast(GetField(VT_STATE_TYPE, 0)); - } - const void *state() const { - return GetPointer(VT_STATE); - } - template const T *state_as() const; - const StartArm *state_as_StartArm() const { - return state_type() == ArmState::StartArm ? static_cast(state()) : nullptr; - } - const StopArm *state_as_StopArm() const { - return state_type() == ArmState::StopArm ? static_cast(state()) : nullptr; - } - const PauseArm *state_as_PauseArm() const { - return state_type() == ArmState::PauseArm ? static_cast(state()) : nullptr; - } - const ShutdownArm *state_as_ShutdownArm() const { - return state_type() == ArmState::ShutdownArm ? static_cast(state()) : nullptr; - } - const TeachArm *state_as_TeachArm() const { - return state_type() == ArmState::TeachArm ? static_cast(state()) : nullptr; - } - const MoveArmTrajectory *state_as_MoveArmTrajectory() const { - return state_type() == ArmState::MoveArmTrajectory ? static_cast(state()) : nullptr; - } - const MoveArmJointServo *state_as_MoveArmJointServo() const { - return state_type() == ArmState::MoveArmJointServo ? static_cast(state()) : nullptr; - } - const MoveArmCartesianServo *state_as_MoveArmCartesianServo() const { - return state_type() == ArmState::MoveArmCartesianServo ? static_cast(state()) : nullptr; - } - bool Verify(flatbuffers::Verifier &verifier) const { - return VerifyTableStart(verifier) && - VerifyOffset(verifier, VT_NAME) && - verifier.Verify(name()) && - VerifyField(verifier, VT_SEQUENCENUMBER) && - VerifyField(verifier, VT_TIMESTAMP) && - VerifyField(verifier, VT_STATE_TYPE) && - VerifyOffset(verifier, VT_STATE) && - VerifyArmState(verifier, state(), state_type()) && - verifier.EndTable(); - } - ArmControlStateT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; - void UnPackTo(ArmControlStateT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; - static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const ArmControlStateT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); -}; - -template<> inline const StartArm *ArmControlState::state_as() const { - return state_as_StartArm(); -} - -template<> inline const StopArm *ArmControlState::state_as() const { - return state_as_StopArm(); -} - -template<> inline const PauseArm *ArmControlState::state_as() const { - return state_as_PauseArm(); -} - -template<> inline const ShutdownArm *ArmControlState::state_as() const { - return state_as_ShutdownArm(); -} - -template<> inline const TeachArm *ArmControlState::state_as() const { - return state_as_TeachArm(); -} - -template<> inline const MoveArmTrajectory *ArmControlState::state_as() const { - return state_as_MoveArmTrajectory(); -} - -template<> inline const MoveArmJointServo *ArmControlState::state_as() const { - return state_as_MoveArmJointServo(); -} - -template<> inline const MoveArmCartesianServo *ArmControlState::state_as() const { - return state_as_MoveArmCartesianServo(); -} - -struct ArmControlStateBuilder { - flatbuffers::FlatBufferBuilder &fbb_; - flatbuffers::uoffset_t start_; - void add_name(flatbuffers::Offset name) { - fbb_.AddOffset(ArmControlState::VT_NAME, name); - } - void add_sequenceNumber(int64_t sequenceNumber) { - fbb_.AddElement(ArmControlState::VT_SEQUENCENUMBER, sequenceNumber, 0); - } - void add_timeStamp(double timeStamp) { - fbb_.AddElement(ArmControlState::VT_TIMESTAMP, timeStamp, 0.0); - } - void add_state_type(ArmState state_type) { - fbb_.AddElement(ArmControlState::VT_STATE_TYPE, static_cast(state_type), 0); - } - void add_state(flatbuffers::Offset state) { - fbb_.AddOffset(ArmControlState::VT_STATE, state); - } - explicit ArmControlStateBuilder(flatbuffers::FlatBufferBuilder &_fbb) - : fbb_(_fbb) { - start_ = fbb_.StartTable(); - } - ArmControlStateBuilder &operator=(const ArmControlStateBuilder &); - flatbuffers::Offset Finish() { - const auto end = fbb_.EndTable(start_); - auto o = flatbuffers::Offset(end); - return o; - } -}; - -inline flatbuffers::Offset CreateArmControlState( - flatbuffers::FlatBufferBuilder &_fbb, - flatbuffers::Offset name = 0, - int64_t sequenceNumber = 0, - double timeStamp = 0.0, - ArmState state_type = ArmState::NONE, - flatbuffers::Offset state = 0) { - ArmControlStateBuilder builder_(_fbb); - builder_.add_timeStamp(timeStamp); - builder_.add_sequenceNumber(sequenceNumber); - builder_.add_state(state); - builder_.add_name(name); - builder_.add_state_type(state_type); - return builder_.Finish(); -} - -inline flatbuffers::Offset CreateArmControlStateDirect( - flatbuffers::FlatBufferBuilder &_fbb, - const char *name = nullptr, - int64_t sequenceNumber = 0, - double timeStamp = 0.0, - ArmState state_type = ArmState::NONE, - flatbuffers::Offset state = 0) { - return grl::flatbuffer::CreateArmControlState( - _fbb, - name ? _fbb.CreateString(name) : 0, - sequenceNumber, - timeStamp, - state_type, - state); -} - -flatbuffers::Offset CreateArmControlState(flatbuffers::FlatBufferBuilder &_fbb, const ArmControlStateT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); - -struct ArmControlSeriesT : public flatbuffers::NativeTable { - typedef ArmControlSeries TableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.ArmControlSeriesT"; - } - std::vector> states; - ArmControlSeriesT() { - } -}; - -struct ArmControlSeries FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { - typedef ArmControlSeriesT NativeTableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.ArmControlSeries"; - } - enum { - VT_STATES = 4 - }; - const flatbuffers::Vector> *states() const { - return GetPointer> *>(VT_STATES); - } - bool Verify(flatbuffers::Verifier &verifier) const { - return VerifyTableStart(verifier) && - VerifyOffset(verifier, VT_STATES) && - verifier.Verify(states()) && - verifier.VerifyVectorOfTables(states()) && - verifier.EndTable(); - } - ArmControlSeriesT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; - void UnPackTo(ArmControlSeriesT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; - static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const ArmControlSeriesT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); -}; - -struct ArmControlSeriesBuilder { - flatbuffers::FlatBufferBuilder &fbb_; - flatbuffers::uoffset_t start_; - void add_states(flatbuffers::Offset>> states) { - fbb_.AddOffset(ArmControlSeries::VT_STATES, states); - } - explicit ArmControlSeriesBuilder(flatbuffers::FlatBufferBuilder &_fbb) - : fbb_(_fbb) { - start_ = fbb_.StartTable(); - } - ArmControlSeriesBuilder &operator=(const ArmControlSeriesBuilder &); - flatbuffers::Offset Finish() { - const auto end = fbb_.EndTable(start_); - auto o = flatbuffers::Offset(end); - return o; - } -}; - -inline flatbuffers::Offset CreateArmControlSeries( - flatbuffers::FlatBufferBuilder &_fbb, - flatbuffers::Offset>> states = 0) { - ArmControlSeriesBuilder builder_(_fbb); - builder_.add_states(states); - return builder_.Finish(); -} - -inline flatbuffers::Offset CreateArmControlSeriesDirect( - flatbuffers::FlatBufferBuilder &_fbb, - const std::vector> *states = nullptr) { - return grl::flatbuffer::CreateArmControlSeries( - _fbb, - states ? _fbb.CreateVector>(*states) : 0); -} - -flatbuffers::Offset CreateArmControlSeries(flatbuffers::FlatBufferBuilder &_fbb, const ArmControlSeriesT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); - -inline StartArmT *StartArm::UnPack(const flatbuffers::resolver_function_t *_resolver) const { - auto _o = new StartArmT(); - UnPackTo(_o, _resolver); - return _o; -} - -inline void StartArm::UnPackTo(StartArmT *_o, const flatbuffers::resolver_function_t *_resolver) const { - (void)_o; - (void)_resolver; -} - -inline flatbuffers::Offset StartArm::Pack(flatbuffers::FlatBufferBuilder &_fbb, const StartArmT* _o, const flatbuffers::rehasher_function_t *_rehasher) { - return CreateStartArm(_fbb, _o, _rehasher); -} - -inline flatbuffers::Offset CreateStartArm(flatbuffers::FlatBufferBuilder &_fbb, const StartArmT *_o, const flatbuffers::rehasher_function_t *_rehasher) { - (void)_rehasher; - (void)_o; - struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const StartArmT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; - return grl::flatbuffer::CreateStartArm( - _fbb); -} - -inline StopArmT *StopArm::UnPack(const flatbuffers::resolver_function_t *_resolver) const { - auto _o = new StopArmT(); - UnPackTo(_o, _resolver); - return _o; -} - -inline void StopArm::UnPackTo(StopArmT *_o, const flatbuffers::resolver_function_t *_resolver) const { - (void)_o; - (void)_resolver; -} - -inline flatbuffers::Offset StopArm::Pack(flatbuffers::FlatBufferBuilder &_fbb, const StopArmT* _o, const flatbuffers::rehasher_function_t *_rehasher) { - return CreateStopArm(_fbb, _o, _rehasher); -} - -inline flatbuffers::Offset CreateStopArm(flatbuffers::FlatBufferBuilder &_fbb, const StopArmT *_o, const flatbuffers::rehasher_function_t *_rehasher) { - (void)_rehasher; - (void)_o; - struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const StopArmT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; - return grl::flatbuffer::CreateStopArm( - _fbb); -} - -inline PauseArmT *PauseArm::UnPack(const flatbuffers::resolver_function_t *_resolver) const { - auto _o = new PauseArmT(); - UnPackTo(_o, _resolver); - return _o; -} - -inline void PauseArm::UnPackTo(PauseArmT *_o, const flatbuffers::resolver_function_t *_resolver) const { - (void)_o; - (void)_resolver; -} - -inline flatbuffers::Offset PauseArm::Pack(flatbuffers::FlatBufferBuilder &_fbb, const PauseArmT* _o, const flatbuffers::rehasher_function_t *_rehasher) { - return CreatePauseArm(_fbb, _o, _rehasher); -} - -inline flatbuffers::Offset CreatePauseArm(flatbuffers::FlatBufferBuilder &_fbb, const PauseArmT *_o, const flatbuffers::rehasher_function_t *_rehasher) { - (void)_rehasher; - (void)_o; - struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const PauseArmT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; - return grl::flatbuffer::CreatePauseArm( - _fbb); -} - -inline TeachArmT *TeachArm::UnPack(const flatbuffers::resolver_function_t *_resolver) const { - auto _o = new TeachArmT(); - UnPackTo(_o, _resolver); - return _o; -} - -inline void TeachArm::UnPackTo(TeachArmT *_o, const flatbuffers::resolver_function_t *_resolver) const { - (void)_o; - (void)_resolver; -} - -inline flatbuffers::Offset TeachArm::Pack(flatbuffers::FlatBufferBuilder &_fbb, const TeachArmT* _o, const flatbuffers::rehasher_function_t *_rehasher) { - return CreateTeachArm(_fbb, _o, _rehasher); -} - -inline flatbuffers::Offset CreateTeachArm(flatbuffers::FlatBufferBuilder &_fbb, const TeachArmT *_o, const flatbuffers::rehasher_function_t *_rehasher) { - (void)_rehasher; - (void)_o; - struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const TeachArmT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; - return grl::flatbuffer::CreateTeachArm( - _fbb); -} - -inline ShutdownArmT *ShutdownArm::UnPack(const flatbuffers::resolver_function_t *_resolver) const { - auto _o = new ShutdownArmT(); - UnPackTo(_o, _resolver); - return _o; -} - -inline void ShutdownArm::UnPackTo(ShutdownArmT *_o, const flatbuffers::resolver_function_t *_resolver) const { - (void)_o; - (void)_resolver; -} - -inline flatbuffers::Offset ShutdownArm::Pack(flatbuffers::FlatBufferBuilder &_fbb, const ShutdownArmT* _o, const flatbuffers::rehasher_function_t *_rehasher) { - return CreateShutdownArm(_fbb, _o, _rehasher); -} - -inline flatbuffers::Offset CreateShutdownArm(flatbuffers::FlatBufferBuilder &_fbb, const ShutdownArmT *_o, const flatbuffers::rehasher_function_t *_rehasher) { - (void)_rehasher; - (void)_o; - struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const ShutdownArmT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; - return grl::flatbuffer::CreateShutdownArm( - _fbb); -} - -inline MoveArmTrajectoryT *MoveArmTrajectory::UnPack(const flatbuffers::resolver_function_t *_resolver) const { - auto _o = new MoveArmTrajectoryT(); - UnPackTo(_o, _resolver); - return _o; -} - -inline void MoveArmTrajectory::UnPackTo(MoveArmTrajectoryT *_o, const flatbuffers::resolver_function_t *_resolver) const { - (void)_o; - (void)_resolver; - { auto _e = traj(); if (_e) { _o->traj.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->traj[_i] = std::unique_ptr(_e->Get(_i)->UnPack(_resolver)); } } }; -} - -inline flatbuffers::Offset MoveArmTrajectory::Pack(flatbuffers::FlatBufferBuilder &_fbb, const MoveArmTrajectoryT* _o, const flatbuffers::rehasher_function_t *_rehasher) { - return CreateMoveArmTrajectory(_fbb, _o, _rehasher); -} - -inline flatbuffers::Offset CreateMoveArmTrajectory(flatbuffers::FlatBufferBuilder &_fbb, const MoveArmTrajectoryT *_o, const flatbuffers::rehasher_function_t *_rehasher) { - (void)_rehasher; - (void)_o; - struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const MoveArmTrajectoryT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; - auto _traj = _o->traj.size() ? _fbb.CreateVector> (_o->traj.size(), [](size_t i, _VectorArgs *__va) { return CreateJointState(*__va->__fbb, __va->__o->traj[i].get(), __va->__rehasher); }, &_va ) : 0; - return grl::flatbuffer::CreateMoveArmTrajectory( - _fbb, - _traj); -} - -inline MoveArmJointServoT *MoveArmJointServo::UnPack(const flatbuffers::resolver_function_t *_resolver) const { - auto _o = new MoveArmJointServoT(); - UnPackTo(_o, _resolver); - return _o; -} - -inline void MoveArmJointServo::UnPackTo(MoveArmJointServoT *_o, const flatbuffers::resolver_function_t *_resolver) const { - (void)_o; - (void)_resolver; - { auto _e = goal(); if (_e) _o->goal = std::unique_ptr(_e->UnPack(_resolver)); }; -} - -inline flatbuffers::Offset MoveArmJointServo::Pack(flatbuffers::FlatBufferBuilder &_fbb, const MoveArmJointServoT* _o, const flatbuffers::rehasher_function_t *_rehasher) { - return CreateMoveArmJointServo(_fbb, _o, _rehasher); -} - -inline flatbuffers::Offset CreateMoveArmJointServo(flatbuffers::FlatBufferBuilder &_fbb, const MoveArmJointServoT *_o, const flatbuffers::rehasher_function_t *_rehasher) { - (void)_rehasher; - (void)_o; - struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const MoveArmJointServoT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; - auto _goal = _o->goal ? CreateJointState(_fbb, _o->goal.get(), _rehasher) : 0; - return grl::flatbuffer::CreateMoveArmJointServo( - _fbb, - _goal); -} - -inline MoveArmCartesianServoT *MoveArmCartesianServo::UnPack(const flatbuffers::resolver_function_t *_resolver) const { - auto _o = new MoveArmCartesianServoT(); - UnPackTo(_o, _resolver); - return _o; -} - -inline void MoveArmCartesianServo::UnPackTo(MoveArmCartesianServoT *_o, const flatbuffers::resolver_function_t *_resolver) const { - (void)_o; - (void)_resolver; - { auto _e = parent(); if (_e) _o->parent = _e->str(); }; - { auto _e = goal(); if (_e) _o->goal = std::unique_ptr(new Pose(*_e)); }; -} - -inline flatbuffers::Offset MoveArmCartesianServo::Pack(flatbuffers::FlatBufferBuilder &_fbb, const MoveArmCartesianServoT* _o, const flatbuffers::rehasher_function_t *_rehasher) { - return CreateMoveArmCartesianServo(_fbb, _o, _rehasher); -} - -inline flatbuffers::Offset CreateMoveArmCartesianServo(flatbuffers::FlatBufferBuilder &_fbb, const MoveArmCartesianServoT *_o, const flatbuffers::rehasher_function_t *_rehasher) { - (void)_rehasher; - (void)_o; - struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const MoveArmCartesianServoT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; - auto _parent = _o->parent.empty() ? 0 : _fbb.CreateString(_o->parent); - auto _goal = _o->goal ? _o->goal.get() : 0; - return grl::flatbuffer::CreateMoveArmCartesianServo( - _fbb, - _parent, - _goal); -} - -inline ArmControlStateT *ArmControlState::UnPack(const flatbuffers::resolver_function_t *_resolver) const { - auto _o = new ArmControlStateT(); - UnPackTo(_o, _resolver); - return _o; -} - -inline void ArmControlState::UnPackTo(ArmControlStateT *_o, const flatbuffers::resolver_function_t *_resolver) const { - (void)_o; - (void)_resolver; - { auto _e = name(); if (_e) _o->name = _e->str(); }; - { auto _e = sequenceNumber(); _o->sequenceNumber = _e; }; - { auto _e = timeStamp(); _o->timeStamp = _e; }; - { auto _e = state_type(); _o->state.type = _e; }; - { auto _e = state(); if (_e) _o->state.value = ArmStateUnion::UnPack(_e, state_type(), _resolver); }; -} - -inline flatbuffers::Offset ArmControlState::Pack(flatbuffers::FlatBufferBuilder &_fbb, const ArmControlStateT* _o, const flatbuffers::rehasher_function_t *_rehasher) { - return CreateArmControlState(_fbb, _o, _rehasher); -} - -inline flatbuffers::Offset CreateArmControlState(flatbuffers::FlatBufferBuilder &_fbb, const ArmControlStateT *_o, const flatbuffers::rehasher_function_t *_rehasher) { - (void)_rehasher; - (void)_o; - struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const ArmControlStateT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; - auto _name = _o->name.empty() ? 0 : _fbb.CreateString(_o->name); - auto _sequenceNumber = _o->sequenceNumber; - auto _timeStamp = _o->timeStamp; - auto _state_type = _o->state.type; - auto _state = _o->state.Pack(_fbb); - return grl::flatbuffer::CreateArmControlState( - _fbb, - _name, - _sequenceNumber, - _timeStamp, - _state_type, - _state); -} - -inline ArmControlSeriesT *ArmControlSeries::UnPack(const flatbuffers::resolver_function_t *_resolver) const { - auto _o = new ArmControlSeriesT(); - UnPackTo(_o, _resolver); - return _o; -} - -inline void ArmControlSeries::UnPackTo(ArmControlSeriesT *_o, const flatbuffers::resolver_function_t *_resolver) const { - (void)_o; - (void)_resolver; - { auto _e = states(); if (_e) { _o->states.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->states[_i] = std::unique_ptr(_e->Get(_i)->UnPack(_resolver)); } } }; -} - -inline flatbuffers::Offset ArmControlSeries::Pack(flatbuffers::FlatBufferBuilder &_fbb, const ArmControlSeriesT* _o, const flatbuffers::rehasher_function_t *_rehasher) { - return CreateArmControlSeries(_fbb, _o, _rehasher); -} - -inline flatbuffers::Offset CreateArmControlSeries(flatbuffers::FlatBufferBuilder &_fbb, const ArmControlSeriesT *_o, const flatbuffers::rehasher_function_t *_rehasher) { - (void)_rehasher; - (void)_o; - struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const ArmControlSeriesT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; - auto _states = _o->states.size() ? _fbb.CreateVector> (_o->states.size(), [](size_t i, _VectorArgs *__va) { return CreateArmControlState(*__va->__fbb, __va->__o->states[i].get(), __va->__rehasher); }, &_va ) : 0; - return grl::flatbuffer::CreateArmControlSeries( - _fbb, - _states); -} - -inline bool VerifyArmState(flatbuffers::Verifier &verifier, const void *obj, ArmState type) { - switch (type) { - case ArmState::NONE: { - return true; - } - case ArmState::StartArm: { - auto ptr = reinterpret_cast(obj); - return verifier.VerifyTable(ptr); - } - case ArmState::StopArm: { - auto ptr = reinterpret_cast(obj); - return verifier.VerifyTable(ptr); - } - case ArmState::PauseArm: { - auto ptr = reinterpret_cast(obj); - return verifier.VerifyTable(ptr); - } - case ArmState::ShutdownArm: { - auto ptr = reinterpret_cast(obj); - return verifier.VerifyTable(ptr); - } - case ArmState::TeachArm: { - auto ptr = reinterpret_cast(obj); - return verifier.VerifyTable(ptr); - } - case ArmState::MoveArmTrajectory: { - auto ptr = reinterpret_cast(obj); - return verifier.VerifyTable(ptr); - } - case ArmState::MoveArmJointServo: { - auto ptr = reinterpret_cast(obj); - return verifier.VerifyTable(ptr); - } - case ArmState::MoveArmCartesianServo: { - auto ptr = reinterpret_cast(obj); - return verifier.VerifyTable(ptr); - } - default: return false; - } -} - -inline bool VerifyArmStateVector(flatbuffers::Verifier &verifier, const flatbuffers::Vector> *values, const flatbuffers::Vector *types) { - if (!values || !types) return !values && !types; - if (values->size() != types->size()) return false; - for (flatbuffers::uoffset_t i = 0; i < values->size(); ++i) { - if (!VerifyArmState( - verifier, values->Get(i), types->GetEnum(i))) { - return false; - } - } - return true; -} - -inline void *ArmStateUnion::UnPack(const void *obj, ArmState type, const flatbuffers::resolver_function_t *resolver) { - switch (type) { - case ArmState::StartArm: { - auto ptr = reinterpret_cast(obj); - return ptr->UnPack(resolver); - } - case ArmState::StopArm: { - auto ptr = reinterpret_cast(obj); - return ptr->UnPack(resolver); - } - case ArmState::PauseArm: { - auto ptr = reinterpret_cast(obj); - return ptr->UnPack(resolver); - } - case ArmState::ShutdownArm: { - auto ptr = reinterpret_cast(obj); - return ptr->UnPack(resolver); - } - case ArmState::TeachArm: { - auto ptr = reinterpret_cast(obj); - return ptr->UnPack(resolver); - } - case ArmState::MoveArmTrajectory: { - auto ptr = reinterpret_cast(obj); - return ptr->UnPack(resolver); - } - case ArmState::MoveArmJointServo: { - auto ptr = reinterpret_cast(obj); - return ptr->UnPack(resolver); - } - case ArmState::MoveArmCartesianServo: { - auto ptr = reinterpret_cast(obj); - return ptr->UnPack(resolver); - } - default: return nullptr; - } -} - -inline flatbuffers::Offset ArmStateUnion::Pack(flatbuffers::FlatBufferBuilder &_fbb, const flatbuffers::rehasher_function_t *_rehasher) const { - switch (type) { - case ArmState::StartArm: { - auto ptr = reinterpret_cast(value); - return CreateStartArm(_fbb, ptr, _rehasher).Union(); - } - case ArmState::StopArm: { - auto ptr = reinterpret_cast(value); - return CreateStopArm(_fbb, ptr, _rehasher).Union(); - } - case ArmState::PauseArm: { - auto ptr = reinterpret_cast(value); - return CreatePauseArm(_fbb, ptr, _rehasher).Union(); - } - case ArmState::ShutdownArm: { - auto ptr = reinterpret_cast(value); - return CreateShutdownArm(_fbb, ptr, _rehasher).Union(); - } - case ArmState::TeachArm: { - auto ptr = reinterpret_cast(value); - return CreateTeachArm(_fbb, ptr, _rehasher).Union(); - } - case ArmState::MoveArmTrajectory: { - auto ptr = reinterpret_cast(value); - return CreateMoveArmTrajectory(_fbb, ptr, _rehasher).Union(); - } - case ArmState::MoveArmJointServo: { - auto ptr = reinterpret_cast(value); - return CreateMoveArmJointServo(_fbb, ptr, _rehasher).Union(); - } - case ArmState::MoveArmCartesianServo: { - auto ptr = reinterpret_cast(value); - return CreateMoveArmCartesianServo(_fbb, ptr, _rehasher).Union(); - } - default: return 0; - } -} - -inline ArmStateUnion::ArmStateUnion(const ArmStateUnion &u) FLATBUFFERS_NOEXCEPT : type(u.type), value(nullptr) { - switch (type) { - case ArmState::StartArm: { - value = new StartArmT(*reinterpret_cast(u.value)); - break; - } - case ArmState::StopArm: { - value = new StopArmT(*reinterpret_cast(u.value)); - break; - } - case ArmState::PauseArm: { - value = new PauseArmT(*reinterpret_cast(u.value)); - break; - } - case ArmState::ShutdownArm: { - value = new ShutdownArmT(*reinterpret_cast(u.value)); - break; - } - case ArmState::TeachArm: { - value = new TeachArmT(*reinterpret_cast(u.value)); - break; - } - case ArmState::MoveArmTrajectory: { - assert(false); // MoveArmTrajectoryT not copyable. - break; - } - case ArmState::MoveArmJointServo: { - assert(false); // MoveArmJointServoT not copyable. - break; - } - case ArmState::MoveArmCartesianServo: { - assert(false); // MoveArmCartesianServoT not copyable. - break; - } - default: - break; - } -} - -inline void ArmStateUnion::Reset() { - switch (type) { - case ArmState::StartArm: { - auto ptr = reinterpret_cast(value); - delete ptr; - break; - } - case ArmState::StopArm: { - auto ptr = reinterpret_cast(value); - delete ptr; - break; - } - case ArmState::PauseArm: { - auto ptr = reinterpret_cast(value); - delete ptr; - break; - } - case ArmState::ShutdownArm: { - auto ptr = reinterpret_cast(value); - delete ptr; - break; - } - case ArmState::TeachArm: { - auto ptr = reinterpret_cast(value); - delete ptr; - break; - } - case ArmState::MoveArmTrajectory: { - auto ptr = reinterpret_cast(value); - delete ptr; - break; - } - case ArmState::MoveArmJointServo: { - auto ptr = reinterpret_cast(value); - delete ptr; - break; - } - case ArmState::MoveArmCartesianServo: { - auto ptr = reinterpret_cast(value); - delete ptr; - break; - } - default: break; - } - value = nullptr; - type = ArmState::NONE; -} - -inline const grl::flatbuffer::ArmControlSeries *GetArmControlSeries(const void *buf) { - return flatbuffers::GetRoot(buf); -} - -inline const grl::flatbuffer::ArmControlSeries *GetSizePrefixedArmControlSeries(const void *buf) { - return flatbuffers::GetSizePrefixedRoot(buf); -} - -inline const char *ArmControlSeriesIdentifier() { - return "armc"; -} - -inline bool ArmControlSeriesBufferHasIdentifier(const void *buf) { - return flatbuffers::BufferHasIdentifier( - buf, ArmControlSeriesIdentifier()); -} - -inline bool VerifyArmControlSeriesBuffer( - flatbuffers::Verifier &verifier) { - return verifier.VerifyBuffer(ArmControlSeriesIdentifier()); -} - -inline bool VerifySizePrefixedArmControlSeriesBuffer( - flatbuffers::Verifier &verifier) { - return verifier.VerifySizePrefixedBuffer(ArmControlSeriesIdentifier()); -} - -inline const char *ArmControlSeriesExtension() { - return "armc"; -} - -inline void FinishArmControlSeriesBuffer( - flatbuffers::FlatBufferBuilder &fbb, - flatbuffers::Offset root) { - fbb.Finish(root, ArmControlSeriesIdentifier()); -} - -inline void FinishSizePrefixedArmControlSeriesBuffer( - flatbuffers::FlatBufferBuilder &fbb, - flatbuffers::Offset root) { - fbb.FinishSizePrefixed(root, ArmControlSeriesIdentifier()); -} - -inline std::unique_ptr UnPackArmControlSeries( - const void *buf, - const flatbuffers::resolver_function_t *res = nullptr) { - return std::unique_ptr(GetArmControlSeries(buf)->UnPack(res)); -} - -} // namespace flatbuffer -} // namespace grl - -#endif // FLATBUFFERS_GENERATED_ARMCONTROLSTATE_GRL_FLATBUFFER_H_ diff --git a/include/grl/flatbuffer/Euler_generated.h b/include/grl/flatbuffer/Euler_generated.h deleted file mode 100644 index 2c3785c..0000000 --- a/include/grl/flatbuffer/Euler_generated.h +++ /dev/null @@ -1,529 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - - -#ifndef FLATBUFFERS_GENERATED_EULER_GRL_FLATBUFFER_H_ -#define FLATBUFFERS_GENERATED_EULER_GRL_FLATBUFFER_H_ - -#include "flatbuffers/flatbuffers.h" - -#include "Geometry_generated.h" - -namespace grl { -namespace flatbuffer { - -struct EulerXYZd; - -struct EulerRotation; - -struct EulerPose; - -struct EulerTranslationParams; -struct EulerTranslationParamsT; - -struct EulerRotationParams; -struct EulerRotationParamsT; - -struct EulerPoseParams; -struct EulerPoseParamsT; - -enum class EulerOrder : int8_t { - xyz = 0, - yzx = 1, - zxy = 2, - xzy = 3, - zyx = 4, - yxz = 5, - zxz = 6, - xyx = 7, - yzy = 8, - xzx = 9, - yxy = 10, - MIN = xyz, - MAX = yxy -}; - -inline const EulerOrder (&EnumValuesEulerOrder())[11] { - static const EulerOrder values[] = { - EulerOrder::xyz, - EulerOrder::yzx, - EulerOrder::zxy, - EulerOrder::xzy, - EulerOrder::zyx, - EulerOrder::yxz, - EulerOrder::zxz, - EulerOrder::xyx, - EulerOrder::yzy, - EulerOrder::xzx, - EulerOrder::yxy - }; - return values; -} - -inline const char * const *EnumNamesEulerOrder() { - static const char * const names[] = { - "xyz", - "yzx", - "zxy", - "xzy", - "zyx", - "yxz", - "zxz", - "xyx", - "yzy", - "xzx", - "yxy", - nullptr - }; - return names; -} - -inline const char *EnumNameEulerOrder(EulerOrder e) { - const size_t index = static_cast(e); - return EnumNamesEulerOrder()[index]; -} - -FLATBUFFERS_MANUALLY_ALIGNED_STRUCT(8) EulerXYZd FLATBUFFERS_FINAL_CLASS { - private: - double rx_; - double ry_; - double rz_; - - public: - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.EulerXYZd"; - } - EulerXYZd() { - memset(this, 0, sizeof(EulerXYZd)); - } - EulerXYZd(double _rx, double _ry, double _rz) - : rx_(flatbuffers::EndianScalar(_rx)), - ry_(flatbuffers::EndianScalar(_ry)), - rz_(flatbuffers::EndianScalar(_rz)) { - } - double rx() const { - return flatbuffers::EndianScalar(rx_); - } - double ry() const { - return flatbuffers::EndianScalar(ry_); - } - double rz() const { - return flatbuffers::EndianScalar(rz_); - } -}; -FLATBUFFERS_STRUCT_END(EulerXYZd, 24); - -FLATBUFFERS_MANUALLY_ALIGNED_STRUCT(8) EulerRotation FLATBUFFERS_FINAL_CLASS { - private: - double r1_; - double r2_; - double r3_; - int8_t eulerOrder_; - int8_t padding0__; int16_t padding1__; int32_t padding2__; - - public: - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.EulerRotation"; - } - EulerRotation() { - memset(this, 0, sizeof(EulerRotation)); - } - EulerRotation(double _r1, double _r2, double _r3, EulerOrder _eulerOrder) - : r1_(flatbuffers::EndianScalar(_r1)), - r2_(flatbuffers::EndianScalar(_r2)), - r3_(flatbuffers::EndianScalar(_r3)), - eulerOrder_(flatbuffers::EndianScalar(static_cast(_eulerOrder))), - padding0__(0), - padding1__(0), - padding2__(0) { - (void)padding0__; (void)padding1__; (void)padding2__; - } - double r1() const { - return flatbuffers::EndianScalar(r1_); - } - double r2() const { - return flatbuffers::EndianScalar(r2_); - } - double r3() const { - return flatbuffers::EndianScalar(r3_); - } - EulerOrder eulerOrder() const { - return static_cast(flatbuffers::EndianScalar(eulerOrder_)); - } -}; -FLATBUFFERS_STRUCT_END(EulerRotation, 32); - -FLATBUFFERS_MANUALLY_ALIGNED_STRUCT(8) EulerPose FLATBUFFERS_FINAL_CLASS { - private: - Vector3d position_; - EulerRotation rotation_; - - public: - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.EulerPose"; - } - EulerPose() { - memset(this, 0, sizeof(EulerPose)); - } - EulerPose(const Vector3d &_position, const EulerRotation &_rotation) - : position_(_position), - rotation_(_rotation) { - } - const Vector3d &position() const { - return position_; - } - const EulerRotation &rotation() const { - return rotation_; - } -}; -FLATBUFFERS_STRUCT_END(EulerPose, 56); - -struct EulerTranslationParamsT : public flatbuffers::NativeTable { - typedef EulerTranslationParams TableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.EulerTranslationParamsT"; - } - double x; - double y; - double z; - EulerTranslationParamsT() - : x(0.0), - y(0.0), - z(0.0) { - } -}; - -struct EulerTranslationParams FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { - typedef EulerTranslationParamsT NativeTableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.EulerTranslationParams"; - } - enum { - VT_X = 4, - VT_Y = 6, - VT_Z = 8 - }; - double x() const { - return GetField(VT_X, 0.0); - } - double y() const { - return GetField(VT_Y, 0.0); - } - double z() const { - return GetField(VT_Z, 0.0); - } - bool Verify(flatbuffers::Verifier &verifier) const { - return VerifyTableStart(verifier) && - VerifyField(verifier, VT_X) && - VerifyField(verifier, VT_Y) && - VerifyField(verifier, VT_Z) && - verifier.EndTable(); - } - EulerTranslationParamsT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; - void UnPackTo(EulerTranslationParamsT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; - static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const EulerTranslationParamsT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); -}; - -struct EulerTranslationParamsBuilder { - flatbuffers::FlatBufferBuilder &fbb_; - flatbuffers::uoffset_t start_; - void add_x(double x) { - fbb_.AddElement(EulerTranslationParams::VT_X, x, 0.0); - } - void add_y(double y) { - fbb_.AddElement(EulerTranslationParams::VT_Y, y, 0.0); - } - void add_z(double z) { - fbb_.AddElement(EulerTranslationParams::VT_Z, z, 0.0); - } - explicit EulerTranslationParamsBuilder(flatbuffers::FlatBufferBuilder &_fbb) - : fbb_(_fbb) { - start_ = fbb_.StartTable(); - } - EulerTranslationParamsBuilder &operator=(const EulerTranslationParamsBuilder &); - flatbuffers::Offset Finish() { - const auto end = fbb_.EndTable(start_); - auto o = flatbuffers::Offset(end); - return o; - } -}; - -inline flatbuffers::Offset CreateEulerTranslationParams( - flatbuffers::FlatBufferBuilder &_fbb, - double x = 0.0, - double y = 0.0, - double z = 0.0) { - EulerTranslationParamsBuilder builder_(_fbb); - builder_.add_z(z); - builder_.add_y(y); - builder_.add_x(x); - return builder_.Finish(); -} - -flatbuffers::Offset CreateEulerTranslationParams(flatbuffers::FlatBufferBuilder &_fbb, const EulerTranslationParamsT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); - -struct EulerRotationParamsT : public flatbuffers::NativeTable { - typedef EulerRotationParams TableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.EulerRotationParamsT"; - } - double r1; - double r2; - double r3; - EulerOrder eulerOrder; - EulerRotationParamsT() - : r1(0.0), - r2(0.0), - r3(0.0), - eulerOrder(EulerOrder::xyz) { - } -}; - -struct EulerRotationParams FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { - typedef EulerRotationParamsT NativeTableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.EulerRotationParams"; - } - enum { - VT_R1 = 4, - VT_R2 = 6, - VT_R3 = 8, - VT_EULERORDER = 10 - }; - double r1() const { - return GetField(VT_R1, 0.0); - } - double r2() const { - return GetField(VT_R2, 0.0); - } - double r3() const { - return GetField(VT_R3, 0.0); - } - EulerOrder eulerOrder() const { - return static_cast(GetField(VT_EULERORDER, 0)); - } - bool Verify(flatbuffers::Verifier &verifier) const { - return VerifyTableStart(verifier) && - VerifyField(verifier, VT_R1) && - VerifyField(verifier, VT_R2) && - VerifyField(verifier, VT_R3) && - VerifyField(verifier, VT_EULERORDER) && - verifier.EndTable(); - } - EulerRotationParamsT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; - void UnPackTo(EulerRotationParamsT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; - static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const EulerRotationParamsT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); -}; - -struct EulerRotationParamsBuilder { - flatbuffers::FlatBufferBuilder &fbb_; - flatbuffers::uoffset_t start_; - void add_r1(double r1) { - fbb_.AddElement(EulerRotationParams::VT_R1, r1, 0.0); - } - void add_r2(double r2) { - fbb_.AddElement(EulerRotationParams::VT_R2, r2, 0.0); - } - void add_r3(double r3) { - fbb_.AddElement(EulerRotationParams::VT_R3, r3, 0.0); - } - void add_eulerOrder(EulerOrder eulerOrder) { - fbb_.AddElement(EulerRotationParams::VT_EULERORDER, static_cast(eulerOrder), 0); - } - explicit EulerRotationParamsBuilder(flatbuffers::FlatBufferBuilder &_fbb) - : fbb_(_fbb) { - start_ = fbb_.StartTable(); - } - EulerRotationParamsBuilder &operator=(const EulerRotationParamsBuilder &); - flatbuffers::Offset Finish() { - const auto end = fbb_.EndTable(start_); - auto o = flatbuffers::Offset(end); - return o; - } -}; - -inline flatbuffers::Offset CreateEulerRotationParams( - flatbuffers::FlatBufferBuilder &_fbb, - double r1 = 0.0, - double r2 = 0.0, - double r3 = 0.0, - EulerOrder eulerOrder = EulerOrder::xyz) { - EulerRotationParamsBuilder builder_(_fbb); - builder_.add_r3(r3); - builder_.add_r2(r2); - builder_.add_r1(r1); - builder_.add_eulerOrder(eulerOrder); - return builder_.Finish(); -} - -flatbuffers::Offset CreateEulerRotationParams(flatbuffers::FlatBufferBuilder &_fbb, const EulerRotationParamsT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); - -struct EulerPoseParamsT : public flatbuffers::NativeTable { - typedef EulerPoseParams TableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.EulerPoseParamsT"; - } - std::unique_ptr position; - std::unique_ptr rotation; - EulerPoseParamsT() { - } -}; - -struct EulerPoseParams FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { - typedef EulerPoseParamsT NativeTableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.EulerPoseParams"; - } - enum { - VT_POSITION = 4, - VT_ROTATION = 6 - }; - const Vector3d *position() const { - return GetStruct(VT_POSITION); - } - const EulerRotation *rotation() const { - return GetStruct(VT_ROTATION); - } - bool Verify(flatbuffers::Verifier &verifier) const { - return VerifyTableStart(verifier) && - VerifyField(verifier, VT_POSITION) && - VerifyField(verifier, VT_ROTATION) && - verifier.EndTable(); - } - EulerPoseParamsT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; - void UnPackTo(EulerPoseParamsT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; - static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const EulerPoseParamsT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); -}; - -struct EulerPoseParamsBuilder { - flatbuffers::FlatBufferBuilder &fbb_; - flatbuffers::uoffset_t start_; - void add_position(const Vector3d *position) { - fbb_.AddStruct(EulerPoseParams::VT_POSITION, position); - } - void add_rotation(const EulerRotation *rotation) { - fbb_.AddStruct(EulerPoseParams::VT_ROTATION, rotation); - } - explicit EulerPoseParamsBuilder(flatbuffers::FlatBufferBuilder &_fbb) - : fbb_(_fbb) { - start_ = fbb_.StartTable(); - } - EulerPoseParamsBuilder &operator=(const EulerPoseParamsBuilder &); - flatbuffers::Offset Finish() { - const auto end = fbb_.EndTable(start_); - auto o = flatbuffers::Offset(end); - return o; - } -}; - -inline flatbuffers::Offset CreateEulerPoseParams( - flatbuffers::FlatBufferBuilder &_fbb, - const Vector3d *position = 0, - const EulerRotation *rotation = 0) { - EulerPoseParamsBuilder builder_(_fbb); - builder_.add_rotation(rotation); - builder_.add_position(position); - return builder_.Finish(); -} - -flatbuffers::Offset CreateEulerPoseParams(flatbuffers::FlatBufferBuilder &_fbb, const EulerPoseParamsT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); - -inline EulerTranslationParamsT *EulerTranslationParams::UnPack(const flatbuffers::resolver_function_t *_resolver) const { - auto _o = new EulerTranslationParamsT(); - UnPackTo(_o, _resolver); - return _o; -} - -inline void EulerTranslationParams::UnPackTo(EulerTranslationParamsT *_o, const flatbuffers::resolver_function_t *_resolver) const { - (void)_o; - (void)_resolver; - { auto _e = x(); _o->x = _e; }; - { auto _e = y(); _o->y = _e; }; - { auto _e = z(); _o->z = _e; }; -} - -inline flatbuffers::Offset EulerTranslationParams::Pack(flatbuffers::FlatBufferBuilder &_fbb, const EulerTranslationParamsT* _o, const flatbuffers::rehasher_function_t *_rehasher) { - return CreateEulerTranslationParams(_fbb, _o, _rehasher); -} - -inline flatbuffers::Offset CreateEulerTranslationParams(flatbuffers::FlatBufferBuilder &_fbb, const EulerTranslationParamsT *_o, const flatbuffers::rehasher_function_t *_rehasher) { - (void)_rehasher; - (void)_o; - struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const EulerTranslationParamsT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; - auto _x = _o->x; - auto _y = _o->y; - auto _z = _o->z; - return grl::flatbuffer::CreateEulerTranslationParams( - _fbb, - _x, - _y, - _z); -} - -inline EulerRotationParamsT *EulerRotationParams::UnPack(const flatbuffers::resolver_function_t *_resolver) const { - auto _o = new EulerRotationParamsT(); - UnPackTo(_o, _resolver); - return _o; -} - -inline void EulerRotationParams::UnPackTo(EulerRotationParamsT *_o, const flatbuffers::resolver_function_t *_resolver) const { - (void)_o; - (void)_resolver; - { auto _e = r1(); _o->r1 = _e; }; - { auto _e = r2(); _o->r2 = _e; }; - { auto _e = r3(); _o->r3 = _e; }; - { auto _e = eulerOrder(); _o->eulerOrder = _e; }; -} - -inline flatbuffers::Offset EulerRotationParams::Pack(flatbuffers::FlatBufferBuilder &_fbb, const EulerRotationParamsT* _o, const flatbuffers::rehasher_function_t *_rehasher) { - return CreateEulerRotationParams(_fbb, _o, _rehasher); -} - -inline flatbuffers::Offset CreateEulerRotationParams(flatbuffers::FlatBufferBuilder &_fbb, const EulerRotationParamsT *_o, const flatbuffers::rehasher_function_t *_rehasher) { - (void)_rehasher; - (void)_o; - struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const EulerRotationParamsT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; - auto _r1 = _o->r1; - auto _r2 = _o->r2; - auto _r3 = _o->r3; - auto _eulerOrder = _o->eulerOrder; - return grl::flatbuffer::CreateEulerRotationParams( - _fbb, - _r1, - _r2, - _r3, - _eulerOrder); -} - -inline EulerPoseParamsT *EulerPoseParams::UnPack(const flatbuffers::resolver_function_t *_resolver) const { - auto _o = new EulerPoseParamsT(); - UnPackTo(_o, _resolver); - return _o; -} - -inline void EulerPoseParams::UnPackTo(EulerPoseParamsT *_o, const flatbuffers::resolver_function_t *_resolver) const { - (void)_o; - (void)_resolver; - { auto _e = position(); if (_e) _o->position = std::unique_ptr(new Vector3d(*_e)); }; - { auto _e = rotation(); if (_e) _o->rotation = std::unique_ptr(new EulerRotation(*_e)); }; -} - -inline flatbuffers::Offset EulerPoseParams::Pack(flatbuffers::FlatBufferBuilder &_fbb, const EulerPoseParamsT* _o, const flatbuffers::rehasher_function_t *_rehasher) { - return CreateEulerPoseParams(_fbb, _o, _rehasher); -} - -inline flatbuffers::Offset CreateEulerPoseParams(flatbuffers::FlatBufferBuilder &_fbb, const EulerPoseParamsT *_o, const flatbuffers::rehasher_function_t *_rehasher) { - (void)_rehasher; - (void)_o; - struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const EulerPoseParamsT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; - auto _position = _o->position ? _o->position.get() : 0; - auto _rotation = _o->rotation ? _o->rotation.get() : 0; - return grl::flatbuffer::CreateEulerPoseParams( - _fbb, - _position, - _rotation); -} - -} // namespace flatbuffer -} // namespace grl - -#endif // FLATBUFFERS_GENERATED_EULER_GRL_FLATBUFFER_H_ diff --git a/include/grl/flatbuffer/FusionTrack_generated.h b/include/grl/flatbuffer/FusionTrack_generated.h deleted file mode 100644 index ab0b966..0000000 --- a/include/grl/flatbuffer/FusionTrack_generated.h +++ /dev/null @@ -1,1803 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - - -#ifndef FLATBUFFERS_GENERATED_FUSIONTRACK_GRL_FLATBUFFER_H_ -#define FLATBUFFERS_GENERATED_FUSIONTRACK_GRL_FLATBUFFER_H_ - -#include "flatbuffers/flatbuffers.h" - -#include "Geometry_generated.h" -#include "Time_generated.h" - -namespace grl { -namespace flatbuffer { - -struct ftkGeometry; -struct ftkGeometryT; - -struct ftkMarker; -struct ftkMarkerT; - -struct ftk3DFiducial; -struct ftk3DFiducialT; - -struct ftkRegionOfInterest; -struct ftkRegionOfInterestT; - -struct FusionTrackFrame; -struct FusionTrackFrameT; - -struct FusionTrackParameters; -struct FusionTrackParametersT; - -struct FusionTrackMessage; -struct FusionTrackMessageT; - -enum class ftkQueryStatus : int32_t { - QS_WAR_SKIPPED = -1, - QS_OK = 0, - QS_ERR_OVERFLOW = 1, - QS_ERR_INVALID_RESERVED_SIZE = 2, - QS_REPROCESS = 10, - MIN = QS_WAR_SKIPPED, - MAX = QS_REPROCESS -}; - -inline const ftkQueryStatus (&EnumValuesftkQueryStatus())[5] { - static const ftkQueryStatus values[] = { - ftkQueryStatus::QS_WAR_SKIPPED, - ftkQueryStatus::QS_OK, - ftkQueryStatus::QS_ERR_OVERFLOW, - ftkQueryStatus::QS_ERR_INVALID_RESERVED_SIZE, - ftkQueryStatus::QS_REPROCESS - }; - return values; -} - -inline const char * const *EnumNamesftkQueryStatus() { - static const char * const names[] = { - "QS_WAR_SKIPPED", - "QS_OK", - "QS_ERR_OVERFLOW", - "QS_ERR_INVALID_RESERVED_SIZE", - "", - "", - "", - "", - "", - "", - "", - "QS_REPROCESS", - nullptr - }; - return names; -} - -inline const char *EnumNameftkQueryStatus(ftkQueryStatus e) { - const size_t index = static_cast(e) - static_cast(ftkQueryStatus::QS_WAR_SKIPPED); - return EnumNamesftkQueryStatus()[index]; -} - -enum class ftkDeviceType : int32_t { - DEV_SIMULATOR = 0, - DEV_INFINITRACK = 1, - DEV_FUSIONTRACK_500 = 2, - DEV_FUSIONTRACK_250 = 3, - DEV_UNKNOWN_DEVICE = 127, - MIN = DEV_SIMULATOR, - MAX = DEV_UNKNOWN_DEVICE -}; - -inline const ftkDeviceType (&EnumValuesftkDeviceType())[5] { - static const ftkDeviceType values[] = { - ftkDeviceType::DEV_SIMULATOR, - ftkDeviceType::DEV_INFINITRACK, - ftkDeviceType::DEV_FUSIONTRACK_500, - ftkDeviceType::DEV_FUSIONTRACK_250, - ftkDeviceType::DEV_UNKNOWN_DEVICE - }; - return values; -} - -struct ftkGeometryT : public flatbuffers::NativeTable { - typedef ftkGeometry TableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.ftkGeometryT"; - } - std::string name; - uint32_t geometryID; - uint32_t version; - std::vector positions; - ftkGeometryT() - : geometryID(0), - version(0) { - } -}; - -struct ftkGeometry FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { - typedef ftkGeometryT NativeTableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.ftkGeometry"; - } - enum { - VT_NAME = 4, - VT_GEOMETRYID = 6, - VT_VERSION = 8, - VT_POSITIONS = 10 - }; - const flatbuffers::String *name() const { - return GetPointer(VT_NAME); - } - uint32_t geometryID() const { - return GetField(VT_GEOMETRYID, 0); - } - uint32_t version() const { - return GetField(VT_VERSION, 0); - } - const flatbuffers::Vector *positions() const { - return GetPointer *>(VT_POSITIONS); - } - bool Verify(flatbuffers::Verifier &verifier) const { - return VerifyTableStart(verifier) && - VerifyOffset(verifier, VT_NAME) && - verifier.Verify(name()) && - VerifyField(verifier, VT_GEOMETRYID) && - VerifyField(verifier, VT_VERSION) && - VerifyOffset(verifier, VT_POSITIONS) && - verifier.Verify(positions()) && - verifier.EndTable(); - } - ftkGeometryT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; - void UnPackTo(ftkGeometryT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; - static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const ftkGeometryT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); -}; - -struct ftkGeometryBuilder { - flatbuffers::FlatBufferBuilder &fbb_; - flatbuffers::uoffset_t start_; - void add_name(flatbuffers::Offset name) { - fbb_.AddOffset(ftkGeometry::VT_NAME, name); - } - void add_geometryID(uint32_t geometryID) { - fbb_.AddElement(ftkGeometry::VT_GEOMETRYID, geometryID, 0); - } - void add_version(uint32_t version) { - fbb_.AddElement(ftkGeometry::VT_VERSION, version, 0); - } - void add_positions(flatbuffers::Offset> positions) { - fbb_.AddOffset(ftkGeometry::VT_POSITIONS, positions); - } - explicit ftkGeometryBuilder(flatbuffers::FlatBufferBuilder &_fbb) - : fbb_(_fbb) { - start_ = fbb_.StartTable(); - } - ftkGeometryBuilder &operator=(const ftkGeometryBuilder &); - flatbuffers::Offset Finish() { - const auto end = fbb_.EndTable(start_); - auto o = flatbuffers::Offset(end); - return o; - } -}; - -inline flatbuffers::Offset CreateftkGeometry( - flatbuffers::FlatBufferBuilder &_fbb, - flatbuffers::Offset name = 0, - uint32_t geometryID = 0, - uint32_t version = 0, - flatbuffers::Offset> positions = 0) { - ftkGeometryBuilder builder_(_fbb); - builder_.add_positions(positions); - builder_.add_version(version); - builder_.add_geometryID(geometryID); - builder_.add_name(name); - return builder_.Finish(); -} - -inline flatbuffers::Offset CreateftkGeometryDirect( - flatbuffers::FlatBufferBuilder &_fbb, - const char *name = nullptr, - uint32_t geometryID = 0, - uint32_t version = 0, - const std::vector *positions = nullptr) { - return grl::flatbuffer::CreateftkGeometry( - _fbb, - name ? _fbb.CreateString(name) : 0, - geometryID, - version, - positions ? _fbb.CreateVectorOfStructs(*positions) : 0); -} - -flatbuffers::Offset CreateftkGeometry(flatbuffers::FlatBufferBuilder &_fbb, const ftkGeometryT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); - -struct ftkMarkerT : public flatbuffers::NativeTable { - typedef ftkMarker TableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.ftkMarkerT"; - } - std::string name; - uint32_t ID; - uint32_t geometryID; - std::vector geometryPresenceMask; - std::unique_ptr transform; - ftkMarkerT() - : ID(0), - geometryID(0) { - } -}; - -struct ftkMarker FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { - typedef ftkMarkerT NativeTableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.ftkMarker"; - } - enum { - VT_NAME = 4, - VT_ID = 6, - VT_GEOMETRYID = 8, - VT_GEOMETRYPRESENCEMASK = 10, - VT_TRANSFORM = 12 - }; - const flatbuffers::String *name() const { - return GetPointer(VT_NAME); - } - uint32_t ID() const { - return GetField(VT_ID, 0); - } - uint32_t geometryID() const { - return GetField(VT_GEOMETRYID, 0); - } - const flatbuffers::Vector *geometryPresenceMask() const { - return GetPointer *>(VT_GEOMETRYPRESENCEMASK); - } - const Pose *transform() const { - return GetStruct(VT_TRANSFORM); - } - bool Verify(flatbuffers::Verifier &verifier) const { - return VerifyTableStart(verifier) && - VerifyOffset(verifier, VT_NAME) && - verifier.Verify(name()) && - VerifyField(verifier, VT_ID) && - VerifyField(verifier, VT_GEOMETRYID) && - VerifyOffset(verifier, VT_GEOMETRYPRESENCEMASK) && - verifier.Verify(geometryPresenceMask()) && - VerifyField(verifier, VT_TRANSFORM) && - verifier.EndTable(); - } - ftkMarkerT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; - void UnPackTo(ftkMarkerT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; - static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const ftkMarkerT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); -}; - -struct ftkMarkerBuilder { - flatbuffers::FlatBufferBuilder &fbb_; - flatbuffers::uoffset_t start_; - void add_name(flatbuffers::Offset name) { - fbb_.AddOffset(ftkMarker::VT_NAME, name); - } - void add_ID(uint32_t ID) { - fbb_.AddElement(ftkMarker::VT_ID, ID, 0); - } - void add_geometryID(uint32_t geometryID) { - fbb_.AddElement(ftkMarker::VT_GEOMETRYID, geometryID, 0); - } - void add_geometryPresenceMask(flatbuffers::Offset> geometryPresenceMask) { - fbb_.AddOffset(ftkMarker::VT_GEOMETRYPRESENCEMASK, geometryPresenceMask); - } - void add_transform(const Pose *transform) { - fbb_.AddStruct(ftkMarker::VT_TRANSFORM, transform); - } - explicit ftkMarkerBuilder(flatbuffers::FlatBufferBuilder &_fbb) - : fbb_(_fbb) { - start_ = fbb_.StartTable(); - } - ftkMarkerBuilder &operator=(const ftkMarkerBuilder &); - flatbuffers::Offset Finish() { - const auto end = fbb_.EndTable(start_); - auto o = flatbuffers::Offset(end); - return o; - } -}; - -inline flatbuffers::Offset CreateftkMarker( - flatbuffers::FlatBufferBuilder &_fbb, - flatbuffers::Offset name = 0, - uint32_t ID = 0, - uint32_t geometryID = 0, - flatbuffers::Offset> geometryPresenceMask = 0, - const Pose *transform = 0) { - ftkMarkerBuilder builder_(_fbb); - builder_.add_transform(transform); - builder_.add_geometryPresenceMask(geometryPresenceMask); - builder_.add_geometryID(geometryID); - builder_.add_ID(ID); - builder_.add_name(name); - return builder_.Finish(); -} - -inline flatbuffers::Offset CreateftkMarkerDirect( - flatbuffers::FlatBufferBuilder &_fbb, - const char *name = nullptr, - uint32_t ID = 0, - uint32_t geometryID = 0, - const std::vector *geometryPresenceMask = nullptr, - const Pose *transform = 0) { - return grl::flatbuffer::CreateftkMarker( - _fbb, - name ? _fbb.CreateString(name) : 0, - ID, - geometryID, - geometryPresenceMask ? _fbb.CreateVector(*geometryPresenceMask) : 0, - transform); -} - -flatbuffers::Offset CreateftkMarker(flatbuffers::FlatBufferBuilder &_fbb, const ftkMarkerT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); - -struct ftk3DFiducialT : public flatbuffers::NativeTable { - typedef ftk3DFiducial TableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.ftk3DFiducialT"; - } - uint32_t markerID; - uint32_t leftIndex; - uint32_t rightIndex; - std::unique_ptr position; - double epipolarErrorPixels; - double triangulationError; - double probability; - ftk3DFiducialT() - : markerID(0), - leftIndex(0), - rightIndex(0), - epipolarErrorPixels(0.0), - triangulationError(0.0), - probability(0.0) { - } -}; - -struct ftk3DFiducial FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { - typedef ftk3DFiducialT NativeTableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.ftk3DFiducial"; - } - enum { - VT_MARKERID = 4, - VT_LEFTINDEX = 6, - VT_RIGHTINDEX = 8, - VT_POSITION = 10, - VT_EPIPOLARERRORPIXELS = 12, - VT_TRIANGULATIONERROR = 14, - VT_PROBABILITY = 16 - }; - uint32_t markerID() const { - return GetField(VT_MARKERID, 0); - } - uint32_t leftIndex() const { - return GetField(VT_LEFTINDEX, 0); - } - uint32_t rightIndex() const { - return GetField(VT_RIGHTINDEX, 0); - } - const Vector3d *position() const { - return GetStruct(VT_POSITION); - } - double epipolarErrorPixels() const { - return GetField(VT_EPIPOLARERRORPIXELS, 0.0); - } - double triangulationError() const { - return GetField(VT_TRIANGULATIONERROR, 0.0); - } - double probability() const { - return GetField(VT_PROBABILITY, 0.0); - } - bool Verify(flatbuffers::Verifier &verifier) const { - return VerifyTableStart(verifier) && - VerifyField(verifier, VT_MARKERID) && - VerifyField(verifier, VT_LEFTINDEX) && - VerifyField(verifier, VT_RIGHTINDEX) && - VerifyField(verifier, VT_POSITION) && - VerifyField(verifier, VT_EPIPOLARERRORPIXELS) && - VerifyField(verifier, VT_TRIANGULATIONERROR) && - VerifyField(verifier, VT_PROBABILITY) && - verifier.EndTable(); - } - ftk3DFiducialT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; - void UnPackTo(ftk3DFiducialT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; - static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const ftk3DFiducialT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); -}; - -struct ftk3DFiducialBuilder { - flatbuffers::FlatBufferBuilder &fbb_; - flatbuffers::uoffset_t start_; - void add_markerID(uint32_t markerID) { - fbb_.AddElement(ftk3DFiducial::VT_MARKERID, markerID, 0); - } - void add_leftIndex(uint32_t leftIndex) { - fbb_.AddElement(ftk3DFiducial::VT_LEFTINDEX, leftIndex, 0); - } - void add_rightIndex(uint32_t rightIndex) { - fbb_.AddElement(ftk3DFiducial::VT_RIGHTINDEX, rightIndex, 0); - } - void add_position(const Vector3d *position) { - fbb_.AddStruct(ftk3DFiducial::VT_POSITION, position); - } - void add_epipolarErrorPixels(double epipolarErrorPixels) { - fbb_.AddElement(ftk3DFiducial::VT_EPIPOLARERRORPIXELS, epipolarErrorPixels, 0.0); - } - void add_triangulationError(double triangulationError) { - fbb_.AddElement(ftk3DFiducial::VT_TRIANGULATIONERROR, triangulationError, 0.0); - } - void add_probability(double probability) { - fbb_.AddElement(ftk3DFiducial::VT_PROBABILITY, probability, 0.0); - } - explicit ftk3DFiducialBuilder(flatbuffers::FlatBufferBuilder &_fbb) - : fbb_(_fbb) { - start_ = fbb_.StartTable(); - } - ftk3DFiducialBuilder &operator=(const ftk3DFiducialBuilder &); - flatbuffers::Offset Finish() { - const auto end = fbb_.EndTable(start_); - auto o = flatbuffers::Offset(end); - return o; - } -}; - -inline flatbuffers::Offset Createftk3DFiducial( - flatbuffers::FlatBufferBuilder &_fbb, - uint32_t markerID = 0, - uint32_t leftIndex = 0, - uint32_t rightIndex = 0, - const Vector3d *position = 0, - double epipolarErrorPixels = 0.0, - double triangulationError = 0.0, - double probability = 0.0) { - ftk3DFiducialBuilder builder_(_fbb); - builder_.add_probability(probability); - builder_.add_triangulationError(triangulationError); - builder_.add_epipolarErrorPixels(epipolarErrorPixels); - builder_.add_position(position); - builder_.add_rightIndex(rightIndex); - builder_.add_leftIndex(leftIndex); - builder_.add_markerID(markerID); - return builder_.Finish(); -} - -flatbuffers::Offset Createftk3DFiducial(flatbuffers::FlatBufferBuilder &_fbb, const ftk3DFiducialT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); - -struct ftkRegionOfInterestT : public flatbuffers::NativeTable { - typedef ftkRegionOfInterest TableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.ftkRegionOfInterestT"; - } - double centerXPixels; - double centerYPixels; - uint32_t RightEdge; - uint32_t BottomEdge; - uint32_t LeftEdge; - uint32_t TopEdge; - uint32_t pixelsCount; - double probability; - ftkRegionOfInterestT() - : centerXPixels(0.0), - centerYPixels(0.0), - RightEdge(0), - BottomEdge(0), - LeftEdge(0), - TopEdge(0), - pixelsCount(0), - probability(0.0) { - } -}; - -struct ftkRegionOfInterest FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { - typedef ftkRegionOfInterestT NativeTableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.ftkRegionOfInterest"; - } - enum { - VT_CENTERXPIXELS = 4, - VT_CENTERYPIXELS = 6, - VT_RIGHTEDGE = 8, - VT_BOTTOMEDGE = 10, - VT_LEFTEDGE = 12, - VT_TOPEDGE = 14, - VT_PIXELSCOUNT = 16, - VT_PROBABILITY = 18 - }; - double centerXPixels() const { - return GetField(VT_CENTERXPIXELS, 0.0); - } - double centerYPixels() const { - return GetField(VT_CENTERYPIXELS, 0.0); - } - uint32_t RightEdge() const { - return GetField(VT_RIGHTEDGE, 0); - } - uint32_t BottomEdge() const { - return GetField(VT_BOTTOMEDGE, 0); - } - uint32_t LeftEdge() const { - return GetField(VT_LEFTEDGE, 0); - } - uint32_t TopEdge() const { - return GetField(VT_TOPEDGE, 0); - } - uint32_t pixelsCount() const { - return GetField(VT_PIXELSCOUNT, 0); - } - double probability() const { - return GetField(VT_PROBABILITY, 0.0); - } - bool Verify(flatbuffers::Verifier &verifier) const { - return VerifyTableStart(verifier) && - VerifyField(verifier, VT_CENTERXPIXELS) && - VerifyField(verifier, VT_CENTERYPIXELS) && - VerifyField(verifier, VT_RIGHTEDGE) && - VerifyField(verifier, VT_BOTTOMEDGE) && - VerifyField(verifier, VT_LEFTEDGE) && - VerifyField(verifier, VT_TOPEDGE) && - VerifyField(verifier, VT_PIXELSCOUNT) && - VerifyField(verifier, VT_PROBABILITY) && - verifier.EndTable(); - } - ftkRegionOfInterestT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; - void UnPackTo(ftkRegionOfInterestT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; - static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const ftkRegionOfInterestT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); -}; - -struct ftkRegionOfInterestBuilder { - flatbuffers::FlatBufferBuilder &fbb_; - flatbuffers::uoffset_t start_; - void add_centerXPixels(double centerXPixels) { - fbb_.AddElement(ftkRegionOfInterest::VT_CENTERXPIXELS, centerXPixels, 0.0); - } - void add_centerYPixels(double centerYPixels) { - fbb_.AddElement(ftkRegionOfInterest::VT_CENTERYPIXELS, centerYPixels, 0.0); - } - void add_RightEdge(uint32_t RightEdge) { - fbb_.AddElement(ftkRegionOfInterest::VT_RIGHTEDGE, RightEdge, 0); - } - void add_BottomEdge(uint32_t BottomEdge) { - fbb_.AddElement(ftkRegionOfInterest::VT_BOTTOMEDGE, BottomEdge, 0); - } - void add_LeftEdge(uint32_t LeftEdge) { - fbb_.AddElement(ftkRegionOfInterest::VT_LEFTEDGE, LeftEdge, 0); - } - void add_TopEdge(uint32_t TopEdge) { - fbb_.AddElement(ftkRegionOfInterest::VT_TOPEDGE, TopEdge, 0); - } - void add_pixelsCount(uint32_t pixelsCount) { - fbb_.AddElement(ftkRegionOfInterest::VT_PIXELSCOUNT, pixelsCount, 0); - } - void add_probability(double probability) { - fbb_.AddElement(ftkRegionOfInterest::VT_PROBABILITY, probability, 0.0); - } - explicit ftkRegionOfInterestBuilder(flatbuffers::FlatBufferBuilder &_fbb) - : fbb_(_fbb) { - start_ = fbb_.StartTable(); - } - ftkRegionOfInterestBuilder &operator=(const ftkRegionOfInterestBuilder &); - flatbuffers::Offset Finish() { - const auto end = fbb_.EndTable(start_); - auto o = flatbuffers::Offset(end); - return o; - } -}; - -inline flatbuffers::Offset CreateftkRegionOfInterest( - flatbuffers::FlatBufferBuilder &_fbb, - double centerXPixels = 0.0, - double centerYPixels = 0.0, - uint32_t RightEdge = 0, - uint32_t BottomEdge = 0, - uint32_t LeftEdge = 0, - uint32_t TopEdge = 0, - uint32_t pixelsCount = 0, - double probability = 0.0) { - ftkRegionOfInterestBuilder builder_(_fbb); - builder_.add_probability(probability); - builder_.add_centerYPixels(centerYPixels); - builder_.add_centerXPixels(centerXPixels); - builder_.add_pixelsCount(pixelsCount); - builder_.add_TopEdge(TopEdge); - builder_.add_LeftEdge(LeftEdge); - builder_.add_BottomEdge(BottomEdge); - builder_.add_RightEdge(RightEdge); - return builder_.Finish(); -} - -flatbuffers::Offset CreateftkRegionOfInterest(flatbuffers::FlatBufferBuilder &_fbb, const ftkRegionOfInterestT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); - -struct FusionTrackFrameT : public flatbuffers::NativeTable { - typedef FusionTrackFrame TableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.FusionTrackFrameT"; - } - double timestamp; - uint64_t serialNumber; - uint64_t hardwareTimestampUS; - uint64_t desynchroUS; - uint32_t counter; - uint32_t format; - uint32_t width; - uint32_t height; - int32_t imageStrideInBytes; - uint32_t imageHeaderVersion; - int32_t imageHeaderStatus; - std::string imageLeftPixels; - uint32_t imageLeftPixelsVersion; - int32_t imageLeftStatus; - std::string imageRightPixels; - uint32_t imageRightPixelsVersion; - int32_t imageRightStatus; - std::vector> regionsOfInterestLeft; - uint32_t regionsOfInterestLeftVersion; - int32_t regionsOfInterestLeftStatus; - std::vector> regionsOfInterestRight; - uint32_t regionsOfInterestRightVersion; - int32_t regionsOfInterestRightStatus; - std::vector> threeDFiducials; - uint32_t threeDFiducialsVersion; - int32_t threeDFiducialsStatus; - std::vector> markers; - uint32_t markersVersion; - int32_t markersStatus; - int32_t deviceType; - int64_t ftkError; - FusionTrackFrameT() - : timestamp(0.0), - serialNumber(0), - hardwareTimestampUS(0), - desynchroUS(0), - counter(0), - format(0), - width(0), - height(0), - imageStrideInBytes(0), - imageHeaderVersion(0), - imageHeaderStatus(0), - imageLeftPixelsVersion(0), - imageLeftStatus(0), - imageRightPixelsVersion(0), - imageRightStatus(0), - regionsOfInterestLeftVersion(0), - regionsOfInterestLeftStatus(0), - regionsOfInterestRightVersion(0), - regionsOfInterestRightStatus(0), - threeDFiducialsVersion(0), - threeDFiducialsStatus(0), - markersVersion(0), - markersStatus(0), - deviceType(0), - ftkError(0) { - } -}; - -/// Data for one frame capture -/// On the Atracsys FusionTrack optical tracker -/// look at ftkInterface.h for details -/// Frame class is defined in FusionTrack.hpp -struct FusionTrackFrame FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { - typedef FusionTrackFrameT NativeTableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.FusionTrackFrame"; - } - enum { - VT_TIMESTAMP = 4, - VT_SERIALNUMBER = 6, - VT_HARDWARETIMESTAMPUS = 8, - VT_DESYNCHROUS = 10, - VT_COUNTER = 12, - VT_FORMAT = 14, - VT_WIDTH = 16, - VT_HEIGHT = 18, - VT_IMAGESTRIDEINBYTES = 20, - VT_IMAGEHEADERVERSION = 22, - VT_IMAGEHEADERSTATUS = 24, - VT_IMAGELEFTPIXELS = 26, - VT_IMAGELEFTPIXELSVERSION = 28, - VT_IMAGELEFTSTATUS = 30, - VT_IMAGERIGHTPIXELS = 32, - VT_IMAGERIGHTPIXELSVERSION = 34, - VT_IMAGERIGHTSTATUS = 36, - VT_REGIONSOFINTERESTLEFT = 38, - VT_REGIONSOFINTERESTLEFTVERSION = 40, - VT_REGIONSOFINTERESTLEFTSTATUS = 42, - VT_REGIONSOFINTERESTRIGHT = 44, - VT_REGIONSOFINTERESTRIGHTVERSION = 46, - VT_REGIONSOFINTERESTRIGHTSTATUS = 48, - VT_THREEDFIDUCIALS = 50, - VT_THREEDFIDUCIALSVERSION = 52, - VT_THREEDFIDUCIALSSTATUS = 54, - VT_MARKERS = 56, - VT_MARKERSVERSION = 58, - VT_MARKERSSTATUS = 60, - VT_DEVICETYPE = 62, - VT_FTKERROR = 64 - }; - double timestamp() const { - return GetField(VT_TIMESTAMP, 0.0); - } - uint64_t serialNumber() const { - return GetField(VT_SERIALNUMBER, 0); - } - uint64_t hardwareTimestampUS() const { - return GetField(VT_HARDWARETIMESTAMPUS, 0); - } - uint64_t desynchroUS() const { - return GetField(VT_DESYNCHROUS, 0); - } - uint32_t counter() const { - return GetField(VT_COUNTER, 0); - } - uint32_t format() const { - return GetField(VT_FORMAT, 0); - } - uint32_t width() const { - return GetField(VT_WIDTH, 0); - } - uint32_t height() const { - return GetField(VT_HEIGHT, 0); - } - int32_t imageStrideInBytes() const { - return GetField(VT_IMAGESTRIDEINBYTES, 0); - } - uint32_t imageHeaderVersion() const { - return GetField(VT_IMAGEHEADERVERSION, 0); - } - int32_t imageHeaderStatus() const { - return GetField(VT_IMAGEHEADERSTATUS, 0); - } - const flatbuffers::String *imageLeftPixels() const { - return GetPointer(VT_IMAGELEFTPIXELS); - } - uint32_t imageLeftPixelsVersion() const { - return GetField(VT_IMAGELEFTPIXELSVERSION, 0); - } - int32_t imageLeftStatus() const { - return GetField(VT_IMAGELEFTSTATUS, 0); - } - const flatbuffers::String *imageRightPixels() const { - return GetPointer(VT_IMAGERIGHTPIXELS); - } - uint32_t imageRightPixelsVersion() const { - return GetField(VT_IMAGERIGHTPIXELSVERSION, 0); - } - int32_t imageRightStatus() const { - return GetField(VT_IMAGERIGHTSTATUS, 0); - } - const flatbuffers::Vector> *regionsOfInterestLeft() const { - return GetPointer> *>(VT_REGIONSOFINTERESTLEFT); - } - uint32_t regionsOfInterestLeftVersion() const { - return GetField(VT_REGIONSOFINTERESTLEFTVERSION, 0); - } - int32_t regionsOfInterestLeftStatus() const { - return GetField(VT_REGIONSOFINTERESTLEFTSTATUS, 0); - } - const flatbuffers::Vector> *regionsOfInterestRight() const { - return GetPointer> *>(VT_REGIONSOFINTERESTRIGHT); - } - uint32_t regionsOfInterestRightVersion() const { - return GetField(VT_REGIONSOFINTERESTRIGHTVERSION, 0); - } - int32_t regionsOfInterestRightStatus() const { - return GetField(VT_REGIONSOFINTERESTRIGHTSTATUS, 0); - } - const flatbuffers::Vector> *threeDFiducials() const { - return GetPointer> *>(VT_THREEDFIDUCIALS); - } - uint32_t threeDFiducialsVersion() const { - return GetField(VT_THREEDFIDUCIALSVERSION, 0); - } - int32_t threeDFiducialsStatus() const { - return GetField(VT_THREEDFIDUCIALSSTATUS, 0); - } - const flatbuffers::Vector> *markers() const { - return GetPointer> *>(VT_MARKERS); - } - uint32_t markersVersion() const { - return GetField(VT_MARKERSVERSION, 0); - } - int32_t markersStatus() const { - return GetField(VT_MARKERSSTATUS, 0); - } - int32_t deviceType() const { - return GetField(VT_DEVICETYPE, 0); - } - int64_t ftkError() const { - return GetField(VT_FTKERROR, 0); - } - bool Verify(flatbuffers::Verifier &verifier) const { - return VerifyTableStart(verifier) && - VerifyField(verifier, VT_TIMESTAMP) && - VerifyField(verifier, VT_SERIALNUMBER) && - VerifyField(verifier, VT_HARDWARETIMESTAMPUS) && - VerifyField(verifier, VT_DESYNCHROUS) && - VerifyField(verifier, VT_COUNTER) && - VerifyField(verifier, VT_FORMAT) && - VerifyField(verifier, VT_WIDTH) && - VerifyField(verifier, VT_HEIGHT) && - VerifyField(verifier, VT_IMAGESTRIDEINBYTES) && - VerifyField(verifier, VT_IMAGEHEADERVERSION) && - VerifyField(verifier, VT_IMAGEHEADERSTATUS) && - VerifyOffset(verifier, VT_IMAGELEFTPIXELS) && - verifier.Verify(imageLeftPixels()) && - VerifyField(verifier, VT_IMAGELEFTPIXELSVERSION) && - VerifyField(verifier, VT_IMAGELEFTSTATUS) && - VerifyOffset(verifier, VT_IMAGERIGHTPIXELS) && - verifier.Verify(imageRightPixels()) && - VerifyField(verifier, VT_IMAGERIGHTPIXELSVERSION) && - VerifyField(verifier, VT_IMAGERIGHTSTATUS) && - VerifyOffset(verifier, VT_REGIONSOFINTERESTLEFT) && - verifier.Verify(regionsOfInterestLeft()) && - verifier.VerifyVectorOfTables(regionsOfInterestLeft()) && - VerifyField(verifier, VT_REGIONSOFINTERESTLEFTVERSION) && - VerifyField(verifier, VT_REGIONSOFINTERESTLEFTSTATUS) && - VerifyOffset(verifier, VT_REGIONSOFINTERESTRIGHT) && - verifier.Verify(regionsOfInterestRight()) && - verifier.VerifyVectorOfTables(regionsOfInterestRight()) && - VerifyField(verifier, VT_REGIONSOFINTERESTRIGHTVERSION) && - VerifyField(verifier, VT_REGIONSOFINTERESTRIGHTSTATUS) && - VerifyOffset(verifier, VT_THREEDFIDUCIALS) && - verifier.Verify(threeDFiducials()) && - verifier.VerifyVectorOfTables(threeDFiducials()) && - VerifyField(verifier, VT_THREEDFIDUCIALSVERSION) && - VerifyField(verifier, VT_THREEDFIDUCIALSSTATUS) && - VerifyOffset(verifier, VT_MARKERS) && - verifier.Verify(markers()) && - verifier.VerifyVectorOfTables(markers()) && - VerifyField(verifier, VT_MARKERSVERSION) && - VerifyField(verifier, VT_MARKERSSTATUS) && - VerifyField(verifier, VT_DEVICETYPE) && - VerifyField(verifier, VT_FTKERROR) && - verifier.EndTable(); - } - FusionTrackFrameT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; - void UnPackTo(FusionTrackFrameT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; - static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const FusionTrackFrameT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); -}; - -struct FusionTrackFrameBuilder { - flatbuffers::FlatBufferBuilder &fbb_; - flatbuffers::uoffset_t start_; - void add_timestamp(double timestamp) { - fbb_.AddElement(FusionTrackFrame::VT_TIMESTAMP, timestamp, 0.0); - } - void add_serialNumber(uint64_t serialNumber) { - fbb_.AddElement(FusionTrackFrame::VT_SERIALNUMBER, serialNumber, 0); - } - void add_hardwareTimestampUS(uint64_t hardwareTimestampUS) { - fbb_.AddElement(FusionTrackFrame::VT_HARDWARETIMESTAMPUS, hardwareTimestampUS, 0); - } - void add_desynchroUS(uint64_t desynchroUS) { - fbb_.AddElement(FusionTrackFrame::VT_DESYNCHROUS, desynchroUS, 0); - } - void add_counter(uint32_t counter) { - fbb_.AddElement(FusionTrackFrame::VT_COUNTER, counter, 0); - } - void add_format(uint32_t format) { - fbb_.AddElement(FusionTrackFrame::VT_FORMAT, format, 0); - } - void add_width(uint32_t width) { - fbb_.AddElement(FusionTrackFrame::VT_WIDTH, width, 0); - } - void add_height(uint32_t height) { - fbb_.AddElement(FusionTrackFrame::VT_HEIGHT, height, 0); - } - void add_imageStrideInBytes(int32_t imageStrideInBytes) { - fbb_.AddElement(FusionTrackFrame::VT_IMAGESTRIDEINBYTES, imageStrideInBytes, 0); - } - void add_imageHeaderVersion(uint32_t imageHeaderVersion) { - fbb_.AddElement(FusionTrackFrame::VT_IMAGEHEADERVERSION, imageHeaderVersion, 0); - } - void add_imageHeaderStatus(int32_t imageHeaderStatus) { - fbb_.AddElement(FusionTrackFrame::VT_IMAGEHEADERSTATUS, imageHeaderStatus, 0); - } - void add_imageLeftPixels(flatbuffers::Offset imageLeftPixels) { - fbb_.AddOffset(FusionTrackFrame::VT_IMAGELEFTPIXELS, imageLeftPixels); - } - void add_imageLeftPixelsVersion(uint32_t imageLeftPixelsVersion) { - fbb_.AddElement(FusionTrackFrame::VT_IMAGELEFTPIXELSVERSION, imageLeftPixelsVersion, 0); - } - void add_imageLeftStatus(int32_t imageLeftStatus) { - fbb_.AddElement(FusionTrackFrame::VT_IMAGELEFTSTATUS, imageLeftStatus, 0); - } - void add_imageRightPixels(flatbuffers::Offset imageRightPixels) { - fbb_.AddOffset(FusionTrackFrame::VT_IMAGERIGHTPIXELS, imageRightPixels); - } - void add_imageRightPixelsVersion(uint32_t imageRightPixelsVersion) { - fbb_.AddElement(FusionTrackFrame::VT_IMAGERIGHTPIXELSVERSION, imageRightPixelsVersion, 0); - } - void add_imageRightStatus(int32_t imageRightStatus) { - fbb_.AddElement(FusionTrackFrame::VT_IMAGERIGHTSTATUS, imageRightStatus, 0); - } - void add_regionsOfInterestLeft(flatbuffers::Offset>> regionsOfInterestLeft) { - fbb_.AddOffset(FusionTrackFrame::VT_REGIONSOFINTERESTLEFT, regionsOfInterestLeft); - } - void add_regionsOfInterestLeftVersion(uint32_t regionsOfInterestLeftVersion) { - fbb_.AddElement(FusionTrackFrame::VT_REGIONSOFINTERESTLEFTVERSION, regionsOfInterestLeftVersion, 0); - } - void add_regionsOfInterestLeftStatus(int32_t regionsOfInterestLeftStatus) { - fbb_.AddElement(FusionTrackFrame::VT_REGIONSOFINTERESTLEFTSTATUS, regionsOfInterestLeftStatus, 0); - } - void add_regionsOfInterestRight(flatbuffers::Offset>> regionsOfInterestRight) { - fbb_.AddOffset(FusionTrackFrame::VT_REGIONSOFINTERESTRIGHT, regionsOfInterestRight); - } - void add_regionsOfInterestRightVersion(uint32_t regionsOfInterestRightVersion) { - fbb_.AddElement(FusionTrackFrame::VT_REGIONSOFINTERESTRIGHTVERSION, regionsOfInterestRightVersion, 0); - } - void add_regionsOfInterestRightStatus(int32_t regionsOfInterestRightStatus) { - fbb_.AddElement(FusionTrackFrame::VT_REGIONSOFINTERESTRIGHTSTATUS, regionsOfInterestRightStatus, 0); - } - void add_threeDFiducials(flatbuffers::Offset>> threeDFiducials) { - fbb_.AddOffset(FusionTrackFrame::VT_THREEDFIDUCIALS, threeDFiducials); - } - void add_threeDFiducialsVersion(uint32_t threeDFiducialsVersion) { - fbb_.AddElement(FusionTrackFrame::VT_THREEDFIDUCIALSVERSION, threeDFiducialsVersion, 0); - } - void add_threeDFiducialsStatus(int32_t threeDFiducialsStatus) { - fbb_.AddElement(FusionTrackFrame::VT_THREEDFIDUCIALSSTATUS, threeDFiducialsStatus, 0); - } - void add_markers(flatbuffers::Offset>> markers) { - fbb_.AddOffset(FusionTrackFrame::VT_MARKERS, markers); - } - void add_markersVersion(uint32_t markersVersion) { - fbb_.AddElement(FusionTrackFrame::VT_MARKERSVERSION, markersVersion, 0); - } - void add_markersStatus(int32_t markersStatus) { - fbb_.AddElement(FusionTrackFrame::VT_MARKERSSTATUS, markersStatus, 0); - } - void add_deviceType(int32_t deviceType) { - fbb_.AddElement(FusionTrackFrame::VT_DEVICETYPE, deviceType, 0); - } - void add_ftkError(int64_t ftkError) { - fbb_.AddElement(FusionTrackFrame::VT_FTKERROR, ftkError, 0); - } - explicit FusionTrackFrameBuilder(flatbuffers::FlatBufferBuilder &_fbb) - : fbb_(_fbb) { - start_ = fbb_.StartTable(); - } - FusionTrackFrameBuilder &operator=(const FusionTrackFrameBuilder &); - flatbuffers::Offset Finish() { - const auto end = fbb_.EndTable(start_); - auto o = flatbuffers::Offset(end); - return o; - } -}; - -inline flatbuffers::Offset CreateFusionTrackFrame( - flatbuffers::FlatBufferBuilder &_fbb, - double timestamp = 0.0, - uint64_t serialNumber = 0, - uint64_t hardwareTimestampUS = 0, - uint64_t desynchroUS = 0, - uint32_t counter = 0, - uint32_t format = 0, - uint32_t width = 0, - uint32_t height = 0, - int32_t imageStrideInBytes = 0, - uint32_t imageHeaderVersion = 0, - int32_t imageHeaderStatus = 0, - flatbuffers::Offset imageLeftPixels = 0, - uint32_t imageLeftPixelsVersion = 0, - int32_t imageLeftStatus = 0, - flatbuffers::Offset imageRightPixels = 0, - uint32_t imageRightPixelsVersion = 0, - int32_t imageRightStatus = 0, - flatbuffers::Offset>> regionsOfInterestLeft = 0, - uint32_t regionsOfInterestLeftVersion = 0, - int32_t regionsOfInterestLeftStatus = 0, - flatbuffers::Offset>> regionsOfInterestRight = 0, - uint32_t regionsOfInterestRightVersion = 0, - int32_t regionsOfInterestRightStatus = 0, - flatbuffers::Offset>> threeDFiducials = 0, - uint32_t threeDFiducialsVersion = 0, - int32_t threeDFiducialsStatus = 0, - flatbuffers::Offset>> markers = 0, - uint32_t markersVersion = 0, - int32_t markersStatus = 0, - int32_t deviceType = 0, - int64_t ftkError = 0) { - FusionTrackFrameBuilder builder_(_fbb); - builder_.add_ftkError(ftkError); - builder_.add_desynchroUS(desynchroUS); - builder_.add_hardwareTimestampUS(hardwareTimestampUS); - builder_.add_serialNumber(serialNumber); - builder_.add_timestamp(timestamp); - builder_.add_deviceType(deviceType); - builder_.add_markersStatus(markersStatus); - builder_.add_markersVersion(markersVersion); - builder_.add_markers(markers); - builder_.add_threeDFiducialsStatus(threeDFiducialsStatus); - builder_.add_threeDFiducialsVersion(threeDFiducialsVersion); - builder_.add_threeDFiducials(threeDFiducials); - builder_.add_regionsOfInterestRightStatus(regionsOfInterestRightStatus); - builder_.add_regionsOfInterestRightVersion(regionsOfInterestRightVersion); - builder_.add_regionsOfInterestRight(regionsOfInterestRight); - builder_.add_regionsOfInterestLeftStatus(regionsOfInterestLeftStatus); - builder_.add_regionsOfInterestLeftVersion(regionsOfInterestLeftVersion); - builder_.add_regionsOfInterestLeft(regionsOfInterestLeft); - builder_.add_imageRightStatus(imageRightStatus); - builder_.add_imageRightPixelsVersion(imageRightPixelsVersion); - builder_.add_imageRightPixels(imageRightPixels); - builder_.add_imageLeftStatus(imageLeftStatus); - builder_.add_imageLeftPixelsVersion(imageLeftPixelsVersion); - builder_.add_imageLeftPixels(imageLeftPixels); - builder_.add_imageHeaderStatus(imageHeaderStatus); - builder_.add_imageHeaderVersion(imageHeaderVersion); - builder_.add_imageStrideInBytes(imageStrideInBytes); - builder_.add_height(height); - builder_.add_width(width); - builder_.add_format(format); - builder_.add_counter(counter); - return builder_.Finish(); -} - -inline flatbuffers::Offset CreateFusionTrackFrameDirect( - flatbuffers::FlatBufferBuilder &_fbb, - double timestamp = 0.0, - uint64_t serialNumber = 0, - uint64_t hardwareTimestampUS = 0, - uint64_t desynchroUS = 0, - uint32_t counter = 0, - uint32_t format = 0, - uint32_t width = 0, - uint32_t height = 0, - int32_t imageStrideInBytes = 0, - uint32_t imageHeaderVersion = 0, - int32_t imageHeaderStatus = 0, - const char *imageLeftPixels = nullptr, - uint32_t imageLeftPixelsVersion = 0, - int32_t imageLeftStatus = 0, - const char *imageRightPixels = nullptr, - uint32_t imageRightPixelsVersion = 0, - int32_t imageRightStatus = 0, - const std::vector> *regionsOfInterestLeft = nullptr, - uint32_t regionsOfInterestLeftVersion = 0, - int32_t regionsOfInterestLeftStatus = 0, - const std::vector> *regionsOfInterestRight = nullptr, - uint32_t regionsOfInterestRightVersion = 0, - int32_t regionsOfInterestRightStatus = 0, - const std::vector> *threeDFiducials = nullptr, - uint32_t threeDFiducialsVersion = 0, - int32_t threeDFiducialsStatus = 0, - const std::vector> *markers = nullptr, - uint32_t markersVersion = 0, - int32_t markersStatus = 0, - int32_t deviceType = 0, - int64_t ftkError = 0) { - return grl::flatbuffer::CreateFusionTrackFrame( - _fbb, - timestamp, - serialNumber, - hardwareTimestampUS, - desynchroUS, - counter, - format, - width, - height, - imageStrideInBytes, - imageHeaderVersion, - imageHeaderStatus, - imageLeftPixels ? _fbb.CreateString(imageLeftPixels) : 0, - imageLeftPixelsVersion, - imageLeftStatus, - imageRightPixels ? _fbb.CreateString(imageRightPixels) : 0, - imageRightPixelsVersion, - imageRightStatus, - regionsOfInterestLeft ? _fbb.CreateVector>(*regionsOfInterestLeft) : 0, - regionsOfInterestLeftVersion, - regionsOfInterestLeftStatus, - regionsOfInterestRight ? _fbb.CreateVector>(*regionsOfInterestRight) : 0, - regionsOfInterestRightVersion, - regionsOfInterestRightStatus, - threeDFiducials ? _fbb.CreateVector>(*threeDFiducials) : 0, - threeDFiducialsVersion, - threeDFiducialsStatus, - markers ? _fbb.CreateVector>(*markers) : 0, - markersVersion, - markersStatus, - deviceType, - ftkError); -} - -flatbuffers::Offset CreateFusionTrackFrame(flatbuffers::FlatBufferBuilder &_fbb, const FusionTrackFrameT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); - -struct FusionTrackParametersT : public flatbuffers::NativeTable { - typedef FusionTrackParameters TableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.FusionTrackParametersT"; - } - std::string name; - std::string deviceClockID; - std::string localClockID; - std::vector> geometries; - std::vector geometryFilenames; - std::string geometryDir; - std::vector TrackerDeviceIDs; - std::vector markerIDs; - std::vector markerNames; - std::vector m_deviceSerialNumbers; - std::vector m_device_types; - FusionTrackParametersT() { - } -}; - -struct FusionTrackParameters FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { - typedef FusionTrackParametersT NativeTableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.FusionTrackParameters"; - } - enum { - VT_NAME = 4, - VT_DEVICECLOCKID = 6, - VT_LOCALCLOCKID = 8, - VT_GEOMETRIES = 10, - VT_GEOMETRYFILENAMES = 12, - VT_GEOMETRYDIR = 14, - VT_TRACKERDEVICEIDS = 16, - VT_MARKERIDS = 18, - VT_MARKERNAMES = 20, - VT_M_DEVICESERIALNUMBERS = 22, - VT_M_DEVICE_TYPES = 24 - }; - /// Name for this connection / FusionTrack driver instance - /// useful for debugging and when multiple data sources are used - const flatbuffers::String *name() const { - return GetPointer(VT_NAME); - } - /// Name for the clock on the FusionTrack - /// Useful for timing calculations and debugging. - const flatbuffers::String *deviceClockID() const { - return GetPointer(VT_DEVICECLOCKID); - } - /// Name for the local clock on which this driver runs - /// Useful for timing calculations and debugging. - const flatbuffers::String *localClockID() const { - return GetPointer(VT_LOCALCLOCKID); - } - /// dimensions of the markers that may be present - const flatbuffers::Vector> *geometries() const { - return GetPointer> *>(VT_GEOMETRIES); - } - /// Geometries aka fiducials aka markers to be loaded from ini files. - /// The data loaded should not repeat IDs from MarkerIDs. - const flatbuffers::Vector> *geometryFilenames() const { - return GetPointer> *>(VT_GEOMETRYFILENAMES); - } - /// Path to the directory with the marker ini files listed above - /// Uses the default current working directory if empty - /// geometryDir:[string]; - const flatbuffers::String *geometryDir() const { - return GetPointer(VT_GEOMETRYDIR); - } - /// Optional list of optical tracker device ids to expect - /// will be loaded automatically if empty - const flatbuffers::Vector *TrackerDeviceIDs() const { - return GetPointer *>(VT_TRACKERDEVICEIDS); - } - /// Marker geometry unique integer IDs - const flatbuffers::Vector *markerIDs() const { - return GetPointer *>(VT_MARKERIDS); - } - /// Optional Marker geometry names with one for each ID, none otherwise - const flatbuffers::Vector> *markerNames() const { - return GetPointer> *>(VT_MARKERNAMES); - } - const flatbuffers::Vector *m_deviceSerialNumbers() const { - return GetPointer *>(VT_M_DEVICESERIALNUMBERS); - } - const flatbuffers::Vector *m_device_types() const { - return GetPointer *>(VT_M_DEVICE_TYPES); - } - bool Verify(flatbuffers::Verifier &verifier) const { - return VerifyTableStart(verifier) && - VerifyOffset(verifier, VT_NAME) && - verifier.Verify(name()) && - VerifyOffset(verifier, VT_DEVICECLOCKID) && - verifier.Verify(deviceClockID()) && - VerifyOffset(verifier, VT_LOCALCLOCKID) && - verifier.Verify(localClockID()) && - VerifyOffset(verifier, VT_GEOMETRIES) && - verifier.Verify(geometries()) && - verifier.VerifyVectorOfTables(geometries()) && - VerifyOffset(verifier, VT_GEOMETRYFILENAMES) && - verifier.Verify(geometryFilenames()) && - verifier.VerifyVectorOfStrings(geometryFilenames()) && - VerifyOffset(verifier, VT_GEOMETRYDIR) && - verifier.Verify(geometryDir()) && - VerifyOffset(verifier, VT_TRACKERDEVICEIDS) && - verifier.Verify(TrackerDeviceIDs()) && - VerifyOffset(verifier, VT_MARKERIDS) && - verifier.Verify(markerIDs()) && - VerifyOffset(verifier, VT_MARKERNAMES) && - verifier.Verify(markerNames()) && - verifier.VerifyVectorOfStrings(markerNames()) && - VerifyOffset(verifier, VT_M_DEVICESERIALNUMBERS) && - verifier.Verify(m_deviceSerialNumbers()) && - VerifyOffset(verifier, VT_M_DEVICE_TYPES) && - verifier.Verify(m_device_types()) && - verifier.EndTable(); - } - FusionTrackParametersT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; - void UnPackTo(FusionTrackParametersT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; - static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const FusionTrackParametersT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); -}; - -struct FusionTrackParametersBuilder { - flatbuffers::FlatBufferBuilder &fbb_; - flatbuffers::uoffset_t start_; - void add_name(flatbuffers::Offset name) { - fbb_.AddOffset(FusionTrackParameters::VT_NAME, name); - } - void add_deviceClockID(flatbuffers::Offset deviceClockID) { - fbb_.AddOffset(FusionTrackParameters::VT_DEVICECLOCKID, deviceClockID); - } - void add_localClockID(flatbuffers::Offset localClockID) { - fbb_.AddOffset(FusionTrackParameters::VT_LOCALCLOCKID, localClockID); - } - void add_geometries(flatbuffers::Offset>> geometries) { - fbb_.AddOffset(FusionTrackParameters::VT_GEOMETRIES, geometries); - } - void add_geometryFilenames(flatbuffers::Offset>> geometryFilenames) { - fbb_.AddOffset(FusionTrackParameters::VT_GEOMETRYFILENAMES, geometryFilenames); - } - void add_geometryDir(flatbuffers::Offset geometryDir) { - fbb_.AddOffset(FusionTrackParameters::VT_GEOMETRYDIR, geometryDir); - } - void add_TrackerDeviceIDs(flatbuffers::Offset> TrackerDeviceIDs) { - fbb_.AddOffset(FusionTrackParameters::VT_TRACKERDEVICEIDS, TrackerDeviceIDs); - } - void add_markerIDs(flatbuffers::Offset> markerIDs) { - fbb_.AddOffset(FusionTrackParameters::VT_MARKERIDS, markerIDs); - } - void add_markerNames(flatbuffers::Offset>> markerNames) { - fbb_.AddOffset(FusionTrackParameters::VT_MARKERNAMES, markerNames); - } - void add_m_deviceSerialNumbers(flatbuffers::Offset> m_deviceSerialNumbers) { - fbb_.AddOffset(FusionTrackParameters::VT_M_DEVICESERIALNUMBERS, m_deviceSerialNumbers); - } - void add_m_device_types(flatbuffers::Offset> m_device_types) { - fbb_.AddOffset(FusionTrackParameters::VT_M_DEVICE_TYPES, m_device_types); - } - explicit FusionTrackParametersBuilder(flatbuffers::FlatBufferBuilder &_fbb) - : fbb_(_fbb) { - start_ = fbb_.StartTable(); - } - FusionTrackParametersBuilder &operator=(const FusionTrackParametersBuilder &); - flatbuffers::Offset Finish() { - const auto end = fbb_.EndTable(start_); - auto o = flatbuffers::Offset(end); - return o; - } -}; - -inline flatbuffers::Offset CreateFusionTrackParameters( - flatbuffers::FlatBufferBuilder &_fbb, - flatbuffers::Offset name = 0, - flatbuffers::Offset deviceClockID = 0, - flatbuffers::Offset localClockID = 0, - flatbuffers::Offset>> geometries = 0, - flatbuffers::Offset>> geometryFilenames = 0, - flatbuffers::Offset geometryDir = 0, - flatbuffers::Offset> TrackerDeviceIDs = 0, - flatbuffers::Offset> markerIDs = 0, - flatbuffers::Offset>> markerNames = 0, - flatbuffers::Offset> m_deviceSerialNumbers = 0, - flatbuffers::Offset> m_device_types = 0) { - FusionTrackParametersBuilder builder_(_fbb); - builder_.add_m_device_types(m_device_types); - builder_.add_m_deviceSerialNumbers(m_deviceSerialNumbers); - builder_.add_markerNames(markerNames); - builder_.add_markerIDs(markerIDs); - builder_.add_TrackerDeviceIDs(TrackerDeviceIDs); - builder_.add_geometryDir(geometryDir); - builder_.add_geometryFilenames(geometryFilenames); - builder_.add_geometries(geometries); - builder_.add_localClockID(localClockID); - builder_.add_deviceClockID(deviceClockID); - builder_.add_name(name); - return builder_.Finish(); -} - -inline flatbuffers::Offset CreateFusionTrackParametersDirect( - flatbuffers::FlatBufferBuilder &_fbb, - const char *name = nullptr, - const char *deviceClockID = nullptr, - const char *localClockID = nullptr, - const std::vector> *geometries = nullptr, - const std::vector> *geometryFilenames = nullptr, - const char *geometryDir = nullptr, - const std::vector *TrackerDeviceIDs = nullptr, - const std::vector *markerIDs = nullptr, - const std::vector> *markerNames = nullptr, - const std::vector *m_deviceSerialNumbers = nullptr, - const std::vector *m_device_types = nullptr) { - return grl::flatbuffer::CreateFusionTrackParameters( - _fbb, - name ? _fbb.CreateString(name) : 0, - deviceClockID ? _fbb.CreateString(deviceClockID) : 0, - localClockID ? _fbb.CreateString(localClockID) : 0, - geometries ? _fbb.CreateVector>(*geometries) : 0, - geometryFilenames ? _fbb.CreateVector>(*geometryFilenames) : 0, - geometryDir ? _fbb.CreateString(geometryDir) : 0, - TrackerDeviceIDs ? _fbb.CreateVector(*TrackerDeviceIDs) : 0, - markerIDs ? _fbb.CreateVector(*markerIDs) : 0, - markerNames ? _fbb.CreateVector>(*markerNames) : 0, - m_deviceSerialNumbers ? _fbb.CreateVector(*m_deviceSerialNumbers) : 0, - m_device_types ? _fbb.CreateVector(*m_device_types) : 0); -} - -flatbuffers::Offset CreateFusionTrackParameters(flatbuffers::FlatBufferBuilder &_fbb, const FusionTrackParametersT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); - -struct FusionTrackMessageT : public flatbuffers::NativeTable { - typedef FusionTrackMessage TableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.FusionTrackMessageT"; - } - double timestamp; - std::unique_ptr parameters; - std::unique_ptr timeEvent; - std::unique_ptr frame; - FusionTrackMessageT() - : timestamp(0.0) { - } -}; - -struct FusionTrackMessage FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { - typedef FusionTrackMessageT NativeTableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.FusionTrackMessage"; - } - enum { - VT_TIMESTAMP = 4, - VT_PARAMETERS = 6, - VT_TIMEEVENT = 8, - VT_FRAME = 10 - }; - double timestamp() const { - return GetField(VT_TIMESTAMP, 0.0); - } - const FusionTrackParameters *parameters() const { - return GetPointer(VT_PARAMETERS); - } - const TimeEvent *timeEvent() const { - return GetPointer(VT_TIMEEVENT); - } - const FusionTrackFrame *frame() const { - return GetPointer(VT_FRAME); - } - bool Verify(flatbuffers::Verifier &verifier) const { - return VerifyTableStart(verifier) && - VerifyField(verifier, VT_TIMESTAMP) && - VerifyOffset(verifier, VT_PARAMETERS) && - verifier.VerifyTable(parameters()) && - VerifyOffset(verifier, VT_TIMEEVENT) && - verifier.VerifyTable(timeEvent()) && - VerifyOffset(verifier, VT_FRAME) && - verifier.VerifyTable(frame()) && - verifier.EndTable(); - } - FusionTrackMessageT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; - void UnPackTo(FusionTrackMessageT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; - static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const FusionTrackMessageT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); -}; - -struct FusionTrackMessageBuilder { - flatbuffers::FlatBufferBuilder &fbb_; - flatbuffers::uoffset_t start_; - void add_timestamp(double timestamp) { - fbb_.AddElement(FusionTrackMessage::VT_TIMESTAMP, timestamp, 0.0); - } - void add_parameters(flatbuffers::Offset parameters) { - fbb_.AddOffset(FusionTrackMessage::VT_PARAMETERS, parameters); - } - void add_timeEvent(flatbuffers::Offset timeEvent) { - fbb_.AddOffset(FusionTrackMessage::VT_TIMEEVENT, timeEvent); - } - void add_frame(flatbuffers::Offset frame) { - fbb_.AddOffset(FusionTrackMessage::VT_FRAME, frame); - } - explicit FusionTrackMessageBuilder(flatbuffers::FlatBufferBuilder &_fbb) - : fbb_(_fbb) { - start_ = fbb_.StartTable(); - } - FusionTrackMessageBuilder &operator=(const FusionTrackMessageBuilder &); - flatbuffers::Offset Finish() { - const auto end = fbb_.EndTable(start_); - auto o = flatbuffers::Offset(end); - return o; - } -}; - -inline flatbuffers::Offset CreateFusionTrackMessage( - flatbuffers::FlatBufferBuilder &_fbb, - double timestamp = 0.0, - flatbuffers::Offset parameters = 0, - flatbuffers::Offset timeEvent = 0, - flatbuffers::Offset frame = 0) { - FusionTrackMessageBuilder builder_(_fbb); - builder_.add_timestamp(timestamp); - builder_.add_frame(frame); - builder_.add_timeEvent(timeEvent); - builder_.add_parameters(parameters); - return builder_.Finish(); -} - -flatbuffers::Offset CreateFusionTrackMessage(flatbuffers::FlatBufferBuilder &_fbb, const FusionTrackMessageT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); - -inline ftkGeometryT *ftkGeometry::UnPack(const flatbuffers::resolver_function_t *_resolver) const { - auto _o = new ftkGeometryT(); - UnPackTo(_o, _resolver); - return _o; -} - -inline void ftkGeometry::UnPackTo(ftkGeometryT *_o, const flatbuffers::resolver_function_t *_resolver) const { - (void)_o; - (void)_resolver; - { auto _e = name(); if (_e) _o->name = _e->str(); }; - { auto _e = geometryID(); _o->geometryID = _e; }; - { auto _e = version(); _o->version = _e; }; - { auto _e = positions(); if (_e) { _o->positions.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->positions[_i] = *_e->Get(_i); } } }; -} - -inline flatbuffers::Offset ftkGeometry::Pack(flatbuffers::FlatBufferBuilder &_fbb, const ftkGeometryT* _o, const flatbuffers::rehasher_function_t *_rehasher) { - return CreateftkGeometry(_fbb, _o, _rehasher); -} - -inline flatbuffers::Offset CreateftkGeometry(flatbuffers::FlatBufferBuilder &_fbb, const ftkGeometryT *_o, const flatbuffers::rehasher_function_t *_rehasher) { - (void)_rehasher; - (void)_o; - struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const ftkGeometryT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; - auto _name = _o->name.empty() ? 0 : _fbb.CreateString(_o->name); - auto _geometryID = _o->geometryID; - auto _version = _o->version; - auto _positions = _o->positions.size() ? _fbb.CreateVectorOfStructs(_o->positions) : 0; - return grl::flatbuffer::CreateftkGeometry( - _fbb, - _name, - _geometryID, - _version, - _positions); -} - -inline ftkMarkerT *ftkMarker::UnPack(const flatbuffers::resolver_function_t *_resolver) const { - auto _o = new ftkMarkerT(); - UnPackTo(_o, _resolver); - return _o; -} - -inline void ftkMarker::UnPackTo(ftkMarkerT *_o, const flatbuffers::resolver_function_t *_resolver) const { - (void)_o; - (void)_resolver; - { auto _e = name(); if (_e) _o->name = _e->str(); }; - { auto _e = ID(); _o->ID = _e; }; - { auto _e = geometryID(); _o->geometryID = _e; }; - { auto _e = geometryPresenceMask(); if (_e) { _o->geometryPresenceMask.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->geometryPresenceMask[_i] = _e->Get(_i); } } }; - { auto _e = transform(); if (_e) _o->transform = std::unique_ptr(new Pose(*_e)); }; -} - -inline flatbuffers::Offset ftkMarker::Pack(flatbuffers::FlatBufferBuilder &_fbb, const ftkMarkerT* _o, const flatbuffers::rehasher_function_t *_rehasher) { - return CreateftkMarker(_fbb, _o, _rehasher); -} - -inline flatbuffers::Offset CreateftkMarker(flatbuffers::FlatBufferBuilder &_fbb, const ftkMarkerT *_o, const flatbuffers::rehasher_function_t *_rehasher) { - (void)_rehasher; - (void)_o; - struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const ftkMarkerT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; - auto _name = _o->name.empty() ? 0 : _fbb.CreateString(_o->name); - auto _ID = _o->ID; - auto _geometryID = _o->geometryID; - auto _geometryPresenceMask = _o->geometryPresenceMask.size() ? _fbb.CreateVector(_o->geometryPresenceMask) : 0; - auto _transform = _o->transform ? _o->transform.get() : 0; - return grl::flatbuffer::CreateftkMarker( - _fbb, - _name, - _ID, - _geometryID, - _geometryPresenceMask, - _transform); -} - -inline ftk3DFiducialT *ftk3DFiducial::UnPack(const flatbuffers::resolver_function_t *_resolver) const { - auto _o = new ftk3DFiducialT(); - UnPackTo(_o, _resolver); - return _o; -} - -inline void ftk3DFiducial::UnPackTo(ftk3DFiducialT *_o, const flatbuffers::resolver_function_t *_resolver) const { - (void)_o; - (void)_resolver; - { auto _e = markerID(); _o->markerID = _e; }; - { auto _e = leftIndex(); _o->leftIndex = _e; }; - { auto _e = rightIndex(); _o->rightIndex = _e; }; - { auto _e = position(); if (_e) _o->position = std::unique_ptr(new Vector3d(*_e)); }; - { auto _e = epipolarErrorPixels(); _o->epipolarErrorPixels = _e; }; - { auto _e = triangulationError(); _o->triangulationError = _e; }; - { auto _e = probability(); _o->probability = _e; }; -} - -inline flatbuffers::Offset ftk3DFiducial::Pack(flatbuffers::FlatBufferBuilder &_fbb, const ftk3DFiducialT* _o, const flatbuffers::rehasher_function_t *_rehasher) { - return Createftk3DFiducial(_fbb, _o, _rehasher); -} - -inline flatbuffers::Offset Createftk3DFiducial(flatbuffers::FlatBufferBuilder &_fbb, const ftk3DFiducialT *_o, const flatbuffers::rehasher_function_t *_rehasher) { - (void)_rehasher; - (void)_o; - struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const ftk3DFiducialT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; - auto _markerID = _o->markerID; - auto _leftIndex = _o->leftIndex; - auto _rightIndex = _o->rightIndex; - auto _position = _o->position ? _o->position.get() : 0; - auto _epipolarErrorPixels = _o->epipolarErrorPixels; - auto _triangulationError = _o->triangulationError; - auto _probability = _o->probability; - return grl::flatbuffer::Createftk3DFiducial( - _fbb, - _markerID, - _leftIndex, - _rightIndex, - _position, - _epipolarErrorPixels, - _triangulationError, - _probability); -} - -inline ftkRegionOfInterestT *ftkRegionOfInterest::UnPack(const flatbuffers::resolver_function_t *_resolver) const { - auto _o = new ftkRegionOfInterestT(); - UnPackTo(_o, _resolver); - return _o; -} - -inline void ftkRegionOfInterest::UnPackTo(ftkRegionOfInterestT *_o, const flatbuffers::resolver_function_t *_resolver) const { - (void)_o; - (void)_resolver; - { auto _e = centerXPixels(); _o->centerXPixels = _e; }; - { auto _e = centerYPixels(); _o->centerYPixels = _e; }; - { auto _e = RightEdge(); _o->RightEdge = _e; }; - { auto _e = BottomEdge(); _o->BottomEdge = _e; }; - { auto _e = LeftEdge(); _o->LeftEdge = _e; }; - { auto _e = TopEdge(); _o->TopEdge = _e; }; - { auto _e = pixelsCount(); _o->pixelsCount = _e; }; - { auto _e = probability(); _o->probability = _e; }; -} - -inline flatbuffers::Offset ftkRegionOfInterest::Pack(flatbuffers::FlatBufferBuilder &_fbb, const ftkRegionOfInterestT* _o, const flatbuffers::rehasher_function_t *_rehasher) { - return CreateftkRegionOfInterest(_fbb, _o, _rehasher); -} - -inline flatbuffers::Offset CreateftkRegionOfInterest(flatbuffers::FlatBufferBuilder &_fbb, const ftkRegionOfInterestT *_o, const flatbuffers::rehasher_function_t *_rehasher) { - (void)_rehasher; - (void)_o; - struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const ftkRegionOfInterestT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; - auto _centerXPixels = _o->centerXPixels; - auto _centerYPixels = _o->centerYPixels; - auto _RightEdge = _o->RightEdge; - auto _BottomEdge = _o->BottomEdge; - auto _LeftEdge = _o->LeftEdge; - auto _TopEdge = _o->TopEdge; - auto _pixelsCount = _o->pixelsCount; - auto _probability = _o->probability; - return grl::flatbuffer::CreateftkRegionOfInterest( - _fbb, - _centerXPixels, - _centerYPixels, - _RightEdge, - _BottomEdge, - _LeftEdge, - _TopEdge, - _pixelsCount, - _probability); -} - -inline FusionTrackFrameT *FusionTrackFrame::UnPack(const flatbuffers::resolver_function_t *_resolver) const { - auto _o = new FusionTrackFrameT(); - UnPackTo(_o, _resolver); - return _o; -} - -inline void FusionTrackFrame::UnPackTo(FusionTrackFrameT *_o, const flatbuffers::resolver_function_t *_resolver) const { - (void)_o; - (void)_resolver; - { auto _e = timestamp(); _o->timestamp = _e; }; - { auto _e = serialNumber(); _o->serialNumber = _e; }; - { auto _e = hardwareTimestampUS(); _o->hardwareTimestampUS = _e; }; - { auto _e = desynchroUS(); _o->desynchroUS = _e; }; - { auto _e = counter(); _o->counter = _e; }; - { auto _e = format(); _o->format = _e; }; - { auto _e = width(); _o->width = _e; }; - { auto _e = height(); _o->height = _e; }; - { auto _e = imageStrideInBytes(); _o->imageStrideInBytes = _e; }; - { auto _e = imageHeaderVersion(); _o->imageHeaderVersion = _e; }; - { auto _e = imageHeaderStatus(); _o->imageHeaderStatus = _e; }; - { auto _e = imageLeftPixels(); if (_e) _o->imageLeftPixels = _e->str(); }; - { auto _e = imageLeftPixelsVersion(); _o->imageLeftPixelsVersion = _e; }; - { auto _e = imageLeftStatus(); _o->imageLeftStatus = _e; }; - { auto _e = imageRightPixels(); if (_e) _o->imageRightPixels = _e->str(); }; - { auto _e = imageRightPixelsVersion(); _o->imageRightPixelsVersion = _e; }; - { auto _e = imageRightStatus(); _o->imageRightStatus = _e; }; - { auto _e = regionsOfInterestLeft(); if (_e) { _o->regionsOfInterestLeft.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->regionsOfInterestLeft[_i] = std::unique_ptr(_e->Get(_i)->UnPack(_resolver)); } } }; - { auto _e = regionsOfInterestLeftVersion(); _o->regionsOfInterestLeftVersion = _e; }; - { auto _e = regionsOfInterestLeftStatus(); _o->regionsOfInterestLeftStatus = _e; }; - { auto _e = regionsOfInterestRight(); if (_e) { _o->regionsOfInterestRight.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->regionsOfInterestRight[_i] = std::unique_ptr(_e->Get(_i)->UnPack(_resolver)); } } }; - { auto _e = regionsOfInterestRightVersion(); _o->regionsOfInterestRightVersion = _e; }; - { auto _e = regionsOfInterestRightStatus(); _o->regionsOfInterestRightStatus = _e; }; - { auto _e = threeDFiducials(); if (_e) { _o->threeDFiducials.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->threeDFiducials[_i] = std::unique_ptr(_e->Get(_i)->UnPack(_resolver)); } } }; - { auto _e = threeDFiducialsVersion(); _o->threeDFiducialsVersion = _e; }; - { auto _e = threeDFiducialsStatus(); _o->threeDFiducialsStatus = _e; }; - { auto _e = markers(); if (_e) { _o->markers.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->markers[_i] = std::unique_ptr(_e->Get(_i)->UnPack(_resolver)); } } }; - { auto _e = markersVersion(); _o->markersVersion = _e; }; - { auto _e = markersStatus(); _o->markersStatus = _e; }; - { auto _e = deviceType(); _o->deviceType = _e; }; - { auto _e = ftkError(); _o->ftkError = _e; }; -} - -inline flatbuffers::Offset FusionTrackFrame::Pack(flatbuffers::FlatBufferBuilder &_fbb, const FusionTrackFrameT* _o, const flatbuffers::rehasher_function_t *_rehasher) { - return CreateFusionTrackFrame(_fbb, _o, _rehasher); -} - -inline flatbuffers::Offset CreateFusionTrackFrame(flatbuffers::FlatBufferBuilder &_fbb, const FusionTrackFrameT *_o, const flatbuffers::rehasher_function_t *_rehasher) { - (void)_rehasher; - (void)_o; - struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const FusionTrackFrameT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; - auto _timestamp = _o->timestamp; - auto _serialNumber = _o->serialNumber; - auto _hardwareTimestampUS = _o->hardwareTimestampUS; - auto _desynchroUS = _o->desynchroUS; - auto _counter = _o->counter; - auto _format = _o->format; - auto _width = _o->width; - auto _height = _o->height; - auto _imageStrideInBytes = _o->imageStrideInBytes; - auto _imageHeaderVersion = _o->imageHeaderVersion; - auto _imageHeaderStatus = _o->imageHeaderStatus; - auto _imageLeftPixels = _o->imageLeftPixels.empty() ? 0 : _fbb.CreateString(_o->imageLeftPixels); - auto _imageLeftPixelsVersion = _o->imageLeftPixelsVersion; - auto _imageLeftStatus = _o->imageLeftStatus; - auto _imageRightPixels = _o->imageRightPixels.empty() ? 0 : _fbb.CreateString(_o->imageRightPixels); - auto _imageRightPixelsVersion = _o->imageRightPixelsVersion; - auto _imageRightStatus = _o->imageRightStatus; - auto _regionsOfInterestLeft = _o->regionsOfInterestLeft.size() ? _fbb.CreateVector> (_o->regionsOfInterestLeft.size(), [](size_t i, _VectorArgs *__va) { return CreateftkRegionOfInterest(*__va->__fbb, __va->__o->regionsOfInterestLeft[i].get(), __va->__rehasher); }, &_va ) : 0; - auto _regionsOfInterestLeftVersion = _o->regionsOfInterestLeftVersion; - auto _regionsOfInterestLeftStatus = _o->regionsOfInterestLeftStatus; - auto _regionsOfInterestRight = _o->regionsOfInterestRight.size() ? _fbb.CreateVector> (_o->regionsOfInterestRight.size(), [](size_t i, _VectorArgs *__va) { return CreateftkRegionOfInterest(*__va->__fbb, __va->__o->regionsOfInterestRight[i].get(), __va->__rehasher); }, &_va ) : 0; - auto _regionsOfInterestRightVersion = _o->regionsOfInterestRightVersion; - auto _regionsOfInterestRightStatus = _o->regionsOfInterestRightStatus; - auto _threeDFiducials = _o->threeDFiducials.size() ? _fbb.CreateVector> (_o->threeDFiducials.size(), [](size_t i, _VectorArgs *__va) { return Createftk3DFiducial(*__va->__fbb, __va->__o->threeDFiducials[i].get(), __va->__rehasher); }, &_va ) : 0; - auto _threeDFiducialsVersion = _o->threeDFiducialsVersion; - auto _threeDFiducialsStatus = _o->threeDFiducialsStatus; - auto _markers = _o->markers.size() ? _fbb.CreateVector> (_o->markers.size(), [](size_t i, _VectorArgs *__va) { return CreateftkMarker(*__va->__fbb, __va->__o->markers[i].get(), __va->__rehasher); }, &_va ) : 0; - auto _markersVersion = _o->markersVersion; - auto _markersStatus = _o->markersStatus; - auto _deviceType = _o->deviceType; - auto _ftkError = _o->ftkError; - return grl::flatbuffer::CreateFusionTrackFrame( - _fbb, - _timestamp, - _serialNumber, - _hardwareTimestampUS, - _desynchroUS, - _counter, - _format, - _width, - _height, - _imageStrideInBytes, - _imageHeaderVersion, - _imageHeaderStatus, - _imageLeftPixels, - _imageLeftPixelsVersion, - _imageLeftStatus, - _imageRightPixels, - _imageRightPixelsVersion, - _imageRightStatus, - _regionsOfInterestLeft, - _regionsOfInterestLeftVersion, - _regionsOfInterestLeftStatus, - _regionsOfInterestRight, - _regionsOfInterestRightVersion, - _regionsOfInterestRightStatus, - _threeDFiducials, - _threeDFiducialsVersion, - _threeDFiducialsStatus, - _markers, - _markersVersion, - _markersStatus, - _deviceType, - _ftkError); -} - -inline FusionTrackParametersT *FusionTrackParameters::UnPack(const flatbuffers::resolver_function_t *_resolver) const { - auto _o = new FusionTrackParametersT(); - UnPackTo(_o, _resolver); - return _o; -} - -inline void FusionTrackParameters::UnPackTo(FusionTrackParametersT *_o, const flatbuffers::resolver_function_t *_resolver) const { - (void)_o; - (void)_resolver; - { auto _e = name(); if (_e) _o->name = _e->str(); }; - { auto _e = deviceClockID(); if (_e) _o->deviceClockID = _e->str(); }; - { auto _e = localClockID(); if (_e) _o->localClockID = _e->str(); }; - { auto _e = geometries(); if (_e) { _o->geometries.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->geometries[_i] = std::unique_ptr(_e->Get(_i)->UnPack(_resolver)); } } }; - { auto _e = geometryFilenames(); if (_e) { _o->geometryFilenames.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->geometryFilenames[_i] = _e->Get(_i)->str(); } } }; - { auto _e = geometryDir(); if (_e) _o->geometryDir = _e->str(); }; - { auto _e = TrackerDeviceIDs(); if (_e) { _o->TrackerDeviceIDs.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->TrackerDeviceIDs[_i] = _e->Get(_i); } } }; - { auto _e = markerIDs(); if (_e) { _o->markerIDs.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->markerIDs[_i] = _e->Get(_i); } } }; - { auto _e = markerNames(); if (_e) { _o->markerNames.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->markerNames[_i] = _e->Get(_i)->str(); } } }; - { auto _e = m_deviceSerialNumbers(); if (_e) { _o->m_deviceSerialNumbers.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->m_deviceSerialNumbers[_i] = _e->Get(_i); } } }; - { auto _e = m_device_types(); if (_e) { _o->m_device_types.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->m_device_types[_i] = _e->Get(_i); } } }; -} - -inline flatbuffers::Offset FusionTrackParameters::Pack(flatbuffers::FlatBufferBuilder &_fbb, const FusionTrackParametersT* _o, const flatbuffers::rehasher_function_t *_rehasher) { - return CreateFusionTrackParameters(_fbb, _o, _rehasher); -} - -inline flatbuffers::Offset CreateFusionTrackParameters(flatbuffers::FlatBufferBuilder &_fbb, const FusionTrackParametersT *_o, const flatbuffers::rehasher_function_t *_rehasher) { - (void)_rehasher; - (void)_o; - struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const FusionTrackParametersT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; - auto _name = _o->name.empty() ? 0 : _fbb.CreateString(_o->name); - auto _deviceClockID = _o->deviceClockID.empty() ? 0 : _fbb.CreateString(_o->deviceClockID); - auto _localClockID = _o->localClockID.empty() ? 0 : _fbb.CreateString(_o->localClockID); - auto _geometries = _o->geometries.size() ? _fbb.CreateVector> (_o->geometries.size(), [](size_t i, _VectorArgs *__va) { return CreateftkGeometry(*__va->__fbb, __va->__o->geometries[i].get(), __va->__rehasher); }, &_va ) : 0; - auto _geometryFilenames = _o->geometryFilenames.size() ? _fbb.CreateVectorOfStrings(_o->geometryFilenames) : 0; - auto _geometryDir = _o->geometryDir.empty() ? 0 : _fbb.CreateString(_o->geometryDir); - auto _TrackerDeviceIDs = _o->TrackerDeviceIDs.size() ? _fbb.CreateVector(_o->TrackerDeviceIDs) : 0; - auto _markerIDs = _o->markerIDs.size() ? _fbb.CreateVector(_o->markerIDs) : 0; - auto _markerNames = _o->markerNames.size() ? _fbb.CreateVectorOfStrings(_o->markerNames) : 0; - auto _m_deviceSerialNumbers = _o->m_deviceSerialNumbers.size() ? _fbb.CreateVector(_o->m_deviceSerialNumbers) : 0; - auto _m_device_types = _o->m_device_types.size() ? _fbb.CreateVector(_o->m_device_types) : 0; - return grl::flatbuffer::CreateFusionTrackParameters( - _fbb, - _name, - _deviceClockID, - _localClockID, - _geometries, - _geometryFilenames, - _geometryDir, - _TrackerDeviceIDs, - _markerIDs, - _markerNames, - _m_deviceSerialNumbers, - _m_device_types); -} - -inline FusionTrackMessageT *FusionTrackMessage::UnPack(const flatbuffers::resolver_function_t *_resolver) const { - auto _o = new FusionTrackMessageT(); - UnPackTo(_o, _resolver); - return _o; -} - -inline void FusionTrackMessage::UnPackTo(FusionTrackMessageT *_o, const flatbuffers::resolver_function_t *_resolver) const { - (void)_o; - (void)_resolver; - { auto _e = timestamp(); _o->timestamp = _e; }; - { auto _e = parameters(); if (_e) _o->parameters = std::unique_ptr(_e->UnPack(_resolver)); }; - { auto _e = timeEvent(); if (_e) _o->timeEvent = std::unique_ptr(_e->UnPack(_resolver)); }; - { auto _e = frame(); if (_e) _o->frame = std::unique_ptr(_e->UnPack(_resolver)); }; -} - -inline flatbuffers::Offset FusionTrackMessage::Pack(flatbuffers::FlatBufferBuilder &_fbb, const FusionTrackMessageT* _o, const flatbuffers::rehasher_function_t *_rehasher) { - return CreateFusionTrackMessage(_fbb, _o, _rehasher); -} - -inline flatbuffers::Offset CreateFusionTrackMessage(flatbuffers::FlatBufferBuilder &_fbb, const FusionTrackMessageT *_o, const flatbuffers::rehasher_function_t *_rehasher) { - (void)_rehasher; - (void)_o; - struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const FusionTrackMessageT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; - auto _timestamp = _o->timestamp; - auto _parameters = _o->parameters ? CreateFusionTrackParameters(_fbb, _o->parameters.get(), _rehasher) : 0; - auto _timeEvent = _o->timeEvent ? CreateTimeEvent(_fbb, _o->timeEvent.get(), _rehasher) : 0; - auto _frame = _o->frame ? CreateFusionTrackFrame(_fbb, _o->frame.get(), _rehasher) : 0; - return grl::flatbuffer::CreateFusionTrackMessage( - _fbb, - _timestamp, - _parameters, - _timeEvent, - _frame); -} - -} // namespace flatbuffer -} // namespace grl - -#endif // FLATBUFFERS_GENERATED_FUSIONTRACK_GRL_FLATBUFFER_H_ diff --git a/include/grl/flatbuffer/Geometry_generated.h b/include/grl/flatbuffer/Geometry_generated.h deleted file mode 100644 index ebf7d04..0000000 --- a/include/grl/flatbuffer/Geometry_generated.h +++ /dev/null @@ -1,200 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - - -#ifndef FLATBUFFERS_GENERATED_GEOMETRY_GRL_FLATBUFFER_H_ -#define FLATBUFFERS_GENERATED_GEOMETRY_GRL_FLATBUFFER_H_ - -#include "flatbuffers/flatbuffers.h" - -namespace grl { -namespace flatbuffer { - -struct Vector3d; - -struct Quaternion; - -struct Pose; - -struct Wrench; - -struct Inertia; - -FLATBUFFERS_MANUALLY_ALIGNED_STRUCT(8) Vector3d FLATBUFFERS_FINAL_CLASS { - private: - double x_; - double y_; - double z_; - - public: - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.Vector3d"; - } - Vector3d() { - memset(this, 0, sizeof(Vector3d)); - } - Vector3d(double _x, double _y, double _z) - : x_(flatbuffers::EndianScalar(_x)), - y_(flatbuffers::EndianScalar(_y)), - z_(flatbuffers::EndianScalar(_z)) { - } - double x() const { - return flatbuffers::EndianScalar(x_); - } - double y() const { - return flatbuffers::EndianScalar(y_); - } - double z() const { - return flatbuffers::EndianScalar(z_); - } -}; -FLATBUFFERS_STRUCT_END(Vector3d, 24); - -FLATBUFFERS_MANUALLY_ALIGNED_STRUCT(8) Quaternion FLATBUFFERS_FINAL_CLASS { - private: - double x_; - double y_; - double z_; - double w_; - - public: - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.Quaternion"; - } - Quaternion() { - memset(this, 0, sizeof(Quaternion)); - } - Quaternion(double _x, double _y, double _z, double _w) - : x_(flatbuffers::EndianScalar(_x)), - y_(flatbuffers::EndianScalar(_y)), - z_(flatbuffers::EndianScalar(_z)), - w_(flatbuffers::EndianScalar(_w)) { - } - double x() const { - return flatbuffers::EndianScalar(x_); - } - double y() const { - return flatbuffers::EndianScalar(y_); - } - double z() const { - return flatbuffers::EndianScalar(z_); - } - double w() const { - return flatbuffers::EndianScalar(w_); - } -}; -FLATBUFFERS_STRUCT_END(Quaternion, 32); - -FLATBUFFERS_MANUALLY_ALIGNED_STRUCT(8) Pose FLATBUFFERS_FINAL_CLASS { - private: - Vector3d position_; - Quaternion orientation_; - - public: - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.Pose"; - } - Pose() { - memset(this, 0, sizeof(Pose)); - } - Pose(const Vector3d &_position, const Quaternion &_orientation) - : position_(_position), - orientation_(_orientation) { - } - const Vector3d &position() const { - return position_; - } - const Quaternion &orientation() const { - return orientation_; - } -}; -FLATBUFFERS_STRUCT_END(Pose, 56); - -FLATBUFFERS_MANUALLY_ALIGNED_STRUCT(8) Wrench FLATBUFFERS_FINAL_CLASS { - private: - Vector3d force_; - Vector3d torque_; - Vector3d force_offset_; - - public: - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.Wrench"; - } - Wrench() { - memset(this, 0, sizeof(Wrench)); - } - Wrench(const Vector3d &_force, const Vector3d &_torque, const Vector3d &_force_offset) - : force_(_force), - torque_(_torque), - force_offset_(_force_offset) { - } - const Vector3d &force() const { - return force_; - } - const Vector3d &torque() const { - return torque_; - } - const Vector3d &force_offset() const { - return force_offset_; - } -}; -FLATBUFFERS_STRUCT_END(Wrench, 72); - -FLATBUFFERS_MANUALLY_ALIGNED_STRUCT(8) Inertia FLATBUFFERS_FINAL_CLASS { - private: - double mass_; - Pose pose_; - double ixx_; - double ixy_; - double ixz_; - double iyy_; - double iyz_; - double izz_; - - public: - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.Inertia"; - } - Inertia() { - memset(this, 0, sizeof(Inertia)); - } - Inertia(double _mass, const Pose &_pose, double _ixx, double _ixy, double _ixz, double _iyy, double _iyz, double _izz) - : mass_(flatbuffers::EndianScalar(_mass)), - pose_(_pose), - ixx_(flatbuffers::EndianScalar(_ixx)), - ixy_(flatbuffers::EndianScalar(_ixy)), - ixz_(flatbuffers::EndianScalar(_ixz)), - iyy_(flatbuffers::EndianScalar(_iyy)), - iyz_(flatbuffers::EndianScalar(_iyz)), - izz_(flatbuffers::EndianScalar(_izz)) { - } - double mass() const { - return flatbuffers::EndianScalar(mass_); - } - const Pose &pose() const { - return pose_; - } - double ixx() const { - return flatbuffers::EndianScalar(ixx_); - } - double ixy() const { - return flatbuffers::EndianScalar(ixy_); - } - double ixz() const { - return flatbuffers::EndianScalar(ixz_); - } - double iyy() const { - return flatbuffers::EndianScalar(iyy_); - } - double iyz() const { - return flatbuffers::EndianScalar(iyz_); - } - double izz() const { - return flatbuffers::EndianScalar(izz_); - } -}; -FLATBUFFERS_STRUCT_END(Inertia, 112); - -} // namespace flatbuffer -} // namespace grl - -#endif // FLATBUFFERS_GENERATED_GEOMETRY_GRL_FLATBUFFER_H_ diff --git a/include/grl/flatbuffer/JointState_generated.h b/include/grl/flatbuffer/JointState_generated.h deleted file mode 100644 index fb78786..0000000 --- a/include/grl/flatbuffer/JointState_generated.h +++ /dev/null @@ -1,163 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - - -#ifndef FLATBUFFERS_GENERATED_JOINTSTATE_GRL_FLATBUFFER_H_ -#define FLATBUFFERS_GENERATED_JOINTSTATE_GRL_FLATBUFFER_H_ - -#include "flatbuffers/flatbuffers.h" - -namespace grl { -namespace flatbuffer { - -struct JointState; -struct JointStateT; - -struct JointStateT : public flatbuffers::NativeTable { - typedef JointState TableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.JointStateT"; - } - std::vector position; - std::vector velocity; - std::vector acceleration; - std::vector torque; - JointStateT() { - } -}; - -struct JointState FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { - typedef JointStateT NativeTableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.JointState"; - } - enum { - VT_POSITION = 4, - VT_VELOCITY = 6, - VT_ACCELERATION = 8, - VT_TORQUE = 10 - }; - const flatbuffers::Vector *position() const { - return GetPointer *>(VT_POSITION); - } - const flatbuffers::Vector *velocity() const { - return GetPointer *>(VT_VELOCITY); - } - const flatbuffers::Vector *acceleration() const { - return GetPointer *>(VT_ACCELERATION); - } - const flatbuffers::Vector *torque() const { - return GetPointer *>(VT_TORQUE); - } - bool Verify(flatbuffers::Verifier &verifier) const { - return VerifyTableStart(verifier) && - VerifyOffset(verifier, VT_POSITION) && - verifier.Verify(position()) && - VerifyOffset(verifier, VT_VELOCITY) && - verifier.Verify(velocity()) && - VerifyOffset(verifier, VT_ACCELERATION) && - verifier.Verify(acceleration()) && - VerifyOffset(verifier, VT_TORQUE) && - verifier.Verify(torque()) && - verifier.EndTable(); - } - JointStateT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; - void UnPackTo(JointStateT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; - static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const JointStateT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); -}; - -struct JointStateBuilder { - flatbuffers::FlatBufferBuilder &fbb_; - flatbuffers::uoffset_t start_; - void add_position(flatbuffers::Offset> position) { - fbb_.AddOffset(JointState::VT_POSITION, position); - } - void add_velocity(flatbuffers::Offset> velocity) { - fbb_.AddOffset(JointState::VT_VELOCITY, velocity); - } - void add_acceleration(flatbuffers::Offset> acceleration) { - fbb_.AddOffset(JointState::VT_ACCELERATION, acceleration); - } - void add_torque(flatbuffers::Offset> torque) { - fbb_.AddOffset(JointState::VT_TORQUE, torque); - } - explicit JointStateBuilder(flatbuffers::FlatBufferBuilder &_fbb) - : fbb_(_fbb) { - start_ = fbb_.StartTable(); - } - JointStateBuilder &operator=(const JointStateBuilder &); - flatbuffers::Offset Finish() { - const auto end = fbb_.EndTable(start_); - auto o = flatbuffers::Offset(end); - return o; - } -}; - -inline flatbuffers::Offset CreateJointState( - flatbuffers::FlatBufferBuilder &_fbb, - flatbuffers::Offset> position = 0, - flatbuffers::Offset> velocity = 0, - flatbuffers::Offset> acceleration = 0, - flatbuffers::Offset> torque = 0) { - JointStateBuilder builder_(_fbb); - builder_.add_torque(torque); - builder_.add_acceleration(acceleration); - builder_.add_velocity(velocity); - builder_.add_position(position); - return builder_.Finish(); -} - -inline flatbuffers::Offset CreateJointStateDirect( - flatbuffers::FlatBufferBuilder &_fbb, - const std::vector *position = nullptr, - const std::vector *velocity = nullptr, - const std::vector *acceleration = nullptr, - const std::vector *torque = nullptr) { - return grl::flatbuffer::CreateJointState( - _fbb, - position ? _fbb.CreateVector(*position) : 0, - velocity ? _fbb.CreateVector(*velocity) : 0, - acceleration ? _fbb.CreateVector(*acceleration) : 0, - torque ? _fbb.CreateVector(*torque) : 0); -} - -flatbuffers::Offset CreateJointState(flatbuffers::FlatBufferBuilder &_fbb, const JointStateT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); - -inline JointStateT *JointState::UnPack(const flatbuffers::resolver_function_t *_resolver) const { - auto _o = new JointStateT(); - UnPackTo(_o, _resolver); - return _o; -} - -inline void JointState::UnPackTo(JointStateT *_o, const flatbuffers::resolver_function_t *_resolver) const { - (void)_o; - (void)_resolver; - { auto _e = position(); if (_e) { _o->position.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->position[_i] = _e->Get(_i); } } }; - { auto _e = velocity(); if (_e) { _o->velocity.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->velocity[_i] = _e->Get(_i); } } }; - { auto _e = acceleration(); if (_e) { _o->acceleration.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->acceleration[_i] = _e->Get(_i); } } }; - { auto _e = torque(); if (_e) { _o->torque.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->torque[_i] = _e->Get(_i); } } }; -} - -inline flatbuffers::Offset JointState::Pack(flatbuffers::FlatBufferBuilder &_fbb, const JointStateT* _o, const flatbuffers::rehasher_function_t *_rehasher) { - return CreateJointState(_fbb, _o, _rehasher); -} - -inline flatbuffers::Offset CreateJointState(flatbuffers::FlatBufferBuilder &_fbb, const JointStateT *_o, const flatbuffers::rehasher_function_t *_rehasher) { - (void)_rehasher; - (void)_o; - struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const JointStateT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; - auto _position = _o->position.size() ? _fbb.CreateVector(_o->position) : 0; - auto _velocity = _o->velocity.size() ? _fbb.CreateVector(_o->velocity) : 0; - auto _acceleration = _o->acceleration.size() ? _fbb.CreateVector(_o->acceleration) : 0; - auto _torque = _o->torque.size() ? _fbb.CreateVector(_o->torque) : 0; - return grl::flatbuffer::CreateJointState( - _fbb, - _position, - _velocity, - _acceleration, - _torque); -} - -} // namespace flatbuffer -} // namespace grl - -#endif // FLATBUFFERS_GENERATED_JOINTSTATE_GRL_FLATBUFFER_H_ diff --git a/include/grl/flatbuffer/KUKAiiwa_generated.h b/include/grl/flatbuffer/KUKAiiwa_generated.h deleted file mode 100644 index 41cde57..0000000 --- a/include/grl/flatbuffer/KUKAiiwa_generated.h +++ /dev/null @@ -1,2885 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - - -#ifndef FLATBUFFERS_GENERATED_KUKAIIWA_GRL_FLATBUFFER_H_ -#define FLATBUFFERS_GENERATED_KUKAIIWA_GRL_FLATBUFFER_H_ - -#include "flatbuffers/flatbuffers.h" - -#include "ArmControlState_generated.h" -#include "Euler_generated.h" -#include "Geometry_generated.h" -#include "JointState_generated.h" -#include "LinkObject_generated.h" -#include "Time_generated.h" - -namespace grl { -namespace flatbuffer { - -struct CartesianImpedenceControlMode; -struct CartesianImpedenceControlModeT; - -struct JointImpedenceControlMode; -struct JointImpedenceControlModeT; - -struct Disabled; -struct DisabledT; - -struct FRI; -struct FRIT; - -struct SmartServo; -struct SmartServoT; - -struct ProcessData; -struct ProcessDataT; - -struct KUKAiiwaArmConfiguration; -struct KUKAiiwaArmConfigurationT; - -struct KUKAiiwaMonitorConfiguration; -struct KUKAiiwaMonitorConfigurationT; - -struct KUKAiiwaMonitorState; -struct KUKAiiwaMonitorStateT; - -struct FRITimeStamp; -struct FRITimeStampT; - -struct FRIMessageLog; -struct FRIMessageLogT; - -struct KUKAiiwaState; -struct KUKAiiwaStateT; - -struct KUKAiiwaStates; -struct KUKAiiwaStatesT; - -enum class KUKAiiwaInterface : int8_t { - Disabled = 0, - SmartServo = 1, - DirectServo = 2, - FRI = 3, - MIN = Disabled, - MAX = FRI -}; - -inline const KUKAiiwaInterface (&EnumValuesKUKAiiwaInterface())[4] { - static const KUKAiiwaInterface values[] = { - KUKAiiwaInterface::Disabled, - KUKAiiwaInterface::SmartServo, - KUKAiiwaInterface::DirectServo, - KUKAiiwaInterface::FRI - }; - return values; -} - -inline const char * const *EnumNamesKUKAiiwaInterface() { - static const char * const names[] = { - "Disabled", - "SmartServo", - "DirectServo", - "FRI", - nullptr - }; - return names; -} - -inline const char *EnumNameKUKAiiwaInterface(KUKAiiwaInterface e) { - const size_t index = static_cast(e); - return EnumNamesKUKAiiwaInterface()[index]; -} - -enum class ESessionState : int8_t { - /// No session - IDLE = 0 /// Monitoring mode, receive state but connection too inconsistent to command -, - MONITORING_WAIT = 1 /// Monitoring mode -, - MONITORING_READY = 2 /// About to command (Overlay created in Java interface) -, - COMMANDING_WAIT = 3 /// Actively commanding the arm with FRI -, - COMMANDING_ACTIVE = 4, - MIN = IDLE, - MAX = COMMANDING_ACTIVE -}; - -inline const ESessionState (&EnumValuesESessionState())[5] { - static const ESessionState values[] = { - ESessionState::IDLE, - ESessionState::MONITORING_WAIT, - ESessionState::MONITORING_READY, - ESessionState::COMMANDING_WAIT, - ESessionState::COMMANDING_ACTIVE - }; - return values; -} - -inline const char * const *EnumNamesESessionState() { - static const char * const names[] = { - "IDLE", - "MONITORING_WAIT", - "MONITORING_READY", - "COMMANDING_WAIT", - "COMMANDING_ACTIVE", - nullptr - }; - return names; -} - -inline const char *EnumNameESessionState(ESessionState e) { - const size_t index = static_cast(e); - return EnumNamesESessionState()[index]; -} - -enum class EConnectionQuality : int8_t { - POOR = 0, - FAIR = 1, - GOOD = 2, - EXCELLENT = 3, - MIN = POOR, - MAX = EXCELLENT -}; - -inline const EConnectionQuality (&EnumValuesEConnectionQuality())[4] { - static const EConnectionQuality values[] = { - EConnectionQuality::POOR, - EConnectionQuality::FAIR, - EConnectionQuality::GOOD, - EConnectionQuality::EXCELLENT - }; - return values; -} - -inline const char * const *EnumNamesEConnectionQuality() { - static const char * const names[] = { - "POOR", - "FAIR", - "GOOD", - "EXCELLENT", - nullptr - }; - return names; -} - -inline const char *EnumNameEConnectionQuality(EConnectionQuality e) { - const size_t index = static_cast(e); - return EnumNamesEConnectionQuality()[index]; -} - -enum class ESafetyState : int8_t { - NORMAL_OPERATION = 0, - SAFETY_STOP_LEVEL_0 = 1, - SAFETY_STOP_LEVEL_1 = 2, - SAFETY_STOP_LEVEL_2 = 3, - MIN = NORMAL_OPERATION, - MAX = SAFETY_STOP_LEVEL_2 -}; - -inline const ESafetyState (&EnumValuesESafetyState())[4] { - static const ESafetyState values[] = { - ESafetyState::NORMAL_OPERATION, - ESafetyState::SAFETY_STOP_LEVEL_0, - ESafetyState::SAFETY_STOP_LEVEL_1, - ESafetyState::SAFETY_STOP_LEVEL_2 - }; - return values; -} - -inline const char * const *EnumNamesESafetyState() { - static const char * const names[] = { - "NORMAL_OPERATION", - "SAFETY_STOP_LEVEL_0", - "SAFETY_STOP_LEVEL_1", - "SAFETY_STOP_LEVEL_2", - nullptr - }; - return names; -} - -inline const char *EnumNameESafetyState(ESafetyState e) { - const size_t index = static_cast(e); - return EnumNamesESafetyState()[index]; -} - -enum class EOperationMode : int8_t { - TEST_MODE_1 = 0, - TEST_MODE_2 = 1, - AUTOMATIC_MODE = 2, - MIN = TEST_MODE_1, - MAX = AUTOMATIC_MODE -}; - -inline const EOperationMode (&EnumValuesEOperationMode())[3] { - static const EOperationMode values[] = { - EOperationMode::TEST_MODE_1, - EOperationMode::TEST_MODE_2, - EOperationMode::AUTOMATIC_MODE - }; - return values; -} - -inline const char * const *EnumNamesEOperationMode() { - static const char * const names[] = { - "TEST_MODE_1", - "TEST_MODE_2", - "AUTOMATIC_MODE", - nullptr - }; - return names; -} - -inline const char *EnumNameEOperationMode(EOperationMode e) { - const size_t index = static_cast(e); - return EnumNamesEOperationMode()[index]; -} - -enum class EDriveState : int8_t { - /// Driving mode currently unused - OFF = 1 /// About to drive -, - TRANSITIONING = 2 /// Actively commanding arm -, - ACTIVE = 3, - MIN = OFF, - MAX = ACTIVE -}; - -inline const EDriveState (&EnumValuesEDriveState())[3] { - static const EDriveState values[] = { - EDriveState::OFF, - EDriveState::TRANSITIONING, - EDriveState::ACTIVE - }; - return values; -} - -inline const char * const *EnumNamesEDriveState() { - static const char * const names[] = { - "OFF", - "TRANSITIONING", - "ACTIVE", - nullptr - }; - return names; -} - -inline const char *EnumNameEDriveState(EDriveState e) { - const size_t index = static_cast(e) - static_cast(EDriveState::OFF); - return EnumNamesEDriveState()[index]; -} - -enum class EControlMode : int8_t { - POSITION_CONTROL_MODE = 0, - CART_IMP_CONTROL_MODE = 1, - JOINT_IMP_CONTROL_MODE = 2, - NO_CONTROL = 3, - MIN = POSITION_CONTROL_MODE, - MAX = NO_CONTROL -}; - -inline const EControlMode (&EnumValuesEControlMode())[4] { - static const EControlMode values[] = { - EControlMode::POSITION_CONTROL_MODE, - EControlMode::CART_IMP_CONTROL_MODE, - EControlMode::JOINT_IMP_CONTROL_MODE, - EControlMode::NO_CONTROL - }; - return values; -} - -inline const char * const *EnumNamesEControlMode() { - static const char * const names[] = { - "POSITION_CONTROL_MODE", - "CART_IMP_CONTROL_MODE", - "JOINT_IMP_CONTROL_MODE", - "NO_CONTROL", - nullptr - }; - return names; -} - -inline const char *EnumNameEControlMode(EControlMode e) { - const size_t index = static_cast(e); - return EnumNamesEControlMode()[index]; -} - -/// Type of command being sent to the arm (Dimensonal units) -enum class EClientCommandMode : int8_t { - NO_COMMAND_MODE = 0, - POSITION = 1, - WRENCH = 2, - TORQUE = 3, - MIN = NO_COMMAND_MODE, - MAX = TORQUE -}; - -inline const EClientCommandMode (&EnumValuesEClientCommandMode())[4] { - static const EClientCommandMode values[] = { - EClientCommandMode::NO_COMMAND_MODE, - EClientCommandMode::POSITION, - EClientCommandMode::WRENCH, - EClientCommandMode::TORQUE - }; - return values; -} - -inline const char * const *EnumNamesEClientCommandMode() { - static const char * const names[] = { - "NO_COMMAND_MODE", - "POSITION", - "WRENCH", - "TORQUE", - nullptr - }; - return names; -} - -inline const char *EnumNameEClientCommandMode(EClientCommandMode e) { - const size_t index = static_cast(e); - return EnumNamesEClientCommandMode()[index]; -} - -enum class EOverlayType : int8_t { - NO_OVERLAY = 0, - JOINT = 1, - CARTESIAN = 2, - MIN = NO_OVERLAY, - MAX = CARTESIAN -}; - -inline const EOverlayType (&EnumValuesEOverlayType())[3] { - static const EOverlayType values[] = { - EOverlayType::NO_OVERLAY, - EOverlayType::JOINT, - EOverlayType::CARTESIAN - }; - return values; -} - -inline const char * const *EnumNamesEOverlayType() { - static const char * const names[] = { - "NO_OVERLAY", - "JOINT", - "CARTESIAN", - nullptr - }; - return names; -} - -inline const char *EnumNameEOverlayType(EOverlayType e) { - const size_t index = static_cast(e); - return EnumNamesEOverlayType()[index]; -} - -struct CartesianImpedenceControlModeT : public flatbuffers::NativeTable { - typedef CartesianImpedenceControlMode TableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.CartesianImpedenceControlModeT"; - } - std::unique_ptr stiffness; - std::unique_ptr damping; - double nullspaceStiffness; - double nullspaceDamping; - std::unique_ptr maxPathDeviation; - std::unique_ptr maxCartesianVelocity; - std::unique_ptr maxControlForce; - bool maxControlForceExceededStop; - CartesianImpedenceControlModeT() - : nullspaceStiffness(0.0), - nullspaceDamping(0.0), - maxControlForceExceededStop(false) { - } -}; - -struct CartesianImpedenceControlMode FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { - typedef CartesianImpedenceControlModeT NativeTableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.CartesianImpedenceControlMode"; - } - enum { - VT_STIFFNESS = 4, - VT_DAMPING = 6, - VT_NULLSPACESTIFFNESS = 8, - VT_NULLSPACEDAMPING = 10, - VT_MAXPATHDEVIATION = 12, - VT_MAXCARTESIANVELOCITY = 14, - VT_MAXCONTROLFORCE = 16, - VT_MAXCONTROLFORCEEXCEEDEDSTOP = 18 - }; - /// actual stiffness to set rot:[nm/rad] - const EulerPose *stiffness() const { - return GetStruct(VT_STIFFNESS); - } - /// actual damping to set - const EulerPose *damping() const { - return GetStruct(VT_DAMPING); - } - /// [Nm/rad] must be => 0.0 - double nullspaceStiffness() const { - return GetField(VT_NULLSPACESTIFFNESS, 0.0); - } - /// must be between 0.3-1.0 suggested is 0.7 - double nullspaceDamping() const { - return GetField(VT_NULLSPACEDAMPING, 0.0); - } - /// maximum deviation from set goal in mm and radians - const EulerPose *maxPathDeviation() const { - return GetStruct(VT_MAXPATHDEVIATION); - } - /// trans: [mm/s] rot: [rad/s] - const EulerPose *maxCartesianVelocity() const { - return GetStruct(VT_MAXCARTESIANVELOCITY); - } - /// xyz: Newtons rpy:Nm (all >=0) - const EulerPose *maxControlForce() const { - return GetStruct(VT_MAXCONTROLFORCE); - } - /// stop if max control force is exceeded - bool maxControlForceExceededStop() const { - return GetField(VT_MAXCONTROLFORCEEXCEEDEDSTOP, 0) != 0; - } - bool Verify(flatbuffers::Verifier &verifier) const { - return VerifyTableStart(verifier) && - VerifyField(verifier, VT_STIFFNESS) && - VerifyField(verifier, VT_DAMPING) && - VerifyField(verifier, VT_NULLSPACESTIFFNESS) && - VerifyField(verifier, VT_NULLSPACEDAMPING) && - VerifyField(verifier, VT_MAXPATHDEVIATION) && - VerifyField(verifier, VT_MAXCARTESIANVELOCITY) && - VerifyField(verifier, VT_MAXCONTROLFORCE) && - VerifyField(verifier, VT_MAXCONTROLFORCEEXCEEDEDSTOP) && - verifier.EndTable(); - } - CartesianImpedenceControlModeT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; - void UnPackTo(CartesianImpedenceControlModeT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; - static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const CartesianImpedenceControlModeT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); -}; - -struct CartesianImpedenceControlModeBuilder { - flatbuffers::FlatBufferBuilder &fbb_; - flatbuffers::uoffset_t start_; - void add_stiffness(const EulerPose *stiffness) { - fbb_.AddStruct(CartesianImpedenceControlMode::VT_STIFFNESS, stiffness); - } - void add_damping(const EulerPose *damping) { - fbb_.AddStruct(CartesianImpedenceControlMode::VT_DAMPING, damping); - } - void add_nullspaceStiffness(double nullspaceStiffness) { - fbb_.AddElement(CartesianImpedenceControlMode::VT_NULLSPACESTIFFNESS, nullspaceStiffness, 0.0); - } - void add_nullspaceDamping(double nullspaceDamping) { - fbb_.AddElement(CartesianImpedenceControlMode::VT_NULLSPACEDAMPING, nullspaceDamping, 0.0); - } - void add_maxPathDeviation(const EulerPose *maxPathDeviation) { - fbb_.AddStruct(CartesianImpedenceControlMode::VT_MAXPATHDEVIATION, maxPathDeviation); - } - void add_maxCartesianVelocity(const EulerPose *maxCartesianVelocity) { - fbb_.AddStruct(CartesianImpedenceControlMode::VT_MAXCARTESIANVELOCITY, maxCartesianVelocity); - } - void add_maxControlForce(const EulerPose *maxControlForce) { - fbb_.AddStruct(CartesianImpedenceControlMode::VT_MAXCONTROLFORCE, maxControlForce); - } - void add_maxControlForceExceededStop(bool maxControlForceExceededStop) { - fbb_.AddElement(CartesianImpedenceControlMode::VT_MAXCONTROLFORCEEXCEEDEDSTOP, static_cast(maxControlForceExceededStop), 0); - } - explicit CartesianImpedenceControlModeBuilder(flatbuffers::FlatBufferBuilder &_fbb) - : fbb_(_fbb) { - start_ = fbb_.StartTable(); - } - CartesianImpedenceControlModeBuilder &operator=(const CartesianImpedenceControlModeBuilder &); - flatbuffers::Offset Finish() { - const auto end = fbb_.EndTable(start_); - auto o = flatbuffers::Offset(end); - return o; - } -}; - -inline flatbuffers::Offset CreateCartesianImpedenceControlMode( - flatbuffers::FlatBufferBuilder &_fbb, - const EulerPose *stiffness = 0, - const EulerPose *damping = 0, - double nullspaceStiffness = 0.0, - double nullspaceDamping = 0.0, - const EulerPose *maxPathDeviation = 0, - const EulerPose *maxCartesianVelocity = 0, - const EulerPose *maxControlForce = 0, - bool maxControlForceExceededStop = false) { - CartesianImpedenceControlModeBuilder builder_(_fbb); - builder_.add_nullspaceDamping(nullspaceDamping); - builder_.add_nullspaceStiffness(nullspaceStiffness); - builder_.add_maxControlForce(maxControlForce); - builder_.add_maxCartesianVelocity(maxCartesianVelocity); - builder_.add_maxPathDeviation(maxPathDeviation); - builder_.add_damping(damping); - builder_.add_stiffness(stiffness); - builder_.add_maxControlForceExceededStop(maxControlForceExceededStop); - return builder_.Finish(); -} - -flatbuffers::Offset CreateCartesianImpedenceControlMode(flatbuffers::FlatBufferBuilder &_fbb, const CartesianImpedenceControlModeT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); - -struct JointImpedenceControlModeT : public flatbuffers::NativeTable { - typedef JointImpedenceControlMode TableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.JointImpedenceControlModeT"; - } - std::vector stiffness; - std::vector damping; - JointImpedenceControlModeT() { - } -}; - -struct JointImpedenceControlMode FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { - typedef JointImpedenceControlModeT NativeTableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.JointImpedenceControlMode"; - } - enum { - VT_STIFFNESS = 4, - VT_DAMPING = 6 - }; - const flatbuffers::Vector *stiffness() const { - return GetPointer *>(VT_STIFFNESS); - } - const flatbuffers::Vector *damping() const { - return GetPointer *>(VT_DAMPING); - } - bool Verify(flatbuffers::Verifier &verifier) const { - return VerifyTableStart(verifier) && - VerifyOffset(verifier, VT_STIFFNESS) && - verifier.Verify(stiffness()) && - VerifyOffset(verifier, VT_DAMPING) && - verifier.Verify(damping()) && - verifier.EndTable(); - } - JointImpedenceControlModeT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; - void UnPackTo(JointImpedenceControlModeT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; - static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const JointImpedenceControlModeT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); -}; - -struct JointImpedenceControlModeBuilder { - flatbuffers::FlatBufferBuilder &fbb_; - flatbuffers::uoffset_t start_; - void add_stiffness(flatbuffers::Offset> stiffness) { - fbb_.AddOffset(JointImpedenceControlMode::VT_STIFFNESS, stiffness); - } - void add_damping(flatbuffers::Offset> damping) { - fbb_.AddOffset(JointImpedenceControlMode::VT_DAMPING, damping); - } - explicit JointImpedenceControlModeBuilder(flatbuffers::FlatBufferBuilder &_fbb) - : fbb_(_fbb) { - start_ = fbb_.StartTable(); - } - JointImpedenceControlModeBuilder &operator=(const JointImpedenceControlModeBuilder &); - flatbuffers::Offset Finish() { - const auto end = fbb_.EndTable(start_); - auto o = flatbuffers::Offset(end); - return o; - } -}; - -inline flatbuffers::Offset CreateJointImpedenceControlMode( - flatbuffers::FlatBufferBuilder &_fbb, - flatbuffers::Offset> stiffness = 0, - flatbuffers::Offset> damping = 0) { - JointImpedenceControlModeBuilder builder_(_fbb); - builder_.add_damping(damping); - builder_.add_stiffness(stiffness); - return builder_.Finish(); -} - -inline flatbuffers::Offset CreateJointImpedenceControlModeDirect( - flatbuffers::FlatBufferBuilder &_fbb, - const std::vector *stiffness = nullptr, - const std::vector *damping = nullptr) { - return grl::flatbuffer::CreateJointImpedenceControlMode( - _fbb, - stiffness ? _fbb.CreateVector(*stiffness) : 0, - damping ? _fbb.CreateVector(*damping) : 0); -} - -flatbuffers::Offset CreateJointImpedenceControlMode(flatbuffers::FlatBufferBuilder &_fbb, const JointImpedenceControlModeT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); - -struct DisabledT : public flatbuffers::NativeTable { - typedef Disabled TableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.DisabledT"; - } - DisabledT() { - } -}; - -struct Disabled FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { - typedef DisabledT NativeTableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.Disabled"; - } - bool Verify(flatbuffers::Verifier &verifier) const { - return VerifyTableStart(verifier) && - verifier.EndTable(); - } - DisabledT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; - void UnPackTo(DisabledT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; - static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const DisabledT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); -}; - -struct DisabledBuilder { - flatbuffers::FlatBufferBuilder &fbb_; - flatbuffers::uoffset_t start_; - explicit DisabledBuilder(flatbuffers::FlatBufferBuilder &_fbb) - : fbb_(_fbb) { - start_ = fbb_.StartTable(); - } - DisabledBuilder &operator=(const DisabledBuilder &); - flatbuffers::Offset Finish() { - const auto end = fbb_.EndTable(start_); - auto o = flatbuffers::Offset(end); - return o; - } -}; - -inline flatbuffers::Offset CreateDisabled( - flatbuffers::FlatBufferBuilder &_fbb) { - DisabledBuilder builder_(_fbb); - return builder_.Finish(); -} - -flatbuffers::Offset CreateDisabled(flatbuffers::FlatBufferBuilder &_fbb, const DisabledT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); - -struct FRIT : public flatbuffers::NativeTable { - typedef FRI TableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.FRIT"; - } - EOverlayType overlayType; - int32_t sendPeriodMillisec; - int32_t setReceiveMultiplier; - bool updatePortOnRemote; - int16_t portOnRemote; - bool updatePortOnController; - int16_t portOnController; - FRIT() - : overlayType(EOverlayType::JOINT), - sendPeriodMillisec(4), - setReceiveMultiplier(5), - updatePortOnRemote(false), - portOnRemote(0), - updatePortOnController(false), - portOnController(0) { - } -}; - -struct FRI FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { - typedef FRIT NativeTableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.FRI"; - } - enum { - VT_OVERLAYTYPE = 4, - VT_SENDPERIODMILLISEC = 6, - VT_SETRECEIVEMULTIPLIER = 8, - VT_UPDATEPORTONREMOTE = 10, - VT_PORTONREMOTE = 12, - VT_UPDATEPORTONCONTROLLER = 14, - VT_PORTONCONTROLLER = 16 - }; - EOverlayType overlayType() const { - return static_cast(GetField(VT_OVERLAYTYPE, 1)); - } - /// Set the value for the send period of the connection from the KUKA controller to the remote side in [ms]. - /// This means, the KUKA controller will send cyclic FRI messages every sendPeriod milliseconds to the remote side. - /// - /// - /// Parameters: - /// sendPeriod - the send period in milliseconds, 1 <= sendPeriod <= 100. - /// Note: The recommended value for good performance should be between 1-5 milliseconds. - int32_t sendPeriodMillisec() const { - return GetField(VT_SENDPERIODMILLISEC, 4); - } - /// Set the receive multiplier of the cycle time from the remote side to the KUKA controller. - /// This multiplier defines the value of the receivePeriod which is calculated: - /// receivePeriod = receiveMultiplier * sendPeriod - /// - /// The KUKA controller will expect a FRI response message every receivePeriod milliseconds from the remote side. - /// - /// The receivePeriod has to be within the range of: - /// 1 <= receivePeriod <= 100. - int32_t setReceiveMultiplier() const { - return GetField(VT_SETRECEIVEMULTIPLIER, 5); - } - bool updatePortOnRemote() const { - return GetField(VT_UPDATEPORTONREMOTE, 0) != 0; - } - /// Set the port ID of the socket at the controller side. - /// Note: Do not change this port ID, unless your application requires different port IDs on both ends of the FRI channel. - /// For changing the FRI port ID on both sides, it is sufficient to call setPortOnRemote(int). - /// Values of controllerPortID: - /// "-1" - The configuration of setPortOnRemote(int) is used. This is the default. - /// recommended range of port IDs: 30200 <= controllerPortID < 30210 - int16_t portOnRemote() const { - return GetField(VT_PORTONREMOTE, 0); - } - bool updatePortOnController() const { - return GetField(VT_UPDATEPORTONCONTROLLER, 0) != 0; - } - /// Set the port ID of the FRI channel at the remote side. - /// By default, this port ID is used on both sides of the FRI channel, unless specified otherwise by setPortOnController(int). - /// - /// Values of portID: - /// - /// default port ID: 30200 - /// recommended range of port IDs: 30200 <= portID < 30210 - /// Since the FRI channel utilizes UDP as connection layer, make sure, that your network topology (firewall, network services) are chosen accordingly. - /// - /// Parameters: - /// portID - the port ID > 0 (also known as UDP port number) - int16_t portOnController() const { - return GetField(VT_PORTONCONTROLLER, 0); - } - bool Verify(flatbuffers::Verifier &verifier) const { - return VerifyTableStart(verifier) && - VerifyField(verifier, VT_OVERLAYTYPE) && - VerifyField(verifier, VT_SENDPERIODMILLISEC) && - VerifyField(verifier, VT_SETRECEIVEMULTIPLIER) && - VerifyField(verifier, VT_UPDATEPORTONREMOTE) && - VerifyField(verifier, VT_PORTONREMOTE) && - VerifyField(verifier, VT_UPDATEPORTONCONTROLLER) && - VerifyField(verifier, VT_PORTONCONTROLLER) && - verifier.EndTable(); - } - FRIT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; - void UnPackTo(FRIT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; - static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const FRIT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); -}; - -struct FRIBuilder { - flatbuffers::FlatBufferBuilder &fbb_; - flatbuffers::uoffset_t start_; - void add_overlayType(EOverlayType overlayType) { - fbb_.AddElement(FRI::VT_OVERLAYTYPE, static_cast(overlayType), 1); - } - void add_sendPeriodMillisec(int32_t sendPeriodMillisec) { - fbb_.AddElement(FRI::VT_SENDPERIODMILLISEC, sendPeriodMillisec, 4); - } - void add_setReceiveMultiplier(int32_t setReceiveMultiplier) { - fbb_.AddElement(FRI::VT_SETRECEIVEMULTIPLIER, setReceiveMultiplier, 5); - } - void add_updatePortOnRemote(bool updatePortOnRemote) { - fbb_.AddElement(FRI::VT_UPDATEPORTONREMOTE, static_cast(updatePortOnRemote), 0); - } - void add_portOnRemote(int16_t portOnRemote) { - fbb_.AddElement(FRI::VT_PORTONREMOTE, portOnRemote, 0); - } - void add_updatePortOnController(bool updatePortOnController) { - fbb_.AddElement(FRI::VT_UPDATEPORTONCONTROLLER, static_cast(updatePortOnController), 0); - } - void add_portOnController(int16_t portOnController) { - fbb_.AddElement(FRI::VT_PORTONCONTROLLER, portOnController, 0); - } - explicit FRIBuilder(flatbuffers::FlatBufferBuilder &_fbb) - : fbb_(_fbb) { - start_ = fbb_.StartTable(); - } - FRIBuilder &operator=(const FRIBuilder &); - flatbuffers::Offset Finish() { - const auto end = fbb_.EndTable(start_); - auto o = flatbuffers::Offset(end); - return o; - } -}; - -inline flatbuffers::Offset CreateFRI( - flatbuffers::FlatBufferBuilder &_fbb, - EOverlayType overlayType = EOverlayType::JOINT, - int32_t sendPeriodMillisec = 4, - int32_t setReceiveMultiplier = 5, - bool updatePortOnRemote = false, - int16_t portOnRemote = 0, - bool updatePortOnController = false, - int16_t portOnController = 0) { - FRIBuilder builder_(_fbb); - builder_.add_setReceiveMultiplier(setReceiveMultiplier); - builder_.add_sendPeriodMillisec(sendPeriodMillisec); - builder_.add_portOnController(portOnController); - builder_.add_portOnRemote(portOnRemote); - builder_.add_updatePortOnController(updatePortOnController); - builder_.add_updatePortOnRemote(updatePortOnRemote); - builder_.add_overlayType(overlayType); - return builder_.Finish(); -} - -flatbuffers::Offset CreateFRI(flatbuffers::FlatBufferBuilder &_fbb, const FRIT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); - -struct SmartServoT : public flatbuffers::NativeTable { - typedef SmartServo TableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.SmartServoT"; - } - std::vector jointAccelerationRel; - std::vector jointVelocityRel; - bool updateMinimumTrajectoryExecutionTime; - double minimumTrajectoryExecutionTime; - SmartServoT() - : updateMinimumTrajectoryExecutionTime(false), - minimumTrajectoryExecutionTime(0.0) { - } -}; - -struct SmartServo FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { - typedef SmartServoT NativeTableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.SmartServo"; - } - enum { - VT_JOINTACCELERATIONREL = 4, - VT_JOINTVELOCITYREL = 6, - VT_UPDATEMINIMUMTRAJECTORYEXECUTIONTIME = 8, - VT_MINIMUMTRAJECTORYEXECUTIONTIME = 10 - }; - /// normalized joint accelerations from 0 to 1 relative to system capabilities - const flatbuffers::Vector *jointAccelerationRel() const { - return GetPointer *>(VT_JOINTACCELERATIONREL); - } - /// normalized joint velocity from 0 to 1 relative to system capabilities - const flatbuffers::Vector *jointVelocityRel() const { - return GetPointer *>(VT_JOINTVELOCITYREL); - } - bool updateMinimumTrajectoryExecutionTime() const { - return GetField(VT_UPDATEMINIMUMTRAJECTORYEXECUTIONTIME, 0) != 0; - } - double minimumTrajectoryExecutionTime() const { - return GetField(VT_MINIMUMTRAJECTORYEXECUTIONTIME, 0.0); - } - bool Verify(flatbuffers::Verifier &verifier) const { - return VerifyTableStart(verifier) && - VerifyOffset(verifier, VT_JOINTACCELERATIONREL) && - verifier.Verify(jointAccelerationRel()) && - VerifyOffset(verifier, VT_JOINTVELOCITYREL) && - verifier.Verify(jointVelocityRel()) && - VerifyField(verifier, VT_UPDATEMINIMUMTRAJECTORYEXECUTIONTIME) && - VerifyField(verifier, VT_MINIMUMTRAJECTORYEXECUTIONTIME) && - verifier.EndTable(); - } - SmartServoT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; - void UnPackTo(SmartServoT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; - static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const SmartServoT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); -}; - -struct SmartServoBuilder { - flatbuffers::FlatBufferBuilder &fbb_; - flatbuffers::uoffset_t start_; - void add_jointAccelerationRel(flatbuffers::Offset> jointAccelerationRel) { - fbb_.AddOffset(SmartServo::VT_JOINTACCELERATIONREL, jointAccelerationRel); - } - void add_jointVelocityRel(flatbuffers::Offset> jointVelocityRel) { - fbb_.AddOffset(SmartServo::VT_JOINTVELOCITYREL, jointVelocityRel); - } - void add_updateMinimumTrajectoryExecutionTime(bool updateMinimumTrajectoryExecutionTime) { - fbb_.AddElement(SmartServo::VT_UPDATEMINIMUMTRAJECTORYEXECUTIONTIME, static_cast(updateMinimumTrajectoryExecutionTime), 0); - } - void add_minimumTrajectoryExecutionTime(double minimumTrajectoryExecutionTime) { - fbb_.AddElement(SmartServo::VT_MINIMUMTRAJECTORYEXECUTIONTIME, minimumTrajectoryExecutionTime, 0.0); - } - explicit SmartServoBuilder(flatbuffers::FlatBufferBuilder &_fbb) - : fbb_(_fbb) { - start_ = fbb_.StartTable(); - } - SmartServoBuilder &operator=(const SmartServoBuilder &); - flatbuffers::Offset Finish() { - const auto end = fbb_.EndTable(start_); - auto o = flatbuffers::Offset(end); - return o; - } -}; - -inline flatbuffers::Offset CreateSmartServo( - flatbuffers::FlatBufferBuilder &_fbb, - flatbuffers::Offset> jointAccelerationRel = 0, - flatbuffers::Offset> jointVelocityRel = 0, - bool updateMinimumTrajectoryExecutionTime = false, - double minimumTrajectoryExecutionTime = 0.0) { - SmartServoBuilder builder_(_fbb); - builder_.add_minimumTrajectoryExecutionTime(minimumTrajectoryExecutionTime); - builder_.add_jointVelocityRel(jointVelocityRel); - builder_.add_jointAccelerationRel(jointAccelerationRel); - builder_.add_updateMinimumTrajectoryExecutionTime(updateMinimumTrajectoryExecutionTime); - return builder_.Finish(); -} - -inline flatbuffers::Offset CreateSmartServoDirect( - flatbuffers::FlatBufferBuilder &_fbb, - const std::vector *jointAccelerationRel = nullptr, - const std::vector *jointVelocityRel = nullptr, - bool updateMinimumTrajectoryExecutionTime = false, - double minimumTrajectoryExecutionTime = 0.0) { - return grl::flatbuffer::CreateSmartServo( - _fbb, - jointAccelerationRel ? _fbb.CreateVector(*jointAccelerationRel) : 0, - jointVelocityRel ? _fbb.CreateVector(*jointVelocityRel) : 0, - updateMinimumTrajectoryExecutionTime, - minimumTrajectoryExecutionTime); -} - -flatbuffers::Offset CreateSmartServo(flatbuffers::FlatBufferBuilder &_fbb, const SmartServoT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); - -struct ProcessDataT : public flatbuffers::NativeTable { - typedef ProcessData TableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.ProcessDataT"; - } - std::string dataType; - std::string defaultValue; - std::string displayName; - std::string id; - std::string min; - std::string max; - std::string unit; - std::string value; - bool shouldRemove; - bool shouldUpdate; - ProcessDataT() - : shouldRemove(false), - shouldUpdate(false) { - } -}; - -/// "ProcessData" is a field that appears -/// on your physical kuka tablet. -/// This message allows you to update these -/// fields on the tablet yourself. -struct ProcessData FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { - typedef ProcessDataT NativeTableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.ProcessData"; - } - enum { - VT_DATATYPE = 4, - VT_DEFAULTVALUE = 6, - VT_DISPLAYNAME = 8, - VT_ID = 10, - VT_MIN = 12, - VT_MAX = 14, - VT_UNIT = 16, - VT_VALUE = 18, - VT_SHOULDREMOVE = 20, - VT_SHOULDUPDATE = 22 - }; - const flatbuffers::String *dataType() const { - return GetPointer(VT_DATATYPE); - } - const flatbuffers::String *defaultValue() const { - return GetPointer(VT_DEFAULTVALUE); - } - const flatbuffers::String *displayName() const { - return GetPointer(VT_DISPLAYNAME); - } - const flatbuffers::String *id() const { - return GetPointer(VT_ID); - } - const flatbuffers::String *min() const { - return GetPointer(VT_MIN); - } - const flatbuffers::String *max() const { - return GetPointer(VT_MAX); - } - const flatbuffers::String *unit() const { - return GetPointer(VT_UNIT); - } - const flatbuffers::String *value() const { - return GetPointer(VT_VALUE); - } - /// should the data be removed completely? - bool shouldRemove() const { - return GetField(VT_SHOULDREMOVE, 0) != 0; - } - /// should the data be updated to these values? - bool shouldUpdate() const { - return GetField(VT_SHOULDUPDATE, 0) != 0; - } - bool Verify(flatbuffers::Verifier &verifier) const { - return VerifyTableStart(verifier) && - VerifyOffset(verifier, VT_DATATYPE) && - verifier.Verify(dataType()) && - VerifyOffset(verifier, VT_DEFAULTVALUE) && - verifier.Verify(defaultValue()) && - VerifyOffset(verifier, VT_DISPLAYNAME) && - verifier.Verify(displayName()) && - VerifyOffset(verifier, VT_ID) && - verifier.Verify(id()) && - VerifyOffset(verifier, VT_MIN) && - verifier.Verify(min()) && - VerifyOffset(verifier, VT_MAX) && - verifier.Verify(max()) && - VerifyOffset(verifier, VT_UNIT) && - verifier.Verify(unit()) && - VerifyOffset(verifier, VT_VALUE) && - verifier.Verify(value()) && - VerifyField(verifier, VT_SHOULDREMOVE) && - VerifyField(verifier, VT_SHOULDUPDATE) && - verifier.EndTable(); - } - ProcessDataT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; - void UnPackTo(ProcessDataT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; - static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const ProcessDataT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); -}; - -struct ProcessDataBuilder { - flatbuffers::FlatBufferBuilder &fbb_; - flatbuffers::uoffset_t start_; - void add_dataType(flatbuffers::Offset dataType) { - fbb_.AddOffset(ProcessData::VT_DATATYPE, dataType); - } - void add_defaultValue(flatbuffers::Offset defaultValue) { - fbb_.AddOffset(ProcessData::VT_DEFAULTVALUE, defaultValue); - } - void add_displayName(flatbuffers::Offset displayName) { - fbb_.AddOffset(ProcessData::VT_DISPLAYNAME, displayName); - } - void add_id(flatbuffers::Offset id) { - fbb_.AddOffset(ProcessData::VT_ID, id); - } - void add_min(flatbuffers::Offset min) { - fbb_.AddOffset(ProcessData::VT_MIN, min); - } - void add_max(flatbuffers::Offset max) { - fbb_.AddOffset(ProcessData::VT_MAX, max); - } - void add_unit(flatbuffers::Offset unit) { - fbb_.AddOffset(ProcessData::VT_UNIT, unit); - } - void add_value(flatbuffers::Offset value) { - fbb_.AddOffset(ProcessData::VT_VALUE, value); - } - void add_shouldRemove(bool shouldRemove) { - fbb_.AddElement(ProcessData::VT_SHOULDREMOVE, static_cast(shouldRemove), 0); - } - void add_shouldUpdate(bool shouldUpdate) { - fbb_.AddElement(ProcessData::VT_SHOULDUPDATE, static_cast(shouldUpdate), 0); - } - explicit ProcessDataBuilder(flatbuffers::FlatBufferBuilder &_fbb) - : fbb_(_fbb) { - start_ = fbb_.StartTable(); - } - ProcessDataBuilder &operator=(const ProcessDataBuilder &); - flatbuffers::Offset Finish() { - const auto end = fbb_.EndTable(start_); - auto o = flatbuffers::Offset(end); - return o; - } -}; - -inline flatbuffers::Offset CreateProcessData( - flatbuffers::FlatBufferBuilder &_fbb, - flatbuffers::Offset dataType = 0, - flatbuffers::Offset defaultValue = 0, - flatbuffers::Offset displayName = 0, - flatbuffers::Offset id = 0, - flatbuffers::Offset min = 0, - flatbuffers::Offset max = 0, - flatbuffers::Offset unit = 0, - flatbuffers::Offset value = 0, - bool shouldRemove = false, - bool shouldUpdate = false) { - ProcessDataBuilder builder_(_fbb); - builder_.add_value(value); - builder_.add_unit(unit); - builder_.add_max(max); - builder_.add_min(min); - builder_.add_id(id); - builder_.add_displayName(displayName); - builder_.add_defaultValue(defaultValue); - builder_.add_dataType(dataType); - builder_.add_shouldUpdate(shouldUpdate); - builder_.add_shouldRemove(shouldRemove); - return builder_.Finish(); -} - -inline flatbuffers::Offset CreateProcessDataDirect( - flatbuffers::FlatBufferBuilder &_fbb, - const char *dataType = nullptr, - const char *defaultValue = nullptr, - const char *displayName = nullptr, - const char *id = nullptr, - const char *min = nullptr, - const char *max = nullptr, - const char *unit = nullptr, - const char *value = nullptr, - bool shouldRemove = false, - bool shouldUpdate = false) { - return grl::flatbuffer::CreateProcessData( - _fbb, - dataType ? _fbb.CreateString(dataType) : 0, - defaultValue ? _fbb.CreateString(defaultValue) : 0, - displayName ? _fbb.CreateString(displayName) : 0, - id ? _fbb.CreateString(id) : 0, - min ? _fbb.CreateString(min) : 0, - max ? _fbb.CreateString(max) : 0, - unit ? _fbb.CreateString(unit) : 0, - value ? _fbb.CreateString(value) : 0, - shouldRemove, - shouldUpdate); -} - -flatbuffers::Offset CreateProcessData(flatbuffers::FlatBufferBuilder &_fbb, const ProcessDataT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); - -struct KUKAiiwaArmConfigurationT : public flatbuffers::NativeTable { - typedef KUKAiiwaArmConfiguration TableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.KUKAiiwaArmConfigurationT"; - } - std::string name; - KUKAiiwaInterface commandInterface; - KUKAiiwaInterface monitorInterface; - EClientCommandMode clientCommandMode; - EOverlayType overlayType; - EControlMode controlMode; - std::unique_ptr setCartImpedance; - std::unique_ptr setJointImpedance; - std::unique_ptr smartServoConfig; - std::unique_ptr FRIConfig; - std::vector> tools; - std::vector> processData; - std::string currentMotionCenter; - bool requestMonitorProcessData; - KUKAiiwaArmConfigurationT() - : commandInterface(KUKAiiwaInterface::Disabled), - monitorInterface(KUKAiiwaInterface::Disabled), - clientCommandMode(EClientCommandMode::NO_COMMAND_MODE), - overlayType(EOverlayType::NO_OVERLAY), - controlMode(EControlMode::POSITION_CONTROL_MODE), - requestMonitorProcessData(false) { - } -}; - -struct KUKAiiwaArmConfiguration FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { - typedef KUKAiiwaArmConfigurationT NativeTableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.KUKAiiwaArmConfiguration"; - } - enum { - VT_NAME = 4, - VT_COMMANDINTERFACE = 6, - VT_MONITORINTERFACE = 8, - VT_CLIENTCOMMANDMODE = 10, - VT_OVERLAYTYPE = 12, - VT_CONTROLMODE = 14, - VT_SETCARTIMPEDANCE = 16, - VT_SETJOINTIMPEDANCE = 18, - VT_SMARTSERVOCONFIG = 20, - VT_FRICONFIG = 22, - VT_TOOLS = 24, - VT_PROCESSDATA = 26, - VT_CURRENTMOTIONCENTER = 28, - VT_REQUESTMONITORPROCESSDATA = 30 - }; - const flatbuffers::String *name() const { - return GetPointer(VT_NAME); - } - /// how commands will be sent to robot - KUKAiiwaInterface commandInterface() const { - return static_cast(GetField(VT_COMMANDINTERFACE, 0)); - } - /// how robot state will be sent to driver - KUKAiiwaInterface monitorInterface() const { - return static_cast(GetField(VT_MONITORINTERFACE, 0)); - } - /// motion command mode: cartesian, wrench, torque commands - EClientCommandMode clientCommandMode() const { - return static_cast(GetField(VT_CLIENTCOMMANDMODE, 0)); - } - /// The type of commands FRI will use: cartesian, joint - EOverlayType overlayType() const { - return static_cast(GetField(VT_OVERLAYTYPE, 0)); - } - /// position, cartesian impedence, or joint impedence low level controller adjustments - EControlMode controlMode() const { - return static_cast(GetField(VT_CONTROLMODE, 0)); - } - const CartesianImpedenceControlMode *setCartImpedance() const { - return GetPointer(VT_SETCARTIMPEDANCE); - } - const JointImpedenceControlMode *setJointImpedance() const { - return GetPointer(VT_SETJOINTIMPEDANCE); - } - const SmartServo *smartServoConfig() const { - return GetPointer(VT_SMARTSERVOCONFIG); - } - const FRI *FRIConfig() const { - return GetPointer(VT_FRICONFIG); - } - const flatbuffers::Vector> *tools() const { - return GetPointer> *>(VT_TOOLS); - } - /// set kuka tablet "processData" panel UI config strings - const flatbuffers::Vector> *processData() const { - return GetPointer> *>(VT_PROCESSDATA); - } - const flatbuffers::String *currentMotionCenter() const { - return GetPointer(VT_CURRENTMOTIONCENTER); - } - bool requestMonitorProcessData() const { - return GetField(VT_REQUESTMONITORPROCESSDATA, 0) != 0; - } - bool Verify(flatbuffers::Verifier &verifier) const { - return VerifyTableStart(verifier) && - VerifyOffset(verifier, VT_NAME) && - verifier.Verify(name()) && - VerifyField(verifier, VT_COMMANDINTERFACE) && - VerifyField(verifier, VT_MONITORINTERFACE) && - VerifyField(verifier, VT_CLIENTCOMMANDMODE) && - VerifyField(verifier, VT_OVERLAYTYPE) && - VerifyField(verifier, VT_CONTROLMODE) && - VerifyOffset(verifier, VT_SETCARTIMPEDANCE) && - verifier.VerifyTable(setCartImpedance()) && - VerifyOffset(verifier, VT_SETJOINTIMPEDANCE) && - verifier.VerifyTable(setJointImpedance()) && - VerifyOffset(verifier, VT_SMARTSERVOCONFIG) && - verifier.VerifyTable(smartServoConfig()) && - VerifyOffset(verifier, VT_FRICONFIG) && - verifier.VerifyTable(FRIConfig()) && - VerifyOffset(verifier, VT_TOOLS) && - verifier.Verify(tools()) && - verifier.VerifyVectorOfTables(tools()) && - VerifyOffset(verifier, VT_PROCESSDATA) && - verifier.Verify(processData()) && - verifier.VerifyVectorOfTables(processData()) && - VerifyOffset(verifier, VT_CURRENTMOTIONCENTER) && - verifier.Verify(currentMotionCenter()) && - VerifyField(verifier, VT_REQUESTMONITORPROCESSDATA) && - verifier.EndTable(); - } - KUKAiiwaArmConfigurationT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; - void UnPackTo(KUKAiiwaArmConfigurationT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; - static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaArmConfigurationT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); -}; - -struct KUKAiiwaArmConfigurationBuilder { - flatbuffers::FlatBufferBuilder &fbb_; - flatbuffers::uoffset_t start_; - void add_name(flatbuffers::Offset name) { - fbb_.AddOffset(KUKAiiwaArmConfiguration::VT_NAME, name); - } - void add_commandInterface(KUKAiiwaInterface commandInterface) { - fbb_.AddElement(KUKAiiwaArmConfiguration::VT_COMMANDINTERFACE, static_cast(commandInterface), 0); - } - void add_monitorInterface(KUKAiiwaInterface monitorInterface) { - fbb_.AddElement(KUKAiiwaArmConfiguration::VT_MONITORINTERFACE, static_cast(monitorInterface), 0); - } - void add_clientCommandMode(EClientCommandMode clientCommandMode) { - fbb_.AddElement(KUKAiiwaArmConfiguration::VT_CLIENTCOMMANDMODE, static_cast(clientCommandMode), 0); - } - void add_overlayType(EOverlayType overlayType) { - fbb_.AddElement(KUKAiiwaArmConfiguration::VT_OVERLAYTYPE, static_cast(overlayType), 0); - } - void add_controlMode(EControlMode controlMode) { - fbb_.AddElement(KUKAiiwaArmConfiguration::VT_CONTROLMODE, static_cast(controlMode), 0); - } - void add_setCartImpedance(flatbuffers::Offset setCartImpedance) { - fbb_.AddOffset(KUKAiiwaArmConfiguration::VT_SETCARTIMPEDANCE, setCartImpedance); - } - void add_setJointImpedance(flatbuffers::Offset setJointImpedance) { - fbb_.AddOffset(KUKAiiwaArmConfiguration::VT_SETJOINTIMPEDANCE, setJointImpedance); - } - void add_smartServoConfig(flatbuffers::Offset smartServoConfig) { - fbb_.AddOffset(KUKAiiwaArmConfiguration::VT_SMARTSERVOCONFIG, smartServoConfig); - } - void add_FRIConfig(flatbuffers::Offset FRIConfig) { - fbb_.AddOffset(KUKAiiwaArmConfiguration::VT_FRICONFIG, FRIConfig); - } - void add_tools(flatbuffers::Offset>> tools) { - fbb_.AddOffset(KUKAiiwaArmConfiguration::VT_TOOLS, tools); - } - void add_processData(flatbuffers::Offset>> processData) { - fbb_.AddOffset(KUKAiiwaArmConfiguration::VT_PROCESSDATA, processData); - } - void add_currentMotionCenter(flatbuffers::Offset currentMotionCenter) { - fbb_.AddOffset(KUKAiiwaArmConfiguration::VT_CURRENTMOTIONCENTER, currentMotionCenter); - } - void add_requestMonitorProcessData(bool requestMonitorProcessData) { - fbb_.AddElement(KUKAiiwaArmConfiguration::VT_REQUESTMONITORPROCESSDATA, static_cast(requestMonitorProcessData), 0); - } - explicit KUKAiiwaArmConfigurationBuilder(flatbuffers::FlatBufferBuilder &_fbb) - : fbb_(_fbb) { - start_ = fbb_.StartTable(); - } - KUKAiiwaArmConfigurationBuilder &operator=(const KUKAiiwaArmConfigurationBuilder &); - flatbuffers::Offset Finish() { - const auto end = fbb_.EndTable(start_); - auto o = flatbuffers::Offset(end); - return o; - } -}; - -inline flatbuffers::Offset CreateKUKAiiwaArmConfiguration( - flatbuffers::FlatBufferBuilder &_fbb, - flatbuffers::Offset name = 0, - KUKAiiwaInterface commandInterface = KUKAiiwaInterface::Disabled, - KUKAiiwaInterface monitorInterface = KUKAiiwaInterface::Disabled, - EClientCommandMode clientCommandMode = EClientCommandMode::NO_COMMAND_MODE, - EOverlayType overlayType = EOverlayType::NO_OVERLAY, - EControlMode controlMode = EControlMode::POSITION_CONTROL_MODE, - flatbuffers::Offset setCartImpedance = 0, - flatbuffers::Offset setJointImpedance = 0, - flatbuffers::Offset smartServoConfig = 0, - flatbuffers::Offset FRIConfig = 0, - flatbuffers::Offset>> tools = 0, - flatbuffers::Offset>> processData = 0, - flatbuffers::Offset currentMotionCenter = 0, - bool requestMonitorProcessData = false) { - KUKAiiwaArmConfigurationBuilder builder_(_fbb); - builder_.add_currentMotionCenter(currentMotionCenter); - builder_.add_processData(processData); - builder_.add_tools(tools); - builder_.add_FRIConfig(FRIConfig); - builder_.add_smartServoConfig(smartServoConfig); - builder_.add_setJointImpedance(setJointImpedance); - builder_.add_setCartImpedance(setCartImpedance); - builder_.add_name(name); - builder_.add_requestMonitorProcessData(requestMonitorProcessData); - builder_.add_controlMode(controlMode); - builder_.add_overlayType(overlayType); - builder_.add_clientCommandMode(clientCommandMode); - builder_.add_monitorInterface(monitorInterface); - builder_.add_commandInterface(commandInterface); - return builder_.Finish(); -} - -inline flatbuffers::Offset CreateKUKAiiwaArmConfigurationDirect( - flatbuffers::FlatBufferBuilder &_fbb, - const char *name = nullptr, - KUKAiiwaInterface commandInterface = KUKAiiwaInterface::Disabled, - KUKAiiwaInterface monitorInterface = KUKAiiwaInterface::Disabled, - EClientCommandMode clientCommandMode = EClientCommandMode::NO_COMMAND_MODE, - EOverlayType overlayType = EOverlayType::NO_OVERLAY, - EControlMode controlMode = EControlMode::POSITION_CONTROL_MODE, - flatbuffers::Offset setCartImpedance = 0, - flatbuffers::Offset setJointImpedance = 0, - flatbuffers::Offset smartServoConfig = 0, - flatbuffers::Offset FRIConfig = 0, - const std::vector> *tools = nullptr, - const std::vector> *processData = nullptr, - const char *currentMotionCenter = nullptr, - bool requestMonitorProcessData = false) { - return grl::flatbuffer::CreateKUKAiiwaArmConfiguration( - _fbb, - name ? _fbb.CreateString(name) : 0, - commandInterface, - monitorInterface, - clientCommandMode, - overlayType, - controlMode, - setCartImpedance, - setJointImpedance, - smartServoConfig, - FRIConfig, - tools ? _fbb.CreateVector>(*tools) : 0, - processData ? _fbb.CreateVector>(*processData) : 0, - currentMotionCenter ? _fbb.CreateString(currentMotionCenter) : 0, - requestMonitorProcessData); -} - -flatbuffers::Offset CreateKUKAiiwaArmConfiguration(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaArmConfigurationT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); - -struct KUKAiiwaMonitorConfigurationT : public flatbuffers::NativeTable { - typedef KUKAiiwaMonitorConfiguration TableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.KUKAiiwaMonitorConfigurationT"; - } - std::string hardwareVersion; - std::vector torqueSensorLimits; - bool isReadyToMove; - bool isMastered; - std::vector> processData; - KUKAiiwaMonitorConfigurationT() - : isReadyToMove(false), - isMastered(false) { - } -}; - -struct KUKAiiwaMonitorConfiguration FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { - typedef KUKAiiwaMonitorConfigurationT NativeTableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.KUKAiiwaMonitorConfiguration"; - } - enum { - VT_HARDWAREVERSION = 4, - VT_TORQUESENSORLIMITS = 6, - VT_ISREADYTOMOVE = 8, - VT_ISMASTERED = 10, - VT_PROCESSDATA = 12 - }; - const flatbuffers::String *hardwareVersion() const { - return GetPointer(VT_HARDWAREVERSION); - } - const flatbuffers::Vector *torqueSensorLimits() const { - return GetPointer *>(VT_TORQUESENSORLIMITS); - } - bool isReadyToMove() const { - return GetField(VT_ISREADYTOMOVE, 0) != 0; - } - bool isMastered() const { - return GetField(VT_ISMASTERED, 0) != 0; - } - /// set kuka tablet "processData" panel UI config strings - const flatbuffers::Vector> *processData() const { - return GetPointer> *>(VT_PROCESSDATA); - } - bool Verify(flatbuffers::Verifier &verifier) const { - return VerifyTableStart(verifier) && - VerifyOffset(verifier, VT_HARDWAREVERSION) && - verifier.Verify(hardwareVersion()) && - VerifyOffset(verifier, VT_TORQUESENSORLIMITS) && - verifier.Verify(torqueSensorLimits()) && - VerifyField(verifier, VT_ISREADYTOMOVE) && - VerifyField(verifier, VT_ISMASTERED) && - VerifyOffset(verifier, VT_PROCESSDATA) && - verifier.Verify(processData()) && - verifier.VerifyVectorOfTables(processData()) && - verifier.EndTable(); - } - KUKAiiwaMonitorConfigurationT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; - void UnPackTo(KUKAiiwaMonitorConfigurationT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; - static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaMonitorConfigurationT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); -}; - -struct KUKAiiwaMonitorConfigurationBuilder { - flatbuffers::FlatBufferBuilder &fbb_; - flatbuffers::uoffset_t start_; - void add_hardwareVersion(flatbuffers::Offset hardwareVersion) { - fbb_.AddOffset(KUKAiiwaMonitorConfiguration::VT_HARDWAREVERSION, hardwareVersion); - } - void add_torqueSensorLimits(flatbuffers::Offset> torqueSensorLimits) { - fbb_.AddOffset(KUKAiiwaMonitorConfiguration::VT_TORQUESENSORLIMITS, torqueSensorLimits); - } - void add_isReadyToMove(bool isReadyToMove) { - fbb_.AddElement(KUKAiiwaMonitorConfiguration::VT_ISREADYTOMOVE, static_cast(isReadyToMove), 0); - } - void add_isMastered(bool isMastered) { - fbb_.AddElement(KUKAiiwaMonitorConfiguration::VT_ISMASTERED, static_cast(isMastered), 0); - } - void add_processData(flatbuffers::Offset>> processData) { - fbb_.AddOffset(KUKAiiwaMonitorConfiguration::VT_PROCESSDATA, processData); - } - explicit KUKAiiwaMonitorConfigurationBuilder(flatbuffers::FlatBufferBuilder &_fbb) - : fbb_(_fbb) { - start_ = fbb_.StartTable(); - } - KUKAiiwaMonitorConfigurationBuilder &operator=(const KUKAiiwaMonitorConfigurationBuilder &); - flatbuffers::Offset Finish() { - const auto end = fbb_.EndTable(start_); - auto o = flatbuffers::Offset(end); - return o; - } -}; - -inline flatbuffers::Offset CreateKUKAiiwaMonitorConfiguration( - flatbuffers::FlatBufferBuilder &_fbb, - flatbuffers::Offset hardwareVersion = 0, - flatbuffers::Offset> torqueSensorLimits = 0, - bool isReadyToMove = false, - bool isMastered = false, - flatbuffers::Offset>> processData = 0) { - KUKAiiwaMonitorConfigurationBuilder builder_(_fbb); - builder_.add_processData(processData); - builder_.add_torqueSensorLimits(torqueSensorLimits); - builder_.add_hardwareVersion(hardwareVersion); - builder_.add_isMastered(isMastered); - builder_.add_isReadyToMove(isReadyToMove); - return builder_.Finish(); -} - -inline flatbuffers::Offset CreateKUKAiiwaMonitorConfigurationDirect( - flatbuffers::FlatBufferBuilder &_fbb, - const char *hardwareVersion = nullptr, - const std::vector *torqueSensorLimits = nullptr, - bool isReadyToMove = false, - bool isMastered = false, - const std::vector> *processData = nullptr) { - return grl::flatbuffer::CreateKUKAiiwaMonitorConfiguration( - _fbb, - hardwareVersion ? _fbb.CreateString(hardwareVersion) : 0, - torqueSensorLimits ? _fbb.CreateVector(*torqueSensorLimits) : 0, - isReadyToMove, - isMastered, - processData ? _fbb.CreateVector>(*processData) : 0); -} - -flatbuffers::Offset CreateKUKAiiwaMonitorConfiguration(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaMonitorConfigurationT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); - -struct KUKAiiwaMonitorStateT : public flatbuffers::NativeTable { - typedef KUKAiiwaMonitorState TableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.KUKAiiwaMonitorStateT"; - } - std::unique_ptr measuredState; - std::unique_ptr cartesianFlangePose; - std::unique_ptr jointStateReal; - std::unique_ptr jointStateInterpolated; - std::unique_ptr externalState; - EOperationMode operationMode; - ESessionState sessionState; - std::unique_ptr CartesianWrench; - KUKAiiwaMonitorStateT() - : operationMode(EOperationMode::TEST_MODE_1), - sessionState(ESessionState::IDLE) { - } -}; - -struct KUKAiiwaMonitorState FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { - typedef KUKAiiwaMonitorStateT NativeTableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.KUKAiiwaMonitorState"; - } - enum { - VT_MEASUREDSTATE = 4, - VT_CARTESIANFLANGEPOSE = 6, - VT_JOINTSTATEREAL = 8, - VT_JOINTSTATEINTERPOLATED = 10, - VT_EXTERNALSTATE = 12, - VT_OPERATIONMODE = 14, - VT_SESSIONSTATE = 16, - VT_CARTESIANWRENCH = 18 - }; - const JointState *measuredState() const { - return GetPointer(VT_MEASUREDSTATE); - } - const Pose *cartesianFlangePose() const { - return GetStruct(VT_CARTESIANFLANGEPOSE); - } - const JointState *jointStateReal() const { - return GetPointer(VT_JOINTSTATEREAL); - } - const JointState *jointStateInterpolated() const { - return GetPointer(VT_JOINTSTATEINTERPOLATED); - } - /// The state of the arm as calculated by kuka after - /// subtracting the known weights of the arm - /// and any attachments configured to be present. - /// - /// Most likely only contains torque. - /// KukaState::ExternalTorque goes here - const JointState *externalState() const { - return GetPointer(VT_EXTERNALSTATE); - } - /// KUKA::FRI::EOperationMode - EOperationMode operationMode() const { - return static_cast(GetField(VT_OPERATIONMODE, 0)); - } - ESessionState sessionState() const { - return static_cast(GetField(VT_SESSIONSTATE, 0)); - } - const Wrench *CartesianWrench() const { - return GetStruct(VT_CARTESIANWRENCH); - } - bool Verify(flatbuffers::Verifier &verifier) const { - return VerifyTableStart(verifier) && - VerifyOffset(verifier, VT_MEASUREDSTATE) && - verifier.VerifyTable(measuredState()) && - VerifyField(verifier, VT_CARTESIANFLANGEPOSE) && - VerifyOffset(verifier, VT_JOINTSTATEREAL) && - verifier.VerifyTable(jointStateReal()) && - VerifyOffset(verifier, VT_JOINTSTATEINTERPOLATED) && - verifier.VerifyTable(jointStateInterpolated()) && - VerifyOffset(verifier, VT_EXTERNALSTATE) && - verifier.VerifyTable(externalState()) && - VerifyField(verifier, VT_OPERATIONMODE) && - VerifyField(verifier, VT_SESSIONSTATE) && - VerifyField(verifier, VT_CARTESIANWRENCH) && - verifier.EndTable(); - } - KUKAiiwaMonitorStateT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; - void UnPackTo(KUKAiiwaMonitorStateT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; - static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaMonitorStateT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); -}; - -struct KUKAiiwaMonitorStateBuilder { - flatbuffers::FlatBufferBuilder &fbb_; - flatbuffers::uoffset_t start_; - void add_measuredState(flatbuffers::Offset measuredState) { - fbb_.AddOffset(KUKAiiwaMonitorState::VT_MEASUREDSTATE, measuredState); - } - void add_cartesianFlangePose(const Pose *cartesianFlangePose) { - fbb_.AddStruct(KUKAiiwaMonitorState::VT_CARTESIANFLANGEPOSE, cartesianFlangePose); - } - void add_jointStateReal(flatbuffers::Offset jointStateReal) { - fbb_.AddOffset(KUKAiiwaMonitorState::VT_JOINTSTATEREAL, jointStateReal); - } - void add_jointStateInterpolated(flatbuffers::Offset jointStateInterpolated) { - fbb_.AddOffset(KUKAiiwaMonitorState::VT_JOINTSTATEINTERPOLATED, jointStateInterpolated); - } - void add_externalState(flatbuffers::Offset externalState) { - fbb_.AddOffset(KUKAiiwaMonitorState::VT_EXTERNALSTATE, externalState); - } - void add_operationMode(EOperationMode operationMode) { - fbb_.AddElement(KUKAiiwaMonitorState::VT_OPERATIONMODE, static_cast(operationMode), 0); - } - void add_sessionState(ESessionState sessionState) { - fbb_.AddElement(KUKAiiwaMonitorState::VT_SESSIONSTATE, static_cast(sessionState), 0); - } - void add_CartesianWrench(const Wrench *CartesianWrench) { - fbb_.AddStruct(KUKAiiwaMonitorState::VT_CARTESIANWRENCH, CartesianWrench); - } - explicit KUKAiiwaMonitorStateBuilder(flatbuffers::FlatBufferBuilder &_fbb) - : fbb_(_fbb) { - start_ = fbb_.StartTable(); - } - KUKAiiwaMonitorStateBuilder &operator=(const KUKAiiwaMonitorStateBuilder &); - flatbuffers::Offset Finish() { - const auto end = fbb_.EndTable(start_); - auto o = flatbuffers::Offset(end); - return o; - } -}; - -inline flatbuffers::Offset CreateKUKAiiwaMonitorState( - flatbuffers::FlatBufferBuilder &_fbb, - flatbuffers::Offset measuredState = 0, - const Pose *cartesianFlangePose = 0, - flatbuffers::Offset jointStateReal = 0, - flatbuffers::Offset jointStateInterpolated = 0, - flatbuffers::Offset externalState = 0, - EOperationMode operationMode = EOperationMode::TEST_MODE_1, - ESessionState sessionState = ESessionState::IDLE, - const Wrench *CartesianWrench = 0) { - KUKAiiwaMonitorStateBuilder builder_(_fbb); - builder_.add_CartesianWrench(CartesianWrench); - builder_.add_externalState(externalState); - builder_.add_jointStateInterpolated(jointStateInterpolated); - builder_.add_jointStateReal(jointStateReal); - builder_.add_cartesianFlangePose(cartesianFlangePose); - builder_.add_measuredState(measuredState); - builder_.add_sessionState(sessionState); - builder_.add_operationMode(operationMode); - return builder_.Finish(); -} - -flatbuffers::Offset CreateKUKAiiwaMonitorState(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaMonitorStateT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); - -struct FRITimeStampT : public flatbuffers::NativeTable { - typedef FRITimeStamp TableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.FRITimeStampT"; - } - int32_t sec; - int32_t nanosec; - FRITimeStampT() - : sec(0), - nanosec(0) { - } -}; - -struct FRITimeStamp FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { - typedef FRITimeStampT NativeTableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.FRITimeStamp"; - } - enum { - VT_SEC = 4, - VT_NANOSEC = 6 - }; - int32_t sec() const { - return GetField(VT_SEC, 0); - } - int32_t nanosec() const { - return GetField(VT_NANOSEC, 0); - } - bool Verify(flatbuffers::Verifier &verifier) const { - return VerifyTableStart(verifier) && - VerifyField(verifier, VT_SEC) && - VerifyField(verifier, VT_NANOSEC) && - verifier.EndTable(); - } - FRITimeStampT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; - void UnPackTo(FRITimeStampT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; - static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const FRITimeStampT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); -}; - -struct FRITimeStampBuilder { - flatbuffers::FlatBufferBuilder &fbb_; - flatbuffers::uoffset_t start_; - void add_sec(int32_t sec) { - fbb_.AddElement(FRITimeStamp::VT_SEC, sec, 0); - } - void add_nanosec(int32_t nanosec) { - fbb_.AddElement(FRITimeStamp::VT_NANOSEC, nanosec, 0); - } - explicit FRITimeStampBuilder(flatbuffers::FlatBufferBuilder &_fbb) - : fbb_(_fbb) { - start_ = fbb_.StartTable(); - } - FRITimeStampBuilder &operator=(const FRITimeStampBuilder &); - flatbuffers::Offset Finish() { - const auto end = fbb_.EndTable(start_); - auto o = flatbuffers::Offset(end); - return o; - } -}; - -inline flatbuffers::Offset CreateFRITimeStamp( - flatbuffers::FlatBufferBuilder &_fbb, - int32_t sec = 0, - int32_t nanosec = 0) { - FRITimeStampBuilder builder_(_fbb); - builder_.add_nanosec(nanosec); - builder_.add_sec(sec); - return builder_.Finish(); -} - -flatbuffers::Offset CreateFRITimeStamp(flatbuffers::FlatBufferBuilder &_fbb, const FRITimeStampT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); - -struct FRIMessageLogT : public flatbuffers::NativeTable { - typedef FRIMessageLog TableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.FRIMessageLogT"; - } - ESessionState sessionState; - EConnectionQuality connectionQuality; - EControlMode controlMode; - int32_t messageIdentifier; - int32_t sequenceCounter; - int32_t reflectedSequenceCounter; - std::vector measuredJointPosition; - std::vector measuredTorque; - std::vector commandedJointPosition; - std::vector commandedTorque; - std::vector externalTorque; - std::vector jointStateInterpolated; - std::unique_ptr timeStamp; - FRIMessageLogT() - : sessionState(ESessionState::IDLE), - connectionQuality(EConnectionQuality::POOR), - controlMode(EControlMode::POSITION_CONTROL_MODE), - messageIdentifier(0), - sequenceCounter(0), - reflectedSequenceCounter(0) { - } -}; - -struct FRIMessageLog FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { - typedef FRIMessageLogT NativeTableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.FRIMessageLog"; - } - enum { - VT_SESSIONSTATE = 4, - VT_CONNECTIONQUALITY = 6, - VT_CONTROLMODE = 8, - VT_MESSAGEIDENTIFIER = 10, - VT_SEQUENCECOUNTER = 12, - VT_REFLECTEDSEQUENCECOUNTER = 14, - VT_MEASUREDJOINTPOSITION = 16, - VT_MEASUREDTORQUE = 18, - VT_COMMANDEDJOINTPOSITION = 20, - VT_COMMANDEDTORQUE = 22, - VT_EXTERNALTORQUE = 24, - VT_JOINTSTATEINTERPOLATED = 26, - VT_TIMESTAMP = 28 - }; - ESessionState sessionState() const { - return static_cast(GetField(VT_SESSIONSTATE, 0)); - } - EConnectionQuality connectionQuality() const { - return static_cast(GetField(VT_CONNECTIONQUALITY, 0)); - } - EControlMode controlMode() const { - return static_cast(GetField(VT_CONTROLMODE, 0)); - } - int32_t messageIdentifier() const { - return GetField(VT_MESSAGEIDENTIFIER, 0); - } - int32_t sequenceCounter() const { - return GetField(VT_SEQUENCECOUNTER, 0); - } - int32_t reflectedSequenceCounter() const { - return GetField(VT_REFLECTEDSEQUENCECOUNTER, 0); - } - const flatbuffers::Vector *measuredJointPosition() const { - return GetPointer *>(VT_MEASUREDJOINTPOSITION); - } - const flatbuffers::Vector *measuredTorque() const { - return GetPointer *>(VT_MEASUREDTORQUE); - } - const flatbuffers::Vector *commandedJointPosition() const { - return GetPointer *>(VT_COMMANDEDJOINTPOSITION); - } - const flatbuffers::Vector *commandedTorque() const { - return GetPointer *>(VT_COMMANDEDTORQUE); - } - const flatbuffers::Vector *externalTorque() const { - return GetPointer *>(VT_EXTERNALTORQUE); - } - const flatbuffers::Vector *jointStateInterpolated() const { - return GetPointer *>(VT_JOINTSTATEINTERPOLATED); - } - const TimeEvent *timeStamp() const { - return GetPointer(VT_TIMESTAMP); - } - bool Verify(flatbuffers::Verifier &verifier) const { - return VerifyTableStart(verifier) && - VerifyField(verifier, VT_SESSIONSTATE) && - VerifyField(verifier, VT_CONNECTIONQUALITY) && - VerifyField(verifier, VT_CONTROLMODE) && - VerifyField(verifier, VT_MESSAGEIDENTIFIER) && - VerifyField(verifier, VT_SEQUENCECOUNTER) && - VerifyField(verifier, VT_REFLECTEDSEQUENCECOUNTER) && - VerifyOffset(verifier, VT_MEASUREDJOINTPOSITION) && - verifier.Verify(measuredJointPosition()) && - VerifyOffset(verifier, VT_MEASUREDTORQUE) && - verifier.Verify(measuredTorque()) && - VerifyOffset(verifier, VT_COMMANDEDJOINTPOSITION) && - verifier.Verify(commandedJointPosition()) && - VerifyOffset(verifier, VT_COMMANDEDTORQUE) && - verifier.Verify(commandedTorque()) && - VerifyOffset(verifier, VT_EXTERNALTORQUE) && - verifier.Verify(externalTorque()) && - VerifyOffset(verifier, VT_JOINTSTATEINTERPOLATED) && - verifier.Verify(jointStateInterpolated()) && - VerifyOffset(verifier, VT_TIMESTAMP) && - verifier.VerifyTable(timeStamp()) && - verifier.EndTable(); - } - FRIMessageLogT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; - void UnPackTo(FRIMessageLogT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; - static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const FRIMessageLogT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); -}; - -struct FRIMessageLogBuilder { - flatbuffers::FlatBufferBuilder &fbb_; - flatbuffers::uoffset_t start_; - void add_sessionState(ESessionState sessionState) { - fbb_.AddElement(FRIMessageLog::VT_SESSIONSTATE, static_cast(sessionState), 0); - } - void add_connectionQuality(EConnectionQuality connectionQuality) { - fbb_.AddElement(FRIMessageLog::VT_CONNECTIONQUALITY, static_cast(connectionQuality), 0); - } - void add_controlMode(EControlMode controlMode) { - fbb_.AddElement(FRIMessageLog::VT_CONTROLMODE, static_cast(controlMode), 0); - } - void add_messageIdentifier(int32_t messageIdentifier) { - fbb_.AddElement(FRIMessageLog::VT_MESSAGEIDENTIFIER, messageIdentifier, 0); - } - void add_sequenceCounter(int32_t sequenceCounter) { - fbb_.AddElement(FRIMessageLog::VT_SEQUENCECOUNTER, sequenceCounter, 0); - } - void add_reflectedSequenceCounter(int32_t reflectedSequenceCounter) { - fbb_.AddElement(FRIMessageLog::VT_REFLECTEDSEQUENCECOUNTER, reflectedSequenceCounter, 0); - } - void add_measuredJointPosition(flatbuffers::Offset> measuredJointPosition) { - fbb_.AddOffset(FRIMessageLog::VT_MEASUREDJOINTPOSITION, measuredJointPosition); - } - void add_measuredTorque(flatbuffers::Offset> measuredTorque) { - fbb_.AddOffset(FRIMessageLog::VT_MEASUREDTORQUE, measuredTorque); - } - void add_commandedJointPosition(flatbuffers::Offset> commandedJointPosition) { - fbb_.AddOffset(FRIMessageLog::VT_COMMANDEDJOINTPOSITION, commandedJointPosition); - } - void add_commandedTorque(flatbuffers::Offset> commandedTorque) { - fbb_.AddOffset(FRIMessageLog::VT_COMMANDEDTORQUE, commandedTorque); - } - void add_externalTorque(flatbuffers::Offset> externalTorque) { - fbb_.AddOffset(FRIMessageLog::VT_EXTERNALTORQUE, externalTorque); - } - void add_jointStateInterpolated(flatbuffers::Offset> jointStateInterpolated) { - fbb_.AddOffset(FRIMessageLog::VT_JOINTSTATEINTERPOLATED, jointStateInterpolated); - } - void add_timeStamp(flatbuffers::Offset timeStamp) { - fbb_.AddOffset(FRIMessageLog::VT_TIMESTAMP, timeStamp); - } - explicit FRIMessageLogBuilder(flatbuffers::FlatBufferBuilder &_fbb) - : fbb_(_fbb) { - start_ = fbb_.StartTable(); - } - FRIMessageLogBuilder &operator=(const FRIMessageLogBuilder &); - flatbuffers::Offset Finish() { - const auto end = fbb_.EndTable(start_); - auto o = flatbuffers::Offset(end); - return o; - } -}; - -inline flatbuffers::Offset CreateFRIMessageLog( - flatbuffers::FlatBufferBuilder &_fbb, - ESessionState sessionState = ESessionState::IDLE, - EConnectionQuality connectionQuality = EConnectionQuality::POOR, - EControlMode controlMode = EControlMode::POSITION_CONTROL_MODE, - int32_t messageIdentifier = 0, - int32_t sequenceCounter = 0, - int32_t reflectedSequenceCounter = 0, - flatbuffers::Offset> measuredJointPosition = 0, - flatbuffers::Offset> measuredTorque = 0, - flatbuffers::Offset> commandedJointPosition = 0, - flatbuffers::Offset> commandedTorque = 0, - flatbuffers::Offset> externalTorque = 0, - flatbuffers::Offset> jointStateInterpolated = 0, - flatbuffers::Offset timeStamp = 0) { - FRIMessageLogBuilder builder_(_fbb); - builder_.add_timeStamp(timeStamp); - builder_.add_jointStateInterpolated(jointStateInterpolated); - builder_.add_externalTorque(externalTorque); - builder_.add_commandedTorque(commandedTorque); - builder_.add_commandedJointPosition(commandedJointPosition); - builder_.add_measuredTorque(measuredTorque); - builder_.add_measuredJointPosition(measuredJointPosition); - builder_.add_reflectedSequenceCounter(reflectedSequenceCounter); - builder_.add_sequenceCounter(sequenceCounter); - builder_.add_messageIdentifier(messageIdentifier); - builder_.add_controlMode(controlMode); - builder_.add_connectionQuality(connectionQuality); - builder_.add_sessionState(sessionState); - return builder_.Finish(); -} - -inline flatbuffers::Offset CreateFRIMessageLogDirect( - flatbuffers::FlatBufferBuilder &_fbb, - ESessionState sessionState = ESessionState::IDLE, - EConnectionQuality connectionQuality = EConnectionQuality::POOR, - EControlMode controlMode = EControlMode::POSITION_CONTROL_MODE, - int32_t messageIdentifier = 0, - int32_t sequenceCounter = 0, - int32_t reflectedSequenceCounter = 0, - const std::vector *measuredJointPosition = nullptr, - const std::vector *measuredTorque = nullptr, - const std::vector *commandedJointPosition = nullptr, - const std::vector *commandedTorque = nullptr, - const std::vector *externalTorque = nullptr, - const std::vector *jointStateInterpolated = nullptr, - flatbuffers::Offset timeStamp = 0) { - return grl::flatbuffer::CreateFRIMessageLog( - _fbb, - sessionState, - connectionQuality, - controlMode, - messageIdentifier, - sequenceCounter, - reflectedSequenceCounter, - measuredJointPosition ? _fbb.CreateVector(*measuredJointPosition) : 0, - measuredTorque ? _fbb.CreateVector(*measuredTorque) : 0, - commandedJointPosition ? _fbb.CreateVector(*commandedJointPosition) : 0, - commandedTorque ? _fbb.CreateVector(*commandedTorque) : 0, - externalTorque ? _fbb.CreateVector(*externalTorque) : 0, - jointStateInterpolated ? _fbb.CreateVector(*jointStateInterpolated) : 0, - timeStamp); -} - -flatbuffers::Offset CreateFRIMessageLog(flatbuffers::FlatBufferBuilder &_fbb, const FRIMessageLogT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); - -struct KUKAiiwaStateT : public flatbuffers::NativeTable { - typedef KUKAiiwaState TableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.KUKAiiwaStateT"; - } - std::string name; - std::string destination; - std::string source; - std::unique_ptr timeStamp; - bool setArmControlState; - std::unique_ptr armControlState; - bool setArmConfiguration; - std::unique_ptr armConfiguration; - bool hasMonitorState; - std::unique_ptr monitorState; - bool hasMonitorConfig; - std::unique_ptr monitorConfig; - std::unique_ptr FRIMessage; - KUKAiiwaStateT() - : setArmControlState(false), - setArmConfiguration(false), - hasMonitorState(false), - hasMonitorConfig(false) { - } -}; - -struct KUKAiiwaState FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { - typedef KUKAiiwaStateT NativeTableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.KUKAiiwaState"; - } - enum { - VT_NAME = 4, - VT_DESTINATION = 6, - VT_SOURCE = 8, - VT_TIMESTAMP = 10, - VT_SETARMCONTROLSTATE = 12, - VT_ARMCONTROLSTATE = 14, - VT_SETARMCONFIGURATION = 16, - VT_ARMCONFIGURATION = 18, - VT_HASMONITORSTATE = 20, - VT_MONITORSTATE = 22, - VT_HASMONITORCONFIG = 24, - VT_MONITORCONFIG = 26, - VT_FRIMESSAGE = 28 - }; - const flatbuffers::String *name() const { - return GetPointer(VT_NAME); - } - const flatbuffers::String *destination() const { - return GetPointer(VT_DESTINATION); - } - const flatbuffers::String *source() const { - return GetPointer(VT_SOURCE); - } - const TimeEvent *timeStamp() const { - return GetPointer(VT_TIMESTAMP); - } - bool setArmControlState() const { - return GetField(VT_SETARMCONTROLSTATE, 0) != 0; - } - const ArmControlState *armControlState() const { - return GetPointer(VT_ARMCONTROLSTATE); - } - bool setArmConfiguration() const { - return GetField(VT_SETARMCONFIGURATION, 0) != 0; - } - const KUKAiiwaArmConfiguration *armConfiguration() const { - return GetPointer(VT_ARMCONFIGURATION); - } - bool hasMonitorState() const { - return GetField(VT_HASMONITORSTATE, 0) != 0; - } - const KUKAiiwaMonitorState *monitorState() const { - return GetPointer(VT_MONITORSTATE); - } - bool hasMonitorConfig() const { - return GetField(VT_HASMONITORCONFIG, 0) != 0; - } - const KUKAiiwaMonitorConfiguration *monitorConfig() const { - return GetPointer(VT_MONITORCONFIG); - } - const FRIMessageLog *FRIMessage() const { - return GetPointer(VT_FRIMESSAGE); - } - bool Verify(flatbuffers::Verifier &verifier) const { - return VerifyTableStart(verifier) && - VerifyOffset(verifier, VT_NAME) && - verifier.Verify(name()) && - VerifyOffset(verifier, VT_DESTINATION) && - verifier.Verify(destination()) && - VerifyOffset(verifier, VT_SOURCE) && - verifier.Verify(source()) && - VerifyOffset(verifier, VT_TIMESTAMP) && - verifier.VerifyTable(timeStamp()) && - VerifyField(verifier, VT_SETARMCONTROLSTATE) && - VerifyOffset(verifier, VT_ARMCONTROLSTATE) && - verifier.VerifyTable(armControlState()) && - VerifyField(verifier, VT_SETARMCONFIGURATION) && - VerifyOffset(verifier, VT_ARMCONFIGURATION) && - verifier.VerifyTable(armConfiguration()) && - VerifyField(verifier, VT_HASMONITORSTATE) && - VerifyOffset(verifier, VT_MONITORSTATE) && - verifier.VerifyTable(monitorState()) && - VerifyField(verifier, VT_HASMONITORCONFIG) && - VerifyOffset(verifier, VT_MONITORCONFIG) && - verifier.VerifyTable(monitorConfig()) && - VerifyOffset(verifier, VT_FRIMESSAGE) && - verifier.VerifyTable(FRIMessage()) && - verifier.EndTable(); - } - KUKAiiwaStateT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; - void UnPackTo(KUKAiiwaStateT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; - static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaStateT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); -}; - -struct KUKAiiwaStateBuilder { - flatbuffers::FlatBufferBuilder &fbb_; - flatbuffers::uoffset_t start_; - void add_name(flatbuffers::Offset name) { - fbb_.AddOffset(KUKAiiwaState::VT_NAME, name); - } - void add_destination(flatbuffers::Offset destination) { - fbb_.AddOffset(KUKAiiwaState::VT_DESTINATION, destination); - } - void add_source(flatbuffers::Offset source) { - fbb_.AddOffset(KUKAiiwaState::VT_SOURCE, source); - } - void add_timeStamp(flatbuffers::Offset timeStamp) { - fbb_.AddOffset(KUKAiiwaState::VT_TIMESTAMP, timeStamp); - } - void add_setArmControlState(bool setArmControlState) { - fbb_.AddElement(KUKAiiwaState::VT_SETARMCONTROLSTATE, static_cast(setArmControlState), 0); - } - void add_armControlState(flatbuffers::Offset armControlState) { - fbb_.AddOffset(KUKAiiwaState::VT_ARMCONTROLSTATE, armControlState); - } - void add_setArmConfiguration(bool setArmConfiguration) { - fbb_.AddElement(KUKAiiwaState::VT_SETARMCONFIGURATION, static_cast(setArmConfiguration), 0); - } - void add_armConfiguration(flatbuffers::Offset armConfiguration) { - fbb_.AddOffset(KUKAiiwaState::VT_ARMCONFIGURATION, armConfiguration); - } - void add_hasMonitorState(bool hasMonitorState) { - fbb_.AddElement(KUKAiiwaState::VT_HASMONITORSTATE, static_cast(hasMonitorState), 0); - } - void add_monitorState(flatbuffers::Offset monitorState) { - fbb_.AddOffset(KUKAiiwaState::VT_MONITORSTATE, monitorState); - } - void add_hasMonitorConfig(bool hasMonitorConfig) { - fbb_.AddElement(KUKAiiwaState::VT_HASMONITORCONFIG, static_cast(hasMonitorConfig), 0); - } - void add_monitorConfig(flatbuffers::Offset monitorConfig) { - fbb_.AddOffset(KUKAiiwaState::VT_MONITORCONFIG, monitorConfig); - } - void add_FRIMessage(flatbuffers::Offset FRIMessage) { - fbb_.AddOffset(KUKAiiwaState::VT_FRIMESSAGE, FRIMessage); - } - explicit KUKAiiwaStateBuilder(flatbuffers::FlatBufferBuilder &_fbb) - : fbb_(_fbb) { - start_ = fbb_.StartTable(); - } - KUKAiiwaStateBuilder &operator=(const KUKAiiwaStateBuilder &); - flatbuffers::Offset Finish() { - const auto end = fbb_.EndTable(start_); - auto o = flatbuffers::Offset(end); - return o; - } -}; - -inline flatbuffers::Offset CreateKUKAiiwaState( - flatbuffers::FlatBufferBuilder &_fbb, - flatbuffers::Offset name = 0, - flatbuffers::Offset destination = 0, - flatbuffers::Offset source = 0, - flatbuffers::Offset timeStamp = 0, - bool setArmControlState = false, - flatbuffers::Offset armControlState = 0, - bool setArmConfiguration = false, - flatbuffers::Offset armConfiguration = 0, - bool hasMonitorState = false, - flatbuffers::Offset monitorState = 0, - bool hasMonitorConfig = false, - flatbuffers::Offset monitorConfig = 0, - flatbuffers::Offset FRIMessage = 0) { - KUKAiiwaStateBuilder builder_(_fbb); - builder_.add_FRIMessage(FRIMessage); - builder_.add_monitorConfig(monitorConfig); - builder_.add_monitorState(monitorState); - builder_.add_armConfiguration(armConfiguration); - builder_.add_armControlState(armControlState); - builder_.add_timeStamp(timeStamp); - builder_.add_source(source); - builder_.add_destination(destination); - builder_.add_name(name); - builder_.add_hasMonitorConfig(hasMonitorConfig); - builder_.add_hasMonitorState(hasMonitorState); - builder_.add_setArmConfiguration(setArmConfiguration); - builder_.add_setArmControlState(setArmControlState); - return builder_.Finish(); -} - -inline flatbuffers::Offset CreateKUKAiiwaStateDirect( - flatbuffers::FlatBufferBuilder &_fbb, - const char *name = nullptr, - const char *destination = nullptr, - const char *source = nullptr, - flatbuffers::Offset timeStamp = 0, - bool setArmControlState = false, - flatbuffers::Offset armControlState = 0, - bool setArmConfiguration = false, - flatbuffers::Offset armConfiguration = 0, - bool hasMonitorState = false, - flatbuffers::Offset monitorState = 0, - bool hasMonitorConfig = false, - flatbuffers::Offset monitorConfig = 0, - flatbuffers::Offset FRIMessage = 0) { - return grl::flatbuffer::CreateKUKAiiwaState( - _fbb, - name ? _fbb.CreateString(name) : 0, - destination ? _fbb.CreateString(destination) : 0, - source ? _fbb.CreateString(source) : 0, - timeStamp, - setArmControlState, - armControlState, - setArmConfiguration, - armConfiguration, - hasMonitorState, - monitorState, - hasMonitorConfig, - monitorConfig, - FRIMessage); -} - -flatbuffers::Offset CreateKUKAiiwaState(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaStateT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); - -struct KUKAiiwaStatesT : public flatbuffers::NativeTable { - typedef KUKAiiwaStates TableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.KUKAiiwaStatesT"; - } - std::vector> states; - KUKAiiwaStatesT() { - } -}; - -struct KUKAiiwaStates FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { - typedef KUKAiiwaStatesT NativeTableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.KUKAiiwaStates"; - } - enum { - VT_STATES = 4 - }; - const flatbuffers::Vector> *states() const { - return GetPointer> *>(VT_STATES); - } - bool Verify(flatbuffers::Verifier &verifier) const { - return VerifyTableStart(verifier) && - VerifyOffset(verifier, VT_STATES) && - verifier.Verify(states()) && - verifier.VerifyVectorOfTables(states()) && - verifier.EndTable(); - } - KUKAiiwaStatesT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; - void UnPackTo(KUKAiiwaStatesT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; - static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaStatesT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); -}; - -struct KUKAiiwaStatesBuilder { - flatbuffers::FlatBufferBuilder &fbb_; - flatbuffers::uoffset_t start_; - void add_states(flatbuffers::Offset>> states) { - fbb_.AddOffset(KUKAiiwaStates::VT_STATES, states); - } - explicit KUKAiiwaStatesBuilder(flatbuffers::FlatBufferBuilder &_fbb) - : fbb_(_fbb) { - start_ = fbb_.StartTable(); - } - KUKAiiwaStatesBuilder &operator=(const KUKAiiwaStatesBuilder &); - flatbuffers::Offset Finish() { - const auto end = fbb_.EndTable(start_); - auto o = flatbuffers::Offset(end); - return o; - } -}; - -inline flatbuffers::Offset CreateKUKAiiwaStates( - flatbuffers::FlatBufferBuilder &_fbb, - flatbuffers::Offset>> states = 0) { - KUKAiiwaStatesBuilder builder_(_fbb); - builder_.add_states(states); - return builder_.Finish(); -} - -inline flatbuffers::Offset CreateKUKAiiwaStatesDirect( - flatbuffers::FlatBufferBuilder &_fbb, - const std::vector> *states = nullptr) { - return grl::flatbuffer::CreateKUKAiiwaStates( - _fbb, - states ? _fbb.CreateVector>(*states) : 0); -} - -flatbuffers::Offset CreateKUKAiiwaStates(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaStatesT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); - -inline CartesianImpedenceControlModeT *CartesianImpedenceControlMode::UnPack(const flatbuffers::resolver_function_t *_resolver) const { - auto _o = new CartesianImpedenceControlModeT(); - UnPackTo(_o, _resolver); - return _o; -} - -inline void CartesianImpedenceControlMode::UnPackTo(CartesianImpedenceControlModeT *_o, const flatbuffers::resolver_function_t *_resolver) const { - (void)_o; - (void)_resolver; - { auto _e = stiffness(); if (_e) _o->stiffness = std::unique_ptr(new EulerPose(*_e)); }; - { auto _e = damping(); if (_e) _o->damping = std::unique_ptr(new EulerPose(*_e)); }; - { auto _e = nullspaceStiffness(); _o->nullspaceStiffness = _e; }; - { auto _e = nullspaceDamping(); _o->nullspaceDamping = _e; }; - { auto _e = maxPathDeviation(); if (_e) _o->maxPathDeviation = std::unique_ptr(new EulerPose(*_e)); }; - { auto _e = maxCartesianVelocity(); if (_e) _o->maxCartesianVelocity = std::unique_ptr(new EulerPose(*_e)); }; - { auto _e = maxControlForce(); if (_e) _o->maxControlForce = std::unique_ptr(new EulerPose(*_e)); }; - { auto _e = maxControlForceExceededStop(); _o->maxControlForceExceededStop = _e; }; -} - -inline flatbuffers::Offset CartesianImpedenceControlMode::Pack(flatbuffers::FlatBufferBuilder &_fbb, const CartesianImpedenceControlModeT* _o, const flatbuffers::rehasher_function_t *_rehasher) { - return CreateCartesianImpedenceControlMode(_fbb, _o, _rehasher); -} - -inline flatbuffers::Offset CreateCartesianImpedenceControlMode(flatbuffers::FlatBufferBuilder &_fbb, const CartesianImpedenceControlModeT *_o, const flatbuffers::rehasher_function_t *_rehasher) { - (void)_rehasher; - (void)_o; - struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const CartesianImpedenceControlModeT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; - auto _stiffness = _o->stiffness ? _o->stiffness.get() : 0; - auto _damping = _o->damping ? _o->damping.get() : 0; - auto _nullspaceStiffness = _o->nullspaceStiffness; - auto _nullspaceDamping = _o->nullspaceDamping; - auto _maxPathDeviation = _o->maxPathDeviation ? _o->maxPathDeviation.get() : 0; - auto _maxCartesianVelocity = _o->maxCartesianVelocity ? _o->maxCartesianVelocity.get() : 0; - auto _maxControlForce = _o->maxControlForce ? _o->maxControlForce.get() : 0; - auto _maxControlForceExceededStop = _o->maxControlForceExceededStop; - return grl::flatbuffer::CreateCartesianImpedenceControlMode( - _fbb, - _stiffness, - _damping, - _nullspaceStiffness, - _nullspaceDamping, - _maxPathDeviation, - _maxCartesianVelocity, - _maxControlForce, - _maxControlForceExceededStop); -} - -inline JointImpedenceControlModeT *JointImpedenceControlMode::UnPack(const flatbuffers::resolver_function_t *_resolver) const { - auto _o = new JointImpedenceControlModeT(); - UnPackTo(_o, _resolver); - return _o; -} - -inline void JointImpedenceControlMode::UnPackTo(JointImpedenceControlModeT *_o, const flatbuffers::resolver_function_t *_resolver) const { - (void)_o; - (void)_resolver; - { auto _e = stiffness(); if (_e) { _o->stiffness.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->stiffness[_i] = _e->Get(_i); } } }; - { auto _e = damping(); if (_e) { _o->damping.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->damping[_i] = _e->Get(_i); } } }; -} - -inline flatbuffers::Offset JointImpedenceControlMode::Pack(flatbuffers::FlatBufferBuilder &_fbb, const JointImpedenceControlModeT* _o, const flatbuffers::rehasher_function_t *_rehasher) { - return CreateJointImpedenceControlMode(_fbb, _o, _rehasher); -} - -inline flatbuffers::Offset CreateJointImpedenceControlMode(flatbuffers::FlatBufferBuilder &_fbb, const JointImpedenceControlModeT *_o, const flatbuffers::rehasher_function_t *_rehasher) { - (void)_rehasher; - (void)_o; - struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const JointImpedenceControlModeT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; - auto _stiffness = _o->stiffness.size() ? _fbb.CreateVector(_o->stiffness) : 0; - auto _damping = _o->damping.size() ? _fbb.CreateVector(_o->damping) : 0; - return grl::flatbuffer::CreateJointImpedenceControlMode( - _fbb, - _stiffness, - _damping); -} - -inline DisabledT *Disabled::UnPack(const flatbuffers::resolver_function_t *_resolver) const { - auto _o = new DisabledT(); - UnPackTo(_o, _resolver); - return _o; -} - -inline void Disabled::UnPackTo(DisabledT *_o, const flatbuffers::resolver_function_t *_resolver) const { - (void)_o; - (void)_resolver; -} - -inline flatbuffers::Offset Disabled::Pack(flatbuffers::FlatBufferBuilder &_fbb, const DisabledT* _o, const flatbuffers::rehasher_function_t *_rehasher) { - return CreateDisabled(_fbb, _o, _rehasher); -} - -inline flatbuffers::Offset CreateDisabled(flatbuffers::FlatBufferBuilder &_fbb, const DisabledT *_o, const flatbuffers::rehasher_function_t *_rehasher) { - (void)_rehasher; - (void)_o; - struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const DisabledT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; - return grl::flatbuffer::CreateDisabled( - _fbb); -} - -inline FRIT *FRI::UnPack(const flatbuffers::resolver_function_t *_resolver) const { - auto _o = new FRIT(); - UnPackTo(_o, _resolver); - return _o; -} - -inline void FRI::UnPackTo(FRIT *_o, const flatbuffers::resolver_function_t *_resolver) const { - (void)_o; - (void)_resolver; - { auto _e = overlayType(); _o->overlayType = _e; }; - { auto _e = sendPeriodMillisec(); _o->sendPeriodMillisec = _e; }; - { auto _e = setReceiveMultiplier(); _o->setReceiveMultiplier = _e; }; - { auto _e = updatePortOnRemote(); _o->updatePortOnRemote = _e; }; - { auto _e = portOnRemote(); _o->portOnRemote = _e; }; - { auto _e = updatePortOnController(); _o->updatePortOnController = _e; }; - { auto _e = portOnController(); _o->portOnController = _e; }; -} - -inline flatbuffers::Offset FRI::Pack(flatbuffers::FlatBufferBuilder &_fbb, const FRIT* _o, const flatbuffers::rehasher_function_t *_rehasher) { - return CreateFRI(_fbb, _o, _rehasher); -} - -inline flatbuffers::Offset CreateFRI(flatbuffers::FlatBufferBuilder &_fbb, const FRIT *_o, const flatbuffers::rehasher_function_t *_rehasher) { - (void)_rehasher; - (void)_o; - struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const FRIT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; - auto _overlayType = _o->overlayType; - auto _sendPeriodMillisec = _o->sendPeriodMillisec; - auto _setReceiveMultiplier = _o->setReceiveMultiplier; - auto _updatePortOnRemote = _o->updatePortOnRemote; - auto _portOnRemote = _o->portOnRemote; - auto _updatePortOnController = _o->updatePortOnController; - auto _portOnController = _o->portOnController; - return grl::flatbuffer::CreateFRI( - _fbb, - _overlayType, - _sendPeriodMillisec, - _setReceiveMultiplier, - _updatePortOnRemote, - _portOnRemote, - _updatePortOnController, - _portOnController); -} - -inline SmartServoT *SmartServo::UnPack(const flatbuffers::resolver_function_t *_resolver) const { - auto _o = new SmartServoT(); - UnPackTo(_o, _resolver); - return _o; -} - -inline void SmartServo::UnPackTo(SmartServoT *_o, const flatbuffers::resolver_function_t *_resolver) const { - (void)_o; - (void)_resolver; - { auto _e = jointAccelerationRel(); if (_e) { _o->jointAccelerationRel.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->jointAccelerationRel[_i] = _e->Get(_i); } } }; - { auto _e = jointVelocityRel(); if (_e) { _o->jointVelocityRel.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->jointVelocityRel[_i] = _e->Get(_i); } } }; - { auto _e = updateMinimumTrajectoryExecutionTime(); _o->updateMinimumTrajectoryExecutionTime = _e; }; - { auto _e = minimumTrajectoryExecutionTime(); _o->minimumTrajectoryExecutionTime = _e; }; -} - -inline flatbuffers::Offset SmartServo::Pack(flatbuffers::FlatBufferBuilder &_fbb, const SmartServoT* _o, const flatbuffers::rehasher_function_t *_rehasher) { - return CreateSmartServo(_fbb, _o, _rehasher); -} - -inline flatbuffers::Offset CreateSmartServo(flatbuffers::FlatBufferBuilder &_fbb, const SmartServoT *_o, const flatbuffers::rehasher_function_t *_rehasher) { - (void)_rehasher; - (void)_o; - struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const SmartServoT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; - auto _jointAccelerationRel = _o->jointAccelerationRel.size() ? _fbb.CreateVector(_o->jointAccelerationRel) : 0; - auto _jointVelocityRel = _o->jointVelocityRel.size() ? _fbb.CreateVector(_o->jointVelocityRel) : 0; - auto _updateMinimumTrajectoryExecutionTime = _o->updateMinimumTrajectoryExecutionTime; - auto _minimumTrajectoryExecutionTime = _o->minimumTrajectoryExecutionTime; - return grl::flatbuffer::CreateSmartServo( - _fbb, - _jointAccelerationRel, - _jointVelocityRel, - _updateMinimumTrajectoryExecutionTime, - _minimumTrajectoryExecutionTime); -} - -inline ProcessDataT *ProcessData::UnPack(const flatbuffers::resolver_function_t *_resolver) const { - auto _o = new ProcessDataT(); - UnPackTo(_o, _resolver); - return _o; -} - -inline void ProcessData::UnPackTo(ProcessDataT *_o, const flatbuffers::resolver_function_t *_resolver) const { - (void)_o; - (void)_resolver; - { auto _e = dataType(); if (_e) _o->dataType = _e->str(); }; - { auto _e = defaultValue(); if (_e) _o->defaultValue = _e->str(); }; - { auto _e = displayName(); if (_e) _o->displayName = _e->str(); }; - { auto _e = id(); if (_e) _o->id = _e->str(); }; - { auto _e = min(); if (_e) _o->min = _e->str(); }; - { auto _e = max(); if (_e) _o->max = _e->str(); }; - { auto _e = unit(); if (_e) _o->unit = _e->str(); }; - { auto _e = value(); if (_e) _o->value = _e->str(); }; - { auto _e = shouldRemove(); _o->shouldRemove = _e; }; - { auto _e = shouldUpdate(); _o->shouldUpdate = _e; }; -} - -inline flatbuffers::Offset ProcessData::Pack(flatbuffers::FlatBufferBuilder &_fbb, const ProcessDataT* _o, const flatbuffers::rehasher_function_t *_rehasher) { - return CreateProcessData(_fbb, _o, _rehasher); -} - -inline flatbuffers::Offset CreateProcessData(flatbuffers::FlatBufferBuilder &_fbb, const ProcessDataT *_o, const flatbuffers::rehasher_function_t *_rehasher) { - (void)_rehasher; - (void)_o; - struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const ProcessDataT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; - auto _dataType = _o->dataType.empty() ? 0 : _fbb.CreateString(_o->dataType); - auto _defaultValue = _o->defaultValue.empty() ? 0 : _fbb.CreateString(_o->defaultValue); - auto _displayName = _o->displayName.empty() ? 0 : _fbb.CreateString(_o->displayName); - auto _id = _o->id.empty() ? 0 : _fbb.CreateString(_o->id); - auto _min = _o->min.empty() ? 0 : _fbb.CreateString(_o->min); - auto _max = _o->max.empty() ? 0 : _fbb.CreateString(_o->max); - auto _unit = _o->unit.empty() ? 0 : _fbb.CreateString(_o->unit); - auto _value = _o->value.empty() ? 0 : _fbb.CreateString(_o->value); - auto _shouldRemove = _o->shouldRemove; - auto _shouldUpdate = _o->shouldUpdate; - return grl::flatbuffer::CreateProcessData( - _fbb, - _dataType, - _defaultValue, - _displayName, - _id, - _min, - _max, - _unit, - _value, - _shouldRemove, - _shouldUpdate); -} - -inline KUKAiiwaArmConfigurationT *KUKAiiwaArmConfiguration::UnPack(const flatbuffers::resolver_function_t *_resolver) const { - auto _o = new KUKAiiwaArmConfigurationT(); - UnPackTo(_o, _resolver); - return _o; -} - -inline void KUKAiiwaArmConfiguration::UnPackTo(KUKAiiwaArmConfigurationT *_o, const flatbuffers::resolver_function_t *_resolver) const { - (void)_o; - (void)_resolver; - { auto _e = name(); if (_e) _o->name = _e->str(); }; - { auto _e = commandInterface(); _o->commandInterface = _e; }; - { auto _e = monitorInterface(); _o->monitorInterface = _e; }; - { auto _e = clientCommandMode(); _o->clientCommandMode = _e; }; - { auto _e = overlayType(); _o->overlayType = _e; }; - { auto _e = controlMode(); _o->controlMode = _e; }; - { auto _e = setCartImpedance(); if (_e) _o->setCartImpedance = std::unique_ptr(_e->UnPack(_resolver)); }; - { auto _e = setJointImpedance(); if (_e) _o->setJointImpedance = std::unique_ptr(_e->UnPack(_resolver)); }; - { auto _e = smartServoConfig(); if (_e) _o->smartServoConfig = std::unique_ptr(_e->UnPack(_resolver)); }; - { auto _e = FRIConfig(); if (_e) _o->FRIConfig = std::unique_ptr(_e->UnPack(_resolver)); }; - { auto _e = tools(); if (_e) { _o->tools.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->tools[_i] = std::unique_ptr(_e->Get(_i)->UnPack(_resolver)); } } }; - { auto _e = processData(); if (_e) { _o->processData.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->processData[_i] = std::unique_ptr(_e->Get(_i)->UnPack(_resolver)); } } }; - { auto _e = currentMotionCenter(); if (_e) _o->currentMotionCenter = _e->str(); }; - { auto _e = requestMonitorProcessData(); _o->requestMonitorProcessData = _e; }; -} - -inline flatbuffers::Offset KUKAiiwaArmConfiguration::Pack(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaArmConfigurationT* _o, const flatbuffers::rehasher_function_t *_rehasher) { - return CreateKUKAiiwaArmConfiguration(_fbb, _o, _rehasher); -} - -inline flatbuffers::Offset CreateKUKAiiwaArmConfiguration(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaArmConfigurationT *_o, const flatbuffers::rehasher_function_t *_rehasher) { - (void)_rehasher; - (void)_o; - struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const KUKAiiwaArmConfigurationT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; - auto _name = _o->name.empty() ? 0 : _fbb.CreateString(_o->name); - auto _commandInterface = _o->commandInterface; - auto _monitorInterface = _o->monitorInterface; - auto _clientCommandMode = _o->clientCommandMode; - auto _overlayType = _o->overlayType; - auto _controlMode = _o->controlMode; - auto _setCartImpedance = _o->setCartImpedance ? CreateCartesianImpedenceControlMode(_fbb, _o->setCartImpedance.get(), _rehasher) : 0; - auto _setJointImpedance = _o->setJointImpedance ? CreateJointImpedenceControlMode(_fbb, _o->setJointImpedance.get(), _rehasher) : 0; - auto _smartServoConfig = _o->smartServoConfig ? CreateSmartServo(_fbb, _o->smartServoConfig.get(), _rehasher) : 0; - auto _FRIConfig = _o->FRIConfig ? CreateFRI(_fbb, _o->FRIConfig.get(), _rehasher) : 0; - auto _tools = _o->tools.size() ? _fbb.CreateVector> (_o->tools.size(), [](size_t i, _VectorArgs *__va) { return CreateLinkObject(*__va->__fbb, __va->__o->tools[i].get(), __va->__rehasher); }, &_va ) : 0; - auto _processData = _o->processData.size() ? _fbb.CreateVector> (_o->processData.size(), [](size_t i, _VectorArgs *__va) { return CreateProcessData(*__va->__fbb, __va->__o->processData[i].get(), __va->__rehasher); }, &_va ) : 0; - auto _currentMotionCenter = _o->currentMotionCenter.empty() ? 0 : _fbb.CreateString(_o->currentMotionCenter); - auto _requestMonitorProcessData = _o->requestMonitorProcessData; - return grl::flatbuffer::CreateKUKAiiwaArmConfiguration( - _fbb, - _name, - _commandInterface, - _monitorInterface, - _clientCommandMode, - _overlayType, - _controlMode, - _setCartImpedance, - _setJointImpedance, - _smartServoConfig, - _FRIConfig, - _tools, - _processData, - _currentMotionCenter, - _requestMonitorProcessData); -} - -inline KUKAiiwaMonitorConfigurationT *KUKAiiwaMonitorConfiguration::UnPack(const flatbuffers::resolver_function_t *_resolver) const { - auto _o = new KUKAiiwaMonitorConfigurationT(); - UnPackTo(_o, _resolver); - return _o; -} - -inline void KUKAiiwaMonitorConfiguration::UnPackTo(KUKAiiwaMonitorConfigurationT *_o, const flatbuffers::resolver_function_t *_resolver) const { - (void)_o; - (void)_resolver; - { auto _e = hardwareVersion(); if (_e) _o->hardwareVersion = _e->str(); }; - { auto _e = torqueSensorLimits(); if (_e) { _o->torqueSensorLimits.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->torqueSensorLimits[_i] = _e->Get(_i); } } }; - { auto _e = isReadyToMove(); _o->isReadyToMove = _e; }; - { auto _e = isMastered(); _o->isMastered = _e; }; - { auto _e = processData(); if (_e) { _o->processData.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->processData[_i] = std::unique_ptr(_e->Get(_i)->UnPack(_resolver)); } } }; -} - -inline flatbuffers::Offset KUKAiiwaMonitorConfiguration::Pack(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaMonitorConfigurationT* _o, const flatbuffers::rehasher_function_t *_rehasher) { - return CreateKUKAiiwaMonitorConfiguration(_fbb, _o, _rehasher); -} - -inline flatbuffers::Offset CreateKUKAiiwaMonitorConfiguration(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaMonitorConfigurationT *_o, const flatbuffers::rehasher_function_t *_rehasher) { - (void)_rehasher; - (void)_o; - struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const KUKAiiwaMonitorConfigurationT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; - auto _hardwareVersion = _o->hardwareVersion.empty() ? 0 : _fbb.CreateString(_o->hardwareVersion); - auto _torqueSensorLimits = _o->torqueSensorLimits.size() ? _fbb.CreateVector(_o->torqueSensorLimits) : 0; - auto _isReadyToMove = _o->isReadyToMove; - auto _isMastered = _o->isMastered; - auto _processData = _o->processData.size() ? _fbb.CreateVector> (_o->processData.size(), [](size_t i, _VectorArgs *__va) { return CreateProcessData(*__va->__fbb, __va->__o->processData[i].get(), __va->__rehasher); }, &_va ) : 0; - return grl::flatbuffer::CreateKUKAiiwaMonitorConfiguration( - _fbb, - _hardwareVersion, - _torqueSensorLimits, - _isReadyToMove, - _isMastered, - _processData); -} - -inline KUKAiiwaMonitorStateT *KUKAiiwaMonitorState::UnPack(const flatbuffers::resolver_function_t *_resolver) const { - auto _o = new KUKAiiwaMonitorStateT(); - UnPackTo(_o, _resolver); - return _o; -} - -inline void KUKAiiwaMonitorState::UnPackTo(KUKAiiwaMonitorStateT *_o, const flatbuffers::resolver_function_t *_resolver) const { - (void)_o; - (void)_resolver; - { auto _e = measuredState(); if (_e) _o->measuredState = std::unique_ptr(_e->UnPack(_resolver)); }; - { auto _e = cartesianFlangePose(); if (_e) _o->cartesianFlangePose = std::unique_ptr(new Pose(*_e)); }; - { auto _e = jointStateReal(); if (_e) _o->jointStateReal = std::unique_ptr(_e->UnPack(_resolver)); }; - { auto _e = jointStateInterpolated(); if (_e) _o->jointStateInterpolated = std::unique_ptr(_e->UnPack(_resolver)); }; - { auto _e = externalState(); if (_e) _o->externalState = std::unique_ptr(_e->UnPack(_resolver)); }; - { auto _e = operationMode(); _o->operationMode = _e; }; - { auto _e = sessionState(); _o->sessionState = _e; }; - { auto _e = CartesianWrench(); if (_e) _o->CartesianWrench = std::unique_ptr(new Wrench(*_e)); }; -} - -inline flatbuffers::Offset KUKAiiwaMonitorState::Pack(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaMonitorStateT* _o, const flatbuffers::rehasher_function_t *_rehasher) { - return CreateKUKAiiwaMonitorState(_fbb, _o, _rehasher); -} - -inline flatbuffers::Offset CreateKUKAiiwaMonitorState(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaMonitorStateT *_o, const flatbuffers::rehasher_function_t *_rehasher) { - (void)_rehasher; - (void)_o; - struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const KUKAiiwaMonitorStateT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; - auto _measuredState = _o->measuredState ? CreateJointState(_fbb, _o->measuredState.get(), _rehasher) : 0; - auto _cartesianFlangePose = _o->cartesianFlangePose ? _o->cartesianFlangePose.get() : 0; - auto _jointStateReal = _o->jointStateReal ? CreateJointState(_fbb, _o->jointStateReal.get(), _rehasher) : 0; - auto _jointStateInterpolated = _o->jointStateInterpolated ? CreateJointState(_fbb, _o->jointStateInterpolated.get(), _rehasher) : 0; - auto _externalState = _o->externalState ? CreateJointState(_fbb, _o->externalState.get(), _rehasher) : 0; - auto _operationMode = _o->operationMode; - auto _sessionState = _o->sessionState; - auto _CartesianWrench = _o->CartesianWrench ? _o->CartesianWrench.get() : 0; - return grl::flatbuffer::CreateKUKAiiwaMonitorState( - _fbb, - _measuredState, - _cartesianFlangePose, - _jointStateReal, - _jointStateInterpolated, - _externalState, - _operationMode, - _sessionState, - _CartesianWrench); -} - -inline FRITimeStampT *FRITimeStamp::UnPack(const flatbuffers::resolver_function_t *_resolver) const { - auto _o = new FRITimeStampT(); - UnPackTo(_o, _resolver); - return _o; -} - -inline void FRITimeStamp::UnPackTo(FRITimeStampT *_o, const flatbuffers::resolver_function_t *_resolver) const { - (void)_o; - (void)_resolver; - { auto _e = sec(); _o->sec = _e; }; - { auto _e = nanosec(); _o->nanosec = _e; }; -} - -inline flatbuffers::Offset FRITimeStamp::Pack(flatbuffers::FlatBufferBuilder &_fbb, const FRITimeStampT* _o, const flatbuffers::rehasher_function_t *_rehasher) { - return CreateFRITimeStamp(_fbb, _o, _rehasher); -} - -inline flatbuffers::Offset CreateFRITimeStamp(flatbuffers::FlatBufferBuilder &_fbb, const FRITimeStampT *_o, const flatbuffers::rehasher_function_t *_rehasher) { - (void)_rehasher; - (void)_o; - struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const FRITimeStampT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; - auto _sec = _o->sec; - auto _nanosec = _o->nanosec; - return grl::flatbuffer::CreateFRITimeStamp( - _fbb, - _sec, - _nanosec); -} - -inline FRIMessageLogT *FRIMessageLog::UnPack(const flatbuffers::resolver_function_t *_resolver) const { - auto _o = new FRIMessageLogT(); - UnPackTo(_o, _resolver); - return _o; -} - -inline void FRIMessageLog::UnPackTo(FRIMessageLogT *_o, const flatbuffers::resolver_function_t *_resolver) const { - (void)_o; - (void)_resolver; - { auto _e = sessionState(); _o->sessionState = _e; }; - { auto _e = connectionQuality(); _o->connectionQuality = _e; }; - { auto _e = controlMode(); _o->controlMode = _e; }; - { auto _e = messageIdentifier(); _o->messageIdentifier = _e; }; - { auto _e = sequenceCounter(); _o->sequenceCounter = _e; }; - { auto _e = reflectedSequenceCounter(); _o->reflectedSequenceCounter = _e; }; - { auto _e = measuredJointPosition(); if (_e) { _o->measuredJointPosition.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->measuredJointPosition[_i] = _e->Get(_i); } } }; - { auto _e = measuredTorque(); if (_e) { _o->measuredTorque.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->measuredTorque[_i] = _e->Get(_i); } } }; - { auto _e = commandedJointPosition(); if (_e) { _o->commandedJointPosition.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->commandedJointPosition[_i] = _e->Get(_i); } } }; - { auto _e = commandedTorque(); if (_e) { _o->commandedTorque.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->commandedTorque[_i] = _e->Get(_i); } } }; - { auto _e = externalTorque(); if (_e) { _o->externalTorque.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->externalTorque[_i] = _e->Get(_i); } } }; - { auto _e = jointStateInterpolated(); if (_e) { _o->jointStateInterpolated.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->jointStateInterpolated[_i] = _e->Get(_i); } } }; - { auto _e = timeStamp(); if (_e) _o->timeStamp = std::unique_ptr(_e->UnPack(_resolver)); }; -} - -inline flatbuffers::Offset FRIMessageLog::Pack(flatbuffers::FlatBufferBuilder &_fbb, const FRIMessageLogT* _o, const flatbuffers::rehasher_function_t *_rehasher) { - return CreateFRIMessageLog(_fbb, _o, _rehasher); -} - -inline flatbuffers::Offset CreateFRIMessageLog(flatbuffers::FlatBufferBuilder &_fbb, const FRIMessageLogT *_o, const flatbuffers::rehasher_function_t *_rehasher) { - (void)_rehasher; - (void)_o; - struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const FRIMessageLogT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; - auto _sessionState = _o->sessionState; - auto _connectionQuality = _o->connectionQuality; - auto _controlMode = _o->controlMode; - auto _messageIdentifier = _o->messageIdentifier; - auto _sequenceCounter = _o->sequenceCounter; - auto _reflectedSequenceCounter = _o->reflectedSequenceCounter; - auto _measuredJointPosition = _o->measuredJointPosition.size() ? _fbb.CreateVector(_o->measuredJointPosition) : 0; - auto _measuredTorque = _o->measuredTorque.size() ? _fbb.CreateVector(_o->measuredTorque) : 0; - auto _commandedJointPosition = _o->commandedJointPosition.size() ? _fbb.CreateVector(_o->commandedJointPosition) : 0; - auto _commandedTorque = _o->commandedTorque.size() ? _fbb.CreateVector(_o->commandedTorque) : 0; - auto _externalTorque = _o->externalTorque.size() ? _fbb.CreateVector(_o->externalTorque) : 0; - auto _jointStateInterpolated = _o->jointStateInterpolated.size() ? _fbb.CreateVector(_o->jointStateInterpolated) : 0; - auto _timeStamp = _o->timeStamp ? CreateTimeEvent(_fbb, _o->timeStamp.get(), _rehasher) : 0; - return grl::flatbuffer::CreateFRIMessageLog( - _fbb, - _sessionState, - _connectionQuality, - _controlMode, - _messageIdentifier, - _sequenceCounter, - _reflectedSequenceCounter, - _measuredJointPosition, - _measuredTorque, - _commandedJointPosition, - _commandedTorque, - _externalTorque, - _jointStateInterpolated, - _timeStamp); -} - -inline KUKAiiwaStateT *KUKAiiwaState::UnPack(const flatbuffers::resolver_function_t *_resolver) const { - auto _o = new KUKAiiwaStateT(); - UnPackTo(_o, _resolver); - return _o; -} - -inline void KUKAiiwaState::UnPackTo(KUKAiiwaStateT *_o, const flatbuffers::resolver_function_t *_resolver) const { - (void)_o; - (void)_resolver; - { auto _e = name(); if (_e) _o->name = _e->str(); }; - { auto _e = destination(); if (_e) _o->destination = _e->str(); }; - { auto _e = source(); if (_e) _o->source = _e->str(); }; - { auto _e = timeStamp(); if (_e) _o->timeStamp = std::unique_ptr(_e->UnPack(_resolver)); }; - { auto _e = setArmControlState(); _o->setArmControlState = _e; }; - { auto _e = armControlState(); if (_e) _o->armControlState = std::unique_ptr(_e->UnPack(_resolver)); }; - { auto _e = setArmConfiguration(); _o->setArmConfiguration = _e; }; - { auto _e = armConfiguration(); if (_e) _o->armConfiguration = std::unique_ptr(_e->UnPack(_resolver)); }; - { auto _e = hasMonitorState(); _o->hasMonitorState = _e; }; - { auto _e = monitorState(); if (_e) _o->monitorState = std::unique_ptr(_e->UnPack(_resolver)); }; - { auto _e = hasMonitorConfig(); _o->hasMonitorConfig = _e; }; - { auto _e = monitorConfig(); if (_e) _o->monitorConfig = std::unique_ptr(_e->UnPack(_resolver)); }; - { auto _e = FRIMessage(); if (_e) _o->FRIMessage = std::unique_ptr(_e->UnPack(_resolver)); }; -} - -inline flatbuffers::Offset KUKAiiwaState::Pack(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaStateT* _o, const flatbuffers::rehasher_function_t *_rehasher) { - return CreateKUKAiiwaState(_fbb, _o, _rehasher); -} - -inline flatbuffers::Offset CreateKUKAiiwaState(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaStateT *_o, const flatbuffers::rehasher_function_t *_rehasher) { - (void)_rehasher; - (void)_o; - struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const KUKAiiwaStateT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; - auto _name = _o->name.empty() ? 0 : _fbb.CreateString(_o->name); - auto _destination = _o->destination.empty() ? 0 : _fbb.CreateString(_o->destination); - auto _source = _o->source.empty() ? 0 : _fbb.CreateString(_o->source); - auto _timeStamp = _o->timeStamp ? CreateTimeEvent(_fbb, _o->timeStamp.get(), _rehasher) : 0; - auto _setArmControlState = _o->setArmControlState; - auto _armControlState = _o->armControlState ? CreateArmControlState(_fbb, _o->armControlState.get(), _rehasher) : 0; - auto _setArmConfiguration = _o->setArmConfiguration; - auto _armConfiguration = _o->armConfiguration ? CreateKUKAiiwaArmConfiguration(_fbb, _o->armConfiguration.get(), _rehasher) : 0; - auto _hasMonitorState = _o->hasMonitorState; - auto _monitorState = _o->monitorState ? CreateKUKAiiwaMonitorState(_fbb, _o->monitorState.get(), _rehasher) : 0; - auto _hasMonitorConfig = _o->hasMonitorConfig; - auto _monitorConfig = _o->monitorConfig ? CreateKUKAiiwaMonitorConfiguration(_fbb, _o->monitorConfig.get(), _rehasher) : 0; - auto _FRIMessage = _o->FRIMessage ? CreateFRIMessageLog(_fbb, _o->FRIMessage.get(), _rehasher) : 0; - return grl::flatbuffer::CreateKUKAiiwaState( - _fbb, - _name, - _destination, - _source, - _timeStamp, - _setArmControlState, - _armControlState, - _setArmConfiguration, - _armConfiguration, - _hasMonitorState, - _monitorState, - _hasMonitorConfig, - _monitorConfig, - _FRIMessage); -} - -inline KUKAiiwaStatesT *KUKAiiwaStates::UnPack(const flatbuffers::resolver_function_t *_resolver) const { - auto _o = new KUKAiiwaStatesT(); - UnPackTo(_o, _resolver); - return _o; -} - -inline void KUKAiiwaStates::UnPackTo(KUKAiiwaStatesT *_o, const flatbuffers::resolver_function_t *_resolver) const { - (void)_o; - (void)_resolver; - { auto _e = states(); if (_e) { _o->states.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->states[_i] = std::unique_ptr(_e->Get(_i)->UnPack(_resolver)); } } }; -} - -inline flatbuffers::Offset KUKAiiwaStates::Pack(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaStatesT* _o, const flatbuffers::rehasher_function_t *_rehasher) { - return CreateKUKAiiwaStates(_fbb, _o, _rehasher); -} - -inline flatbuffers::Offset CreateKUKAiiwaStates(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaStatesT *_o, const flatbuffers::rehasher_function_t *_rehasher) { - (void)_rehasher; - (void)_o; - struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const KUKAiiwaStatesT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; - auto _states = _o->states.size() ? _fbb.CreateVector> (_o->states.size(), [](size_t i, _VectorArgs *__va) { return CreateKUKAiiwaState(*__va->__fbb, __va->__o->states[i].get(), __va->__rehasher); }, &_va ) : 0; - return grl::flatbuffer::CreateKUKAiiwaStates( - _fbb, - _states); -} - -inline const grl::flatbuffer::KUKAiiwaStates *GetKUKAiiwaStates(const void *buf) { - return flatbuffers::GetRoot(buf); -} - -inline const grl::flatbuffer::KUKAiiwaStates *GetSizePrefixedKUKAiiwaStates(const void *buf) { - return flatbuffers::GetSizePrefixedRoot(buf); -} - -inline const char *KUKAiiwaStatesIdentifier() { - return "iiwa"; -} - -inline bool KUKAiiwaStatesBufferHasIdentifier(const void *buf) { - return flatbuffers::BufferHasIdentifier( - buf, KUKAiiwaStatesIdentifier()); -} - -inline bool VerifyKUKAiiwaStatesBuffer( - flatbuffers::Verifier &verifier) { - return verifier.VerifyBuffer(KUKAiiwaStatesIdentifier()); -} - -inline bool VerifySizePrefixedKUKAiiwaStatesBuffer( - flatbuffers::Verifier &verifier) { - return verifier.VerifySizePrefixedBuffer(KUKAiiwaStatesIdentifier()); -} - -inline const char *KUKAiiwaStatesExtension() { - return "iiwa"; -} - -inline void FinishKUKAiiwaStatesBuffer( - flatbuffers::FlatBufferBuilder &fbb, - flatbuffers::Offset root) { - fbb.Finish(root, KUKAiiwaStatesIdentifier()); -} - -inline void FinishSizePrefixedKUKAiiwaStatesBuffer( - flatbuffers::FlatBufferBuilder &fbb, - flatbuffers::Offset root) { - fbb.FinishSizePrefixed(root, KUKAiiwaStatesIdentifier()); -} - -inline std::unique_ptr UnPackKUKAiiwaStates( - const void *buf, - const flatbuffers::resolver_function_t *res = nullptr) { - return std::unique_ptr(GetKUKAiiwaStates(buf)->UnPack(res)); -} - -} // namespace flatbuffer -} // namespace grl - -#endif // FLATBUFFERS_GENERATED_KUKAIIWA_GRL_FLATBUFFER_H_ diff --git a/include/grl/flatbuffer/LinkObject_generated.h b/include/grl/flatbuffer/LinkObject_generated.h deleted file mode 100644 index 40e5312..0000000 --- a/include/grl/flatbuffer/LinkObject_generated.h +++ /dev/null @@ -1,163 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - - -#ifndef FLATBUFFERS_GENERATED_LINKOBJECT_GRL_FLATBUFFER_H_ -#define FLATBUFFERS_GENERATED_LINKOBJECT_GRL_FLATBUFFER_H_ - -#include "flatbuffers/flatbuffers.h" - -#include "Geometry_generated.h" - -namespace grl { -namespace flatbuffer { - -struct LinkObject; -struct LinkObjectT; - -struct LinkObjectT : public flatbuffers::NativeTable { - typedef LinkObject TableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.LinkObjectT"; - } - std::string name; - std::string parent; - std::unique_ptr pose; - std::unique_ptr inertia; - LinkObjectT() { - } -}; - -struct LinkObject FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { - typedef LinkObjectT NativeTableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.LinkObject"; - } - enum { - VT_NAME = 4, - VT_PARENT = 6, - VT_POSE = 8, - VT_INERTIA = 10 - }; - const flatbuffers::String *name() const { - return GetPointer(VT_NAME); - } - const flatbuffers::String *parent() const { - return GetPointer(VT_PARENT); - } - const Pose *pose() const { - return GetStruct(VT_POSE); - } - const Inertia *inertia() const { - return GetStruct(VT_INERTIA); - } - bool Verify(flatbuffers::Verifier &verifier) const { - return VerifyTableStart(verifier) && - VerifyOffset(verifier, VT_NAME) && - verifier.Verify(name()) && - VerifyOffset(verifier, VT_PARENT) && - verifier.Verify(parent()) && - VerifyField(verifier, VT_POSE) && - VerifyField(verifier, VT_INERTIA) && - verifier.EndTable(); - } - LinkObjectT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; - void UnPackTo(LinkObjectT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; - static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const LinkObjectT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); -}; - -struct LinkObjectBuilder { - flatbuffers::FlatBufferBuilder &fbb_; - flatbuffers::uoffset_t start_; - void add_name(flatbuffers::Offset name) { - fbb_.AddOffset(LinkObject::VT_NAME, name); - } - void add_parent(flatbuffers::Offset parent) { - fbb_.AddOffset(LinkObject::VT_PARENT, parent); - } - void add_pose(const Pose *pose) { - fbb_.AddStruct(LinkObject::VT_POSE, pose); - } - void add_inertia(const Inertia *inertia) { - fbb_.AddStruct(LinkObject::VT_INERTIA, inertia); - } - explicit LinkObjectBuilder(flatbuffers::FlatBufferBuilder &_fbb) - : fbb_(_fbb) { - start_ = fbb_.StartTable(); - } - LinkObjectBuilder &operator=(const LinkObjectBuilder &); - flatbuffers::Offset Finish() { - const auto end = fbb_.EndTable(start_); - auto o = flatbuffers::Offset(end); - return o; - } -}; - -inline flatbuffers::Offset CreateLinkObject( - flatbuffers::FlatBufferBuilder &_fbb, - flatbuffers::Offset name = 0, - flatbuffers::Offset parent = 0, - const Pose *pose = 0, - const Inertia *inertia = 0) { - LinkObjectBuilder builder_(_fbb); - builder_.add_inertia(inertia); - builder_.add_pose(pose); - builder_.add_parent(parent); - builder_.add_name(name); - return builder_.Finish(); -} - -inline flatbuffers::Offset CreateLinkObjectDirect( - flatbuffers::FlatBufferBuilder &_fbb, - const char *name = nullptr, - const char *parent = nullptr, - const Pose *pose = 0, - const Inertia *inertia = 0) { - return grl::flatbuffer::CreateLinkObject( - _fbb, - name ? _fbb.CreateString(name) : 0, - parent ? _fbb.CreateString(parent) : 0, - pose, - inertia); -} - -flatbuffers::Offset CreateLinkObject(flatbuffers::FlatBufferBuilder &_fbb, const LinkObjectT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); - -inline LinkObjectT *LinkObject::UnPack(const flatbuffers::resolver_function_t *_resolver) const { - auto _o = new LinkObjectT(); - UnPackTo(_o, _resolver); - return _o; -} - -inline void LinkObject::UnPackTo(LinkObjectT *_o, const flatbuffers::resolver_function_t *_resolver) const { - (void)_o; - (void)_resolver; - { auto _e = name(); if (_e) _o->name = _e->str(); }; - { auto _e = parent(); if (_e) _o->parent = _e->str(); }; - { auto _e = pose(); if (_e) _o->pose = std::unique_ptr(new Pose(*_e)); }; - { auto _e = inertia(); if (_e) _o->inertia = std::unique_ptr(new Inertia(*_e)); }; -} - -inline flatbuffers::Offset LinkObject::Pack(flatbuffers::FlatBufferBuilder &_fbb, const LinkObjectT* _o, const flatbuffers::rehasher_function_t *_rehasher) { - return CreateLinkObject(_fbb, _o, _rehasher); -} - -inline flatbuffers::Offset CreateLinkObject(flatbuffers::FlatBufferBuilder &_fbb, const LinkObjectT *_o, const flatbuffers::rehasher_function_t *_rehasher) { - (void)_rehasher; - (void)_o; - struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const LinkObjectT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; - auto _name = _o->name.empty() ? 0 : _fbb.CreateString(_o->name); - auto _parent = _o->parent.empty() ? 0 : _fbb.CreateString(_o->parent); - auto _pose = _o->pose ? _o->pose.get() : 0; - auto _inertia = _o->inertia ? _o->inertia.get() : 0; - return grl::flatbuffer::CreateLinkObject( - _fbb, - _name, - _parent, - _pose, - _inertia); -} - -} // namespace flatbuffer -} // namespace grl - -#endif // FLATBUFFERS_GENERATED_LINKOBJECT_GRL_FLATBUFFER_H_ diff --git a/include/grl/flatbuffer/LogKUKAiiwaFusionTrack_generated.h b/include/grl/flatbuffer/LogKUKAiiwaFusionTrack_generated.h deleted file mode 100644 index ab2cf92..0000000 --- a/include/grl/flatbuffer/LogKUKAiiwaFusionTrack_generated.h +++ /dev/null @@ -1,504 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - - -#ifndef FLATBUFFERS_GENERATED_LOGKUKAIIWAFUSIONTRACK_GRL_FLATBUFFER_H_ -#define FLATBUFFERS_GENERATED_LOGKUKAIIWAFUSIONTRACK_GRL_FLATBUFFER_H_ - -#include "flatbuffers/flatbuffers.h" - -#include "ArmControlState_generated.h" -#include "Euler_generated.h" -#include "FusionTrack_generated.h" -#include "Geometry_generated.h" -#include "JointState_generated.h" -#include "KUKAiiwa_generated.h" -#include "LinkObject_generated.h" -#include "Time_generated.h" - -namespace grl { -namespace flatbuffer { - -struct KUKAiiwaFusionTrackMessage; -struct KUKAiiwaFusionTrackMessageT; - -struct LogKUKAiiwaFusionTrack; -struct LogKUKAiiwaFusionTrackT; - -enum class DeviceState : uint8_t { - NONE = 0, - KUKAiiwaState = 1, - FusionTrackMessage = 2, - MIN = NONE, - MAX = FusionTrackMessage -}; - -inline const DeviceState (&EnumValuesDeviceState())[3] { - static const DeviceState values[] = { - DeviceState::NONE, - DeviceState::KUKAiiwaState, - DeviceState::FusionTrackMessage - }; - return values; -} - -inline const char * const *EnumNamesDeviceState() { - static const char * const names[] = { - "NONE", - "KUKAiiwaState", - "FusionTrackMessage", - nullptr - }; - return names; -} - -inline const char *EnumNameDeviceState(DeviceState e) { - const size_t index = static_cast(e); - return EnumNamesDeviceState()[index]; -} - -template struct DeviceStateTraits { - static const DeviceState enum_value = DeviceState::NONE; -}; - -template<> struct DeviceStateTraits { - static const DeviceState enum_value = DeviceState::KUKAiiwaState; -}; - -template<> struct DeviceStateTraits { - static const DeviceState enum_value = DeviceState::FusionTrackMessage; -}; - -struct DeviceStateUnion { - DeviceState type; - void *value; - - DeviceStateUnion() : type(DeviceState::NONE), value(nullptr) {} - DeviceStateUnion(DeviceStateUnion&& u) FLATBUFFERS_NOEXCEPT : - type(DeviceState::NONE), value(nullptr) - { std::swap(type, u.type); std::swap(value, u.value); } - DeviceStateUnion(const DeviceStateUnion &) FLATBUFFERS_NOEXCEPT; - DeviceStateUnion &operator=(const DeviceStateUnion &u) FLATBUFFERS_NOEXCEPT - { DeviceStateUnion t(u); std::swap(type, t.type); std::swap(value, t.value); return *this; } - DeviceStateUnion &operator=(DeviceStateUnion &&u) FLATBUFFERS_NOEXCEPT - { std::swap(type, u.type); std::swap(value, u.value); return *this; } - ~DeviceStateUnion() { Reset(); } - - void Reset(); - -#ifndef FLATBUFFERS_CPP98_STL - template - void Set(T&& val) { - Reset(); - type = DeviceStateTraits::enum_value; - if (type != DeviceState::NONE) { - value = new T(std::forward(val)); - } - } -#endif // FLATBUFFERS_CPP98_STL - - static void *UnPack(const void *obj, DeviceState type, const flatbuffers::resolver_function_t *resolver); - flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const flatbuffers::rehasher_function_t *_rehasher = nullptr) const; - - KUKAiiwaStateT *AsKUKAiiwaState() { - return type == DeviceState::KUKAiiwaState ? - reinterpret_cast(value) : nullptr; - } - const KUKAiiwaStateT *AsKUKAiiwaState() const { - return type == DeviceState::KUKAiiwaState ? - reinterpret_cast(value) : nullptr; - } - FusionTrackMessageT *AsFusionTrackMessage() { - return type == DeviceState::FusionTrackMessage ? - reinterpret_cast(value) : nullptr; - } - const FusionTrackMessageT *AsFusionTrackMessage() const { - return type == DeviceState::FusionTrackMessage ? - reinterpret_cast(value) : nullptr; - } -}; - -bool VerifyDeviceState(flatbuffers::Verifier &verifier, const void *obj, DeviceState type); -bool VerifyDeviceStateVector(flatbuffers::Verifier &verifier, const flatbuffers::Vector> *values, const flatbuffers::Vector *types); - -struct KUKAiiwaFusionTrackMessageT : public flatbuffers::NativeTable { - typedef KUKAiiwaFusionTrackMessage TableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.KUKAiiwaFusionTrackMessageT"; - } - double timestamp; - std::unique_ptr timeEvent; - DeviceStateUnion deviceState; - KUKAiiwaFusionTrackMessageT() - : timestamp(0.0) { - } -}; - -struct KUKAiiwaFusionTrackMessage FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { - typedef KUKAiiwaFusionTrackMessageT NativeTableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.KUKAiiwaFusionTrackMessage"; - } - enum { - VT_TIMESTAMP = 4, - VT_TIMEEVENT = 6, - VT_DEVICESTATE_TYPE = 8, - VT_DEVICESTATE = 10 - }; - double timestamp() const { - return GetField(VT_TIMESTAMP, 0.0); - } - const TimeEvent *timeEvent() const { - return GetPointer(VT_TIMEEVENT); - } - DeviceState deviceState_type() const { - return static_cast(GetField(VT_DEVICESTATE_TYPE, 0)); - } - const void *deviceState() const { - return GetPointer(VT_DEVICESTATE); - } - template const T *deviceState_as() const; - const KUKAiiwaState *deviceState_as_KUKAiiwaState() const { - return deviceState_type() == DeviceState::KUKAiiwaState ? static_cast(deviceState()) : nullptr; - } - const FusionTrackMessage *deviceState_as_FusionTrackMessage() const { - return deviceState_type() == DeviceState::FusionTrackMessage ? static_cast(deviceState()) : nullptr; - } - bool Verify(flatbuffers::Verifier &verifier) const { - return VerifyTableStart(verifier) && - VerifyField(verifier, VT_TIMESTAMP) && - VerifyOffset(verifier, VT_TIMEEVENT) && - verifier.VerifyTable(timeEvent()) && - VerifyField(verifier, VT_DEVICESTATE_TYPE) && - VerifyOffset(verifier, VT_DEVICESTATE) && - VerifyDeviceState(verifier, deviceState(), deviceState_type()) && - verifier.EndTable(); - } - KUKAiiwaFusionTrackMessageT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; - void UnPackTo(KUKAiiwaFusionTrackMessageT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; - static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaFusionTrackMessageT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); -}; - -template<> inline const KUKAiiwaState *KUKAiiwaFusionTrackMessage::deviceState_as() const { - return deviceState_as_KUKAiiwaState(); -} - -template<> inline const FusionTrackMessage *KUKAiiwaFusionTrackMessage::deviceState_as() const { - return deviceState_as_FusionTrackMessage(); -} - -struct KUKAiiwaFusionTrackMessageBuilder { - flatbuffers::FlatBufferBuilder &fbb_; - flatbuffers::uoffset_t start_; - void add_timestamp(double timestamp) { - fbb_.AddElement(KUKAiiwaFusionTrackMessage::VT_TIMESTAMP, timestamp, 0.0); - } - void add_timeEvent(flatbuffers::Offset timeEvent) { - fbb_.AddOffset(KUKAiiwaFusionTrackMessage::VT_TIMEEVENT, timeEvent); - } - void add_deviceState_type(DeviceState deviceState_type) { - fbb_.AddElement(KUKAiiwaFusionTrackMessage::VT_DEVICESTATE_TYPE, static_cast(deviceState_type), 0); - } - void add_deviceState(flatbuffers::Offset deviceState) { - fbb_.AddOffset(KUKAiiwaFusionTrackMessage::VT_DEVICESTATE, deviceState); - } - explicit KUKAiiwaFusionTrackMessageBuilder(flatbuffers::FlatBufferBuilder &_fbb) - : fbb_(_fbb) { - start_ = fbb_.StartTable(); - } - KUKAiiwaFusionTrackMessageBuilder &operator=(const KUKAiiwaFusionTrackMessageBuilder &); - flatbuffers::Offset Finish() { - const auto end = fbb_.EndTable(start_); - auto o = flatbuffers::Offset(end); - return o; - } -}; - -inline flatbuffers::Offset CreateKUKAiiwaFusionTrackMessage( - flatbuffers::FlatBufferBuilder &_fbb, - double timestamp = 0.0, - flatbuffers::Offset timeEvent = 0, - DeviceState deviceState_type = DeviceState::NONE, - flatbuffers::Offset deviceState = 0) { - KUKAiiwaFusionTrackMessageBuilder builder_(_fbb); - builder_.add_timestamp(timestamp); - builder_.add_deviceState(deviceState); - builder_.add_timeEvent(timeEvent); - builder_.add_deviceState_type(deviceState_type); - return builder_.Finish(); -} - -flatbuffers::Offset CreateKUKAiiwaFusionTrackMessage(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaFusionTrackMessageT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); - -struct LogKUKAiiwaFusionTrackT : public flatbuffers::NativeTable { - typedef LogKUKAiiwaFusionTrack TableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.LogKUKAiiwaFusionTrackT"; - } - std::vector> states; - LogKUKAiiwaFusionTrackT() { - } -}; - -struct LogKUKAiiwaFusionTrack FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { - typedef LogKUKAiiwaFusionTrackT NativeTableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.LogKUKAiiwaFusionTrack"; - } - enum { - VT_STATES = 4 - }; - const flatbuffers::Vector> *states() const { - return GetPointer> *>(VT_STATES); - } - bool Verify(flatbuffers::Verifier &verifier) const { - return VerifyTableStart(verifier) && - VerifyOffset(verifier, VT_STATES) && - verifier.Verify(states()) && - verifier.VerifyVectorOfTables(states()) && - verifier.EndTable(); - } - LogKUKAiiwaFusionTrackT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; - void UnPackTo(LogKUKAiiwaFusionTrackT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; - static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const LogKUKAiiwaFusionTrackT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); -}; - -struct LogKUKAiiwaFusionTrackBuilder { - flatbuffers::FlatBufferBuilder &fbb_; - flatbuffers::uoffset_t start_; - void add_states(flatbuffers::Offset>> states) { - fbb_.AddOffset(LogKUKAiiwaFusionTrack::VT_STATES, states); - } - explicit LogKUKAiiwaFusionTrackBuilder(flatbuffers::FlatBufferBuilder &_fbb) - : fbb_(_fbb) { - start_ = fbb_.StartTable(); - } - LogKUKAiiwaFusionTrackBuilder &operator=(const LogKUKAiiwaFusionTrackBuilder &); - flatbuffers::Offset Finish() { - const auto end = fbb_.EndTable(start_); - auto o = flatbuffers::Offset(end); - return o; - } -}; - -inline flatbuffers::Offset CreateLogKUKAiiwaFusionTrack( - flatbuffers::FlatBufferBuilder &_fbb, - flatbuffers::Offset>> states = 0) { - LogKUKAiiwaFusionTrackBuilder builder_(_fbb); - builder_.add_states(states); - return builder_.Finish(); -} - -inline flatbuffers::Offset CreateLogKUKAiiwaFusionTrackDirect( - flatbuffers::FlatBufferBuilder &_fbb, - const std::vector> *states = nullptr) { - return grl::flatbuffer::CreateLogKUKAiiwaFusionTrack( - _fbb, - states ? _fbb.CreateVector>(*states) : 0); -} - -flatbuffers::Offset CreateLogKUKAiiwaFusionTrack(flatbuffers::FlatBufferBuilder &_fbb, const LogKUKAiiwaFusionTrackT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); - -inline KUKAiiwaFusionTrackMessageT *KUKAiiwaFusionTrackMessage::UnPack(const flatbuffers::resolver_function_t *_resolver) const { - auto _o = new KUKAiiwaFusionTrackMessageT(); - UnPackTo(_o, _resolver); - return _o; -} - -inline void KUKAiiwaFusionTrackMessage::UnPackTo(KUKAiiwaFusionTrackMessageT *_o, const flatbuffers::resolver_function_t *_resolver) const { - (void)_o; - (void)_resolver; - { auto _e = timestamp(); _o->timestamp = _e; }; - { auto _e = timeEvent(); if (_e) _o->timeEvent = std::unique_ptr(_e->UnPack(_resolver)); }; - { auto _e = deviceState_type(); _o->deviceState.type = _e; }; - { auto _e = deviceState(); if (_e) _o->deviceState.value = DeviceStateUnion::UnPack(_e, deviceState_type(), _resolver); }; -} - -inline flatbuffers::Offset KUKAiiwaFusionTrackMessage::Pack(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaFusionTrackMessageT* _o, const flatbuffers::rehasher_function_t *_rehasher) { - return CreateKUKAiiwaFusionTrackMessage(_fbb, _o, _rehasher); -} - -inline flatbuffers::Offset CreateKUKAiiwaFusionTrackMessage(flatbuffers::FlatBufferBuilder &_fbb, const KUKAiiwaFusionTrackMessageT *_o, const flatbuffers::rehasher_function_t *_rehasher) { - (void)_rehasher; - (void)_o; - struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const KUKAiiwaFusionTrackMessageT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; - auto _timestamp = _o->timestamp; - auto _timeEvent = _o->timeEvent ? CreateTimeEvent(_fbb, _o->timeEvent.get(), _rehasher) : 0; - auto _deviceState_type = _o->deviceState.type; - auto _deviceState = _o->deviceState.Pack(_fbb); - return grl::flatbuffer::CreateKUKAiiwaFusionTrackMessage( - _fbb, - _timestamp, - _timeEvent, - _deviceState_type, - _deviceState); -} - -inline LogKUKAiiwaFusionTrackT *LogKUKAiiwaFusionTrack::UnPack(const flatbuffers::resolver_function_t *_resolver) const { - auto _o = new LogKUKAiiwaFusionTrackT(); - UnPackTo(_o, _resolver); - return _o; -} - -inline void LogKUKAiiwaFusionTrack::UnPackTo(LogKUKAiiwaFusionTrackT *_o, const flatbuffers::resolver_function_t *_resolver) const { - (void)_o; - (void)_resolver; - { auto _e = states(); if (_e) { _o->states.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->states[_i] = std::unique_ptr(_e->Get(_i)->UnPack(_resolver)); } } }; -} - -inline flatbuffers::Offset LogKUKAiiwaFusionTrack::Pack(flatbuffers::FlatBufferBuilder &_fbb, const LogKUKAiiwaFusionTrackT* _o, const flatbuffers::rehasher_function_t *_rehasher) { - return CreateLogKUKAiiwaFusionTrack(_fbb, _o, _rehasher); -} - -inline flatbuffers::Offset CreateLogKUKAiiwaFusionTrack(flatbuffers::FlatBufferBuilder &_fbb, const LogKUKAiiwaFusionTrackT *_o, const flatbuffers::rehasher_function_t *_rehasher) { - (void)_rehasher; - (void)_o; - struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const LogKUKAiiwaFusionTrackT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; - auto _states = _o->states.size() ? _fbb.CreateVector> (_o->states.size(), [](size_t i, _VectorArgs *__va) { return CreateKUKAiiwaFusionTrackMessage(*__va->__fbb, __va->__o->states[i].get(), __va->__rehasher); }, &_va ) : 0; - return grl::flatbuffer::CreateLogKUKAiiwaFusionTrack( - _fbb, - _states); -} - -inline bool VerifyDeviceState(flatbuffers::Verifier &verifier, const void *obj, DeviceState type) { - switch (type) { - case DeviceState::NONE: { - return true; - } - case DeviceState::KUKAiiwaState: { - auto ptr = reinterpret_cast(obj); - return verifier.VerifyTable(ptr); - } - case DeviceState::FusionTrackMessage: { - auto ptr = reinterpret_cast(obj); - return verifier.VerifyTable(ptr); - } - default: return false; - } -} - -inline bool VerifyDeviceStateVector(flatbuffers::Verifier &verifier, const flatbuffers::Vector> *values, const flatbuffers::Vector *types) { - if (!values || !types) return !values && !types; - if (values->size() != types->size()) return false; - for (flatbuffers::uoffset_t i = 0; i < values->size(); ++i) { - if (!VerifyDeviceState( - verifier, values->Get(i), types->GetEnum(i))) { - return false; - } - } - return true; -} - -inline void *DeviceStateUnion::UnPack(const void *obj, DeviceState type, const flatbuffers::resolver_function_t *resolver) { - switch (type) { - case DeviceState::KUKAiiwaState: { - auto ptr = reinterpret_cast(obj); - return ptr->UnPack(resolver); - } - case DeviceState::FusionTrackMessage: { - auto ptr = reinterpret_cast(obj); - return ptr->UnPack(resolver); - } - default: return nullptr; - } -} - -inline flatbuffers::Offset DeviceStateUnion::Pack(flatbuffers::FlatBufferBuilder &_fbb, const flatbuffers::rehasher_function_t *_rehasher) const { - switch (type) { - case DeviceState::KUKAiiwaState: { - auto ptr = reinterpret_cast(value); - return CreateKUKAiiwaState(_fbb, ptr, _rehasher).Union(); - } - case DeviceState::FusionTrackMessage: { - auto ptr = reinterpret_cast(value); - return CreateFusionTrackMessage(_fbb, ptr, _rehasher).Union(); - } - default: return 0; - } -} - -inline DeviceStateUnion::DeviceStateUnion(const DeviceStateUnion &u) FLATBUFFERS_NOEXCEPT : type(u.type), value(nullptr) { - switch (type) { - case DeviceState::KUKAiiwaState: { - assert(false); // KUKAiiwaStateT not copyable. - break; - } - case DeviceState::FusionTrackMessage: { - assert(false); // FusionTrackMessageT not copyable. - break; - } - default: - break; - } -} - -inline void DeviceStateUnion::Reset() { - switch (type) { - case DeviceState::KUKAiiwaState: { - auto ptr = reinterpret_cast(value); - delete ptr; - break; - } - case DeviceState::FusionTrackMessage: { - auto ptr = reinterpret_cast(value); - delete ptr; - break; - } - default: break; - } - value = nullptr; - type = DeviceState::NONE; -} - -inline const grl::flatbuffer::LogKUKAiiwaFusionTrack *GetLogKUKAiiwaFusionTrack(const void *buf) { - return flatbuffers::GetRoot(buf); -} - -inline const grl::flatbuffer::LogKUKAiiwaFusionTrack *GetSizePrefixedLogKUKAiiwaFusionTrack(const void *buf) { - return flatbuffers::GetSizePrefixedRoot(buf); -} - -inline const char *LogKUKAiiwaFusionTrackIdentifier() { - return "flik"; -} - -inline bool LogKUKAiiwaFusionTrackBufferHasIdentifier(const void *buf) { - return flatbuffers::BufferHasIdentifier( - buf, LogKUKAiiwaFusionTrackIdentifier()); -} - -inline bool VerifyLogKUKAiiwaFusionTrackBuffer( - flatbuffers::Verifier &verifier) { - return verifier.VerifyBuffer(LogKUKAiiwaFusionTrackIdentifier()); -} - -inline bool VerifySizePrefixedLogKUKAiiwaFusionTrackBuffer( - flatbuffers::Verifier &verifier) { - return verifier.VerifySizePrefixedBuffer(LogKUKAiiwaFusionTrackIdentifier()); -} - -inline const char *LogKUKAiiwaFusionTrackExtension() { - return "flik"; -} - -inline void FinishLogKUKAiiwaFusionTrackBuffer( - flatbuffers::FlatBufferBuilder &fbb, - flatbuffers::Offset root) { - fbb.Finish(root, LogKUKAiiwaFusionTrackIdentifier()); -} - -inline void FinishSizePrefixedLogKUKAiiwaFusionTrackBuffer( - flatbuffers::FlatBufferBuilder &fbb, - flatbuffers::Offset root) { - fbb.FinishSizePrefixed(root, LogKUKAiiwaFusionTrackIdentifier()); -} - -inline std::unique_ptr UnPackLogKUKAiiwaFusionTrack( - const void *buf, - const flatbuffers::resolver_function_t *res = nullptr) { - return std::unique_ptr(GetLogKUKAiiwaFusionTrack(buf)->UnPack(res)); -} - -} // namespace flatbuffer -} // namespace grl - -#endif // FLATBUFFERS_GENERATED_LOGKUKAIIWAFUSIONTRACK_GRL_FLATBUFFER_H_ diff --git a/include/grl/flatbuffer/Time_generated.h b/include/grl/flatbuffer/Time_generated.h deleted file mode 100644 index 16fe1c8..0000000 --- a/include/grl/flatbuffer/Time_generated.h +++ /dev/null @@ -1,291 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - - -#ifndef FLATBUFFERS_GENERATED_TIME_GRL_FLATBUFFER_H_ -#define FLATBUFFERS_GENERATED_TIME_GRL_FLATBUFFER_H_ - -#include "flatbuffers/flatbuffers.h" - -namespace grl { -namespace flatbuffer { - -struct Time; - -struct TimeEvent; -struct TimeEventT; - -FLATBUFFERS_MANUALLY_ALIGNED_STRUCT(8) Time FLATBUFFERS_FINAL_CLASS { - private: - double sec_; - - public: - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.Time"; - } - Time() { - memset(this, 0, sizeof(Time)); - } - Time(double _sec) - : sec_(flatbuffers::EndianScalar(_sec)) { - } - double sec() const { - return flatbuffers::EndianScalar(sec_); - } -}; -FLATBUFFERS_STRUCT_END(Time, 8); - -struct TimeEventT : public flatbuffers::NativeTable { - typedef TimeEvent TableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.TimeEventT"; - } - std::string event_name; - int64_t local_request_time; - std::string device_clock_id; - int64_t device_time; - std::string local_clock_id; - int64_t local_receive_time; - int64_t corrected_local_time; - int64_t clock_skew; - int64_t min_transport_delay; - TimeEventT() - : local_request_time(0), - device_time(0), - local_receive_time(0), - corrected_local_time(0), - clock_skew(0), - min_transport_delay(0) { - } -}; - -/// Note that all of these time entries are -/// longs with a minimum step of 100ns, -/// see google cartographer's cartographer::common::time -struct TimeEvent FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { - typedef TimeEventT NativeTableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.TimeEvent"; - } - enum { - VT_EVENT_NAME = 4, - VT_LOCAL_REQUEST_TIME = 6, - VT_DEVICE_CLOCK_ID = 8, - VT_DEVICE_TIME = 10, - VT_LOCAL_CLOCK_ID = 12, - VT_LOCAL_RECEIVE_TIME = 14, - VT_CORRECTED_LOCAL_TIME = 16, - VT_CLOCK_SKEW = 18, - VT_MIN_TRANSPORT_DELAY = 20 - }; - /// Identifying string for this time stamped data topic - /// something like "/opticaltracker/00000000/frame" where - /// 00000000 is the serial number of the optical tracker. - const flatbuffers::String *event_name() const { - return GetPointer(VT_EVENT_NAME); - } - /// The time just before a data update request is made - int64_t local_request_time() const { - return GetField(VT_LOCAL_REQUEST_TIME, 0); - } - /// Identifying string for the clock used to drive the device - /// something like "/opticaltracker/00000000/clock" - /// if it is the clock internal to a sensor like an optical tracker - const flatbuffers::String *device_clock_id() const { - return GetPointer(VT_DEVICE_CLOCK_ID); - } - /// The time provided by the device specified by device_clock_id - int64_t device_time() const { - return GetField(VT_DEVICE_TIME, 0); - } - /// Identifying string for the clock used to drive the device - /// or "/control_computer/clock/steady" if the device has no clock - /// and the time is the desktop computer - /// running the steady clock (vs clocks which might change time) - const flatbuffers::String *local_clock_id() const { - return GetPointer(VT_LOCAL_CLOCK_ID); - } - /// The time at which the data was received - int64_t local_receive_time() const { - return GetField(VT_LOCAL_RECEIVE_TIME, 0); - } - /// The corrected local time which represents when the sensor - /// data was actually captured. - int64_t corrected_local_time() const { - return GetField(VT_CORRECTED_LOCAL_TIME, 0); - } - /// Estimated duration of the skew between the device clock - /// and the local time clock - int64_t clock_skew() const { - return GetField(VT_CLOCK_SKEW, 0); - } - /// The minimum expected delay in transporting the data request - int64_t min_transport_delay() const { - return GetField(VT_MIN_TRANSPORT_DELAY, 0); - } - bool Verify(flatbuffers::Verifier &verifier) const { - return VerifyTableStart(verifier) && - VerifyOffset(verifier, VT_EVENT_NAME) && - verifier.Verify(event_name()) && - VerifyField(verifier, VT_LOCAL_REQUEST_TIME) && - VerifyOffset(verifier, VT_DEVICE_CLOCK_ID) && - verifier.Verify(device_clock_id()) && - VerifyField(verifier, VT_DEVICE_TIME) && - VerifyOffset(verifier, VT_LOCAL_CLOCK_ID) && - verifier.Verify(local_clock_id()) && - VerifyField(verifier, VT_LOCAL_RECEIVE_TIME) && - VerifyField(verifier, VT_CORRECTED_LOCAL_TIME) && - VerifyField(verifier, VT_CLOCK_SKEW) && - VerifyField(verifier, VT_MIN_TRANSPORT_DELAY) && - verifier.EndTable(); - } - TimeEventT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; - void UnPackTo(TimeEventT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; - static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const TimeEventT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); -}; - -struct TimeEventBuilder { - flatbuffers::FlatBufferBuilder &fbb_; - flatbuffers::uoffset_t start_; - void add_event_name(flatbuffers::Offset event_name) { - fbb_.AddOffset(TimeEvent::VT_EVENT_NAME, event_name); - } - void add_local_request_time(int64_t local_request_time) { - fbb_.AddElement(TimeEvent::VT_LOCAL_REQUEST_TIME, local_request_time, 0); - } - void add_device_clock_id(flatbuffers::Offset device_clock_id) { - fbb_.AddOffset(TimeEvent::VT_DEVICE_CLOCK_ID, device_clock_id); - } - void add_device_time(int64_t device_time) { - fbb_.AddElement(TimeEvent::VT_DEVICE_TIME, device_time, 0); - } - void add_local_clock_id(flatbuffers::Offset local_clock_id) { - fbb_.AddOffset(TimeEvent::VT_LOCAL_CLOCK_ID, local_clock_id); - } - void add_local_receive_time(int64_t local_receive_time) { - fbb_.AddElement(TimeEvent::VT_LOCAL_RECEIVE_TIME, local_receive_time, 0); - } - void add_corrected_local_time(int64_t corrected_local_time) { - fbb_.AddElement(TimeEvent::VT_CORRECTED_LOCAL_TIME, corrected_local_time, 0); - } - void add_clock_skew(int64_t clock_skew) { - fbb_.AddElement(TimeEvent::VT_CLOCK_SKEW, clock_skew, 0); - } - void add_min_transport_delay(int64_t min_transport_delay) { - fbb_.AddElement(TimeEvent::VT_MIN_TRANSPORT_DELAY, min_transport_delay, 0); - } - explicit TimeEventBuilder(flatbuffers::FlatBufferBuilder &_fbb) - : fbb_(_fbb) { - start_ = fbb_.StartTable(); - } - TimeEventBuilder &operator=(const TimeEventBuilder &); - flatbuffers::Offset Finish() { - const auto end = fbb_.EndTable(start_); - auto o = flatbuffers::Offset(end); - return o; - } -}; - -inline flatbuffers::Offset CreateTimeEvent( - flatbuffers::FlatBufferBuilder &_fbb, - flatbuffers::Offset event_name = 0, - int64_t local_request_time = 0, - flatbuffers::Offset device_clock_id = 0, - int64_t device_time = 0, - flatbuffers::Offset local_clock_id = 0, - int64_t local_receive_time = 0, - int64_t corrected_local_time = 0, - int64_t clock_skew = 0, - int64_t min_transport_delay = 0) { - TimeEventBuilder builder_(_fbb); - builder_.add_min_transport_delay(min_transport_delay); - builder_.add_clock_skew(clock_skew); - builder_.add_corrected_local_time(corrected_local_time); - builder_.add_local_receive_time(local_receive_time); - builder_.add_device_time(device_time); - builder_.add_local_request_time(local_request_time); - builder_.add_local_clock_id(local_clock_id); - builder_.add_device_clock_id(device_clock_id); - builder_.add_event_name(event_name); - return builder_.Finish(); -} - -inline flatbuffers::Offset CreateTimeEventDirect( - flatbuffers::FlatBufferBuilder &_fbb, - const char *event_name = nullptr, - int64_t local_request_time = 0, - const char *device_clock_id = nullptr, - int64_t device_time = 0, - const char *local_clock_id = nullptr, - int64_t local_receive_time = 0, - int64_t corrected_local_time = 0, - int64_t clock_skew = 0, - int64_t min_transport_delay = 0) { - return grl::flatbuffer::CreateTimeEvent( - _fbb, - event_name ? _fbb.CreateString(event_name) : 0, - local_request_time, - device_clock_id ? _fbb.CreateString(device_clock_id) : 0, - device_time, - local_clock_id ? _fbb.CreateString(local_clock_id) : 0, - local_receive_time, - corrected_local_time, - clock_skew, - min_transport_delay); -} - -flatbuffers::Offset CreateTimeEvent(flatbuffers::FlatBufferBuilder &_fbb, const TimeEventT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); - -inline TimeEventT *TimeEvent::UnPack(const flatbuffers::resolver_function_t *_resolver) const { - auto _o = new TimeEventT(); - UnPackTo(_o, _resolver); - return _o; -} - -inline void TimeEvent::UnPackTo(TimeEventT *_o, const flatbuffers::resolver_function_t *_resolver) const { - (void)_o; - (void)_resolver; - { auto _e = event_name(); if (_e) _o->event_name = _e->str(); }; - { auto _e = local_request_time(); _o->local_request_time = _e; }; - { auto _e = device_clock_id(); if (_e) _o->device_clock_id = _e->str(); }; - { auto _e = device_time(); _o->device_time = _e; }; - { auto _e = local_clock_id(); if (_e) _o->local_clock_id = _e->str(); }; - { auto _e = local_receive_time(); _o->local_receive_time = _e; }; - { auto _e = corrected_local_time(); _o->corrected_local_time = _e; }; - { auto _e = clock_skew(); _o->clock_skew = _e; }; - { auto _e = min_transport_delay(); _o->min_transport_delay = _e; }; -} - -inline flatbuffers::Offset TimeEvent::Pack(flatbuffers::FlatBufferBuilder &_fbb, const TimeEventT* _o, const flatbuffers::rehasher_function_t *_rehasher) { - return CreateTimeEvent(_fbb, _o, _rehasher); -} - -inline flatbuffers::Offset CreateTimeEvent(flatbuffers::FlatBufferBuilder &_fbb, const TimeEventT *_o, const flatbuffers::rehasher_function_t *_rehasher) { - (void)_rehasher; - (void)_o; - struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const TimeEventT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; - auto _event_name = _o->event_name.empty() ? 0 : _fbb.CreateString(_o->event_name); - auto _local_request_time = _o->local_request_time; - auto _device_clock_id = _o->device_clock_id.empty() ? 0 : _fbb.CreateString(_o->device_clock_id); - auto _device_time = _o->device_time; - auto _local_clock_id = _o->local_clock_id.empty() ? 0 : _fbb.CreateString(_o->local_clock_id); - auto _local_receive_time = _o->local_receive_time; - auto _corrected_local_time = _o->corrected_local_time; - auto _clock_skew = _o->clock_skew; - auto _min_transport_delay = _o->min_transport_delay; - return grl::flatbuffer::CreateTimeEvent( - _fbb, - _event_name, - _local_request_time, - _device_clock_id, - _device_time, - _local_clock_id, - _local_receive_time, - _corrected_local_time, - _clock_skew, - _min_transport_delay); -} - -} // namespace flatbuffer -} // namespace grl - -#endif // FLATBUFFERS_GENERATED_TIME_GRL_FLATBUFFER_H_ diff --git a/include/grl/flatbuffer/VrepControlPoint_generated.h b/include/grl/flatbuffer/VrepControlPoint_generated.h deleted file mode 100644 index 5582206..0000000 --- a/include/grl/flatbuffer/VrepControlPoint_generated.h +++ /dev/null @@ -1,306 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - - -#ifndef FLATBUFFERS_GENERATED_VREPCONTROLPOINT_GRL_FLATBUFFER_H_ -#define FLATBUFFERS_GENERATED_VREPCONTROLPOINT_GRL_FLATBUFFER_H_ - -#include "flatbuffers/flatbuffers.h" - -#include "Euler_generated.h" -#include "Geometry_generated.h" - -namespace grl { -namespace flatbuffer { - -struct VrepControlPoint; -struct VrepControlPointT; - -struct VrepControlPointT : public flatbuffers::NativeTable { - typedef VrepControlPoint TableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.VrepControlPointT"; - } - std::unique_ptr position; - std::unique_ptr rotation; - double relativeVelocity; - int32_t bezierPointCount; - double interpolationFactor1; - double interpolationFactor2; - double virtualDistance; - int32_t auxiliaryFlags; - double auxiliaryChannel1; - double auxiliaryChannel2; - double auxiliaryChannel3; - double auxiliaryChannel4; - VrepControlPointT() - : relativeVelocity(1.0), - bezierPointCount(1), - interpolationFactor1(0.5), - interpolationFactor2(0.5), - virtualDistance(0.0), - auxiliaryFlags(0), - auxiliaryChannel1(0.0), - auxiliaryChannel2(0.0), - auxiliaryChannel3(0.0), - auxiliaryChannel4(0.0) { - } -}; - -struct VrepControlPoint FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { - typedef VrepControlPointT NativeTableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.VrepControlPoint"; - } - enum { - VT_POSITION = 4, - VT_ROTATION = 6, - VT_RELATIVEVELOCITY = 8, - VT_BEZIERPOINTCOUNT = 10, - VT_INTERPOLATIONFACTOR1 = 12, - VT_INTERPOLATIONFACTOR2 = 14, - VT_VIRTUALDISTANCE = 16, - VT_AUXILIARYFLAGS = 18, - VT_AUXILIARYCHANNEL1 = 20, - VT_AUXILIARYCHANNEL2 = 22, - VT_AUXILIARYCHANNEL3 = 24, - VT_AUXILIARYCHANNEL4 = 26 - }; - const Vector3d *position() const { - return GetStruct(VT_POSITION); - } - const EulerXYZd *rotation() const { - return GetStruct(VT_ROTATION); - } - double relativeVelocity() const { - return GetField(VT_RELATIVEVELOCITY, 1.0); - } - int32_t bezierPointCount() const { - return GetField(VT_BEZIERPOINTCOUNT, 1); - } - double interpolationFactor1() const { - return GetField(VT_INTERPOLATIONFACTOR1, 0.5); - } - double interpolationFactor2() const { - return GetField(VT_INTERPOLATIONFACTOR2, 0.5); - } - double virtualDistance() const { - return GetField(VT_VIRTUALDISTANCE, 0.0); - } - int32_t auxiliaryFlags() const { - return GetField(VT_AUXILIARYFLAGS, 0); - } - double auxiliaryChannel1() const { - return GetField(VT_AUXILIARYCHANNEL1, 0.0); - } - double auxiliaryChannel2() const { - return GetField(VT_AUXILIARYCHANNEL2, 0.0); - } - double auxiliaryChannel3() const { - return GetField(VT_AUXILIARYCHANNEL3, 0.0); - } - double auxiliaryChannel4() const { - return GetField(VT_AUXILIARYCHANNEL4, 0.0); - } - bool Verify(flatbuffers::Verifier &verifier) const { - return VerifyTableStart(verifier) && - VerifyField(verifier, VT_POSITION) && - VerifyField(verifier, VT_ROTATION) && - VerifyField(verifier, VT_RELATIVEVELOCITY) && - VerifyField(verifier, VT_BEZIERPOINTCOUNT) && - VerifyField(verifier, VT_INTERPOLATIONFACTOR1) && - VerifyField(verifier, VT_INTERPOLATIONFACTOR2) && - VerifyField(verifier, VT_VIRTUALDISTANCE) && - VerifyField(verifier, VT_AUXILIARYFLAGS) && - VerifyField(verifier, VT_AUXILIARYCHANNEL1) && - VerifyField(verifier, VT_AUXILIARYCHANNEL2) && - VerifyField(verifier, VT_AUXILIARYCHANNEL3) && - VerifyField(verifier, VT_AUXILIARYCHANNEL4) && - verifier.EndTable(); - } - VrepControlPointT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; - void UnPackTo(VrepControlPointT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; - static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const VrepControlPointT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); -}; - -struct VrepControlPointBuilder { - flatbuffers::FlatBufferBuilder &fbb_; - flatbuffers::uoffset_t start_; - void add_position(const Vector3d *position) { - fbb_.AddStruct(VrepControlPoint::VT_POSITION, position); - } - void add_rotation(const EulerXYZd *rotation) { - fbb_.AddStruct(VrepControlPoint::VT_ROTATION, rotation); - } - void add_relativeVelocity(double relativeVelocity) { - fbb_.AddElement(VrepControlPoint::VT_RELATIVEVELOCITY, relativeVelocity, 1.0); - } - void add_bezierPointCount(int32_t bezierPointCount) { - fbb_.AddElement(VrepControlPoint::VT_BEZIERPOINTCOUNT, bezierPointCount, 1); - } - void add_interpolationFactor1(double interpolationFactor1) { - fbb_.AddElement(VrepControlPoint::VT_INTERPOLATIONFACTOR1, interpolationFactor1, 0.5); - } - void add_interpolationFactor2(double interpolationFactor2) { - fbb_.AddElement(VrepControlPoint::VT_INTERPOLATIONFACTOR2, interpolationFactor2, 0.5); - } - void add_virtualDistance(double virtualDistance) { - fbb_.AddElement(VrepControlPoint::VT_VIRTUALDISTANCE, virtualDistance, 0.0); - } - void add_auxiliaryFlags(int32_t auxiliaryFlags) { - fbb_.AddElement(VrepControlPoint::VT_AUXILIARYFLAGS, auxiliaryFlags, 0); - } - void add_auxiliaryChannel1(double auxiliaryChannel1) { - fbb_.AddElement(VrepControlPoint::VT_AUXILIARYCHANNEL1, auxiliaryChannel1, 0.0); - } - void add_auxiliaryChannel2(double auxiliaryChannel2) { - fbb_.AddElement(VrepControlPoint::VT_AUXILIARYCHANNEL2, auxiliaryChannel2, 0.0); - } - void add_auxiliaryChannel3(double auxiliaryChannel3) { - fbb_.AddElement(VrepControlPoint::VT_AUXILIARYCHANNEL3, auxiliaryChannel3, 0.0); - } - void add_auxiliaryChannel4(double auxiliaryChannel4) { - fbb_.AddElement(VrepControlPoint::VT_AUXILIARYCHANNEL4, auxiliaryChannel4, 0.0); - } - explicit VrepControlPointBuilder(flatbuffers::FlatBufferBuilder &_fbb) - : fbb_(_fbb) { - start_ = fbb_.StartTable(); - } - VrepControlPointBuilder &operator=(const VrepControlPointBuilder &); - flatbuffers::Offset Finish() { - const auto end = fbb_.EndTable(start_); - auto o = flatbuffers::Offset(end); - return o; - } -}; - -inline flatbuffers::Offset CreateVrepControlPoint( - flatbuffers::FlatBufferBuilder &_fbb, - const Vector3d *position = 0, - const EulerXYZd *rotation = 0, - double relativeVelocity = 1.0, - int32_t bezierPointCount = 1, - double interpolationFactor1 = 0.5, - double interpolationFactor2 = 0.5, - double virtualDistance = 0.0, - int32_t auxiliaryFlags = 0, - double auxiliaryChannel1 = 0.0, - double auxiliaryChannel2 = 0.0, - double auxiliaryChannel3 = 0.0, - double auxiliaryChannel4 = 0.0) { - VrepControlPointBuilder builder_(_fbb); - builder_.add_auxiliaryChannel4(auxiliaryChannel4); - builder_.add_auxiliaryChannel3(auxiliaryChannel3); - builder_.add_auxiliaryChannel2(auxiliaryChannel2); - builder_.add_auxiliaryChannel1(auxiliaryChannel1); - builder_.add_virtualDistance(virtualDistance); - builder_.add_interpolationFactor2(interpolationFactor2); - builder_.add_interpolationFactor1(interpolationFactor1); - builder_.add_relativeVelocity(relativeVelocity); - builder_.add_auxiliaryFlags(auxiliaryFlags); - builder_.add_bezierPointCount(bezierPointCount); - builder_.add_rotation(rotation); - builder_.add_position(position); - return builder_.Finish(); -} - -flatbuffers::Offset CreateVrepControlPoint(flatbuffers::FlatBufferBuilder &_fbb, const VrepControlPointT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); - -inline VrepControlPointT *VrepControlPoint::UnPack(const flatbuffers::resolver_function_t *_resolver) const { - auto _o = new VrepControlPointT(); - UnPackTo(_o, _resolver); - return _o; -} - -inline void VrepControlPoint::UnPackTo(VrepControlPointT *_o, const flatbuffers::resolver_function_t *_resolver) const { - (void)_o; - (void)_resolver; - { auto _e = position(); if (_e) _o->position = std::unique_ptr(new Vector3d(*_e)); }; - { auto _e = rotation(); if (_e) _o->rotation = std::unique_ptr(new EulerXYZd(*_e)); }; - { auto _e = relativeVelocity(); _o->relativeVelocity = _e; }; - { auto _e = bezierPointCount(); _o->bezierPointCount = _e; }; - { auto _e = interpolationFactor1(); _o->interpolationFactor1 = _e; }; - { auto _e = interpolationFactor2(); _o->interpolationFactor2 = _e; }; - { auto _e = virtualDistance(); _o->virtualDistance = _e; }; - { auto _e = auxiliaryFlags(); _o->auxiliaryFlags = _e; }; - { auto _e = auxiliaryChannel1(); _o->auxiliaryChannel1 = _e; }; - { auto _e = auxiliaryChannel2(); _o->auxiliaryChannel2 = _e; }; - { auto _e = auxiliaryChannel3(); _o->auxiliaryChannel3 = _e; }; - { auto _e = auxiliaryChannel4(); _o->auxiliaryChannel4 = _e; }; -} - -inline flatbuffers::Offset VrepControlPoint::Pack(flatbuffers::FlatBufferBuilder &_fbb, const VrepControlPointT* _o, const flatbuffers::rehasher_function_t *_rehasher) { - return CreateVrepControlPoint(_fbb, _o, _rehasher); -} - -inline flatbuffers::Offset CreateVrepControlPoint(flatbuffers::FlatBufferBuilder &_fbb, const VrepControlPointT *_o, const flatbuffers::rehasher_function_t *_rehasher) { - (void)_rehasher; - (void)_o; - struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const VrepControlPointT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; - auto _position = _o->position ? _o->position.get() : 0; - auto _rotation = _o->rotation ? _o->rotation.get() : 0; - auto _relativeVelocity = _o->relativeVelocity; - auto _bezierPointCount = _o->bezierPointCount; - auto _interpolationFactor1 = _o->interpolationFactor1; - auto _interpolationFactor2 = _o->interpolationFactor2; - auto _virtualDistance = _o->virtualDistance; - auto _auxiliaryFlags = _o->auxiliaryFlags; - auto _auxiliaryChannel1 = _o->auxiliaryChannel1; - auto _auxiliaryChannel2 = _o->auxiliaryChannel2; - auto _auxiliaryChannel3 = _o->auxiliaryChannel3; - auto _auxiliaryChannel4 = _o->auxiliaryChannel4; - return grl::flatbuffer::CreateVrepControlPoint( - _fbb, - _position, - _rotation, - _relativeVelocity, - _bezierPointCount, - _interpolationFactor1, - _interpolationFactor2, - _virtualDistance, - _auxiliaryFlags, - _auxiliaryChannel1, - _auxiliaryChannel2, - _auxiliaryChannel3, - _auxiliaryChannel4); -} - -inline const grl::flatbuffer::VrepControlPoint *GetVrepControlPoint(const void *buf) { - return flatbuffers::GetRoot(buf); -} - -inline const grl::flatbuffer::VrepControlPoint *GetSizePrefixedVrepControlPoint(const void *buf) { - return flatbuffers::GetSizePrefixedRoot(buf); -} - -inline bool VerifyVrepControlPointBuffer( - flatbuffers::Verifier &verifier) { - return verifier.VerifyBuffer(nullptr); -} - -inline bool VerifySizePrefixedVrepControlPointBuffer( - flatbuffers::Verifier &verifier) { - return verifier.VerifySizePrefixedBuffer(nullptr); -} - -inline void FinishVrepControlPointBuffer( - flatbuffers::FlatBufferBuilder &fbb, - flatbuffers::Offset root) { - fbb.Finish(root); -} - -inline void FinishSizePrefixedVrepControlPointBuffer( - flatbuffers::FlatBufferBuilder &fbb, - flatbuffers::Offset root) { - fbb.FinishSizePrefixed(root); -} - -inline std::unique_ptr UnPackVrepControlPoint( - const void *buf, - const flatbuffers::resolver_function_t *res = nullptr) { - return std::unique_ptr(GetVrepControlPoint(buf)->UnPack(res)); -} - -} // namespace flatbuffer -} // namespace grl - -#endif // FLATBUFFERS_GENERATED_VREPCONTROLPOINT_GRL_FLATBUFFER_H_ diff --git a/include/grl/flatbuffer/VrepPath_generated.h b/include/grl/flatbuffer/VrepPath_generated.h deleted file mode 100644 index da5343a..0000000 --- a/include/grl/flatbuffer/VrepPath_generated.h +++ /dev/null @@ -1,153 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - - -#ifndef FLATBUFFERS_GENERATED_VREPPATH_GRL_FLATBUFFER_H_ -#define FLATBUFFERS_GENERATED_VREPPATH_GRL_FLATBUFFER_H_ - -#include "flatbuffers/flatbuffers.h" - -#include "Euler_generated.h" -#include "Geometry_generated.h" -#include "VrepControlPoint_generated.h" - -namespace grl { -namespace flatbuffer { - -struct VrepPath; -struct VrepPathT; - -struct VrepPathT : public flatbuffers::NativeTable { - typedef VrepPath TableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.VrepPathT"; - } - std::vector> controlPoints; - VrepPathT() { - } -}; - -struct VrepPath FLATBUFFERS_FINAL_CLASS : private flatbuffers::Table { - typedef VrepPathT NativeTableType; - static FLATBUFFERS_CONSTEXPR const char *GetFullyQualifiedName() { - return "grl.flatbuffer.VrepPath"; - } - enum { - VT_CONTROLPOINTS = 4 - }; - const flatbuffers::Vector> *controlPoints() const { - return GetPointer> *>(VT_CONTROLPOINTS); - } - bool Verify(flatbuffers::Verifier &verifier) const { - return VerifyTableStart(verifier) && - VerifyOffset(verifier, VT_CONTROLPOINTS) && - verifier.Verify(controlPoints()) && - verifier.VerifyVectorOfTables(controlPoints()) && - verifier.EndTable(); - } - VrepPathT *UnPack(const flatbuffers::resolver_function_t *_resolver = nullptr) const; - void UnPackTo(VrepPathT *_o, const flatbuffers::resolver_function_t *_resolver = nullptr) const; - static flatbuffers::Offset Pack(flatbuffers::FlatBufferBuilder &_fbb, const VrepPathT* _o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); -}; - -struct VrepPathBuilder { - flatbuffers::FlatBufferBuilder &fbb_; - flatbuffers::uoffset_t start_; - void add_controlPoints(flatbuffers::Offset>> controlPoints) { - fbb_.AddOffset(VrepPath::VT_CONTROLPOINTS, controlPoints); - } - explicit VrepPathBuilder(flatbuffers::FlatBufferBuilder &_fbb) - : fbb_(_fbb) { - start_ = fbb_.StartTable(); - } - VrepPathBuilder &operator=(const VrepPathBuilder &); - flatbuffers::Offset Finish() { - const auto end = fbb_.EndTable(start_); - auto o = flatbuffers::Offset(end); - return o; - } -}; - -inline flatbuffers::Offset CreateVrepPath( - flatbuffers::FlatBufferBuilder &_fbb, - flatbuffers::Offset>> controlPoints = 0) { - VrepPathBuilder builder_(_fbb); - builder_.add_controlPoints(controlPoints); - return builder_.Finish(); -} - -inline flatbuffers::Offset CreateVrepPathDirect( - flatbuffers::FlatBufferBuilder &_fbb, - const std::vector> *controlPoints = nullptr) { - return grl::flatbuffer::CreateVrepPath( - _fbb, - controlPoints ? _fbb.CreateVector>(*controlPoints) : 0); -} - -flatbuffers::Offset CreateVrepPath(flatbuffers::FlatBufferBuilder &_fbb, const VrepPathT *_o, const flatbuffers::rehasher_function_t *_rehasher = nullptr); - -inline VrepPathT *VrepPath::UnPack(const flatbuffers::resolver_function_t *_resolver) const { - auto _o = new VrepPathT(); - UnPackTo(_o, _resolver); - return _o; -} - -inline void VrepPath::UnPackTo(VrepPathT *_o, const flatbuffers::resolver_function_t *_resolver) const { - (void)_o; - (void)_resolver; - { auto _e = controlPoints(); if (_e) { _o->controlPoints.resize(_e->size()); for (flatbuffers::uoffset_t _i = 0; _i < _e->size(); _i++) { _o->controlPoints[_i] = std::unique_ptr(_e->Get(_i)->UnPack(_resolver)); } } }; -} - -inline flatbuffers::Offset VrepPath::Pack(flatbuffers::FlatBufferBuilder &_fbb, const VrepPathT* _o, const flatbuffers::rehasher_function_t *_rehasher) { - return CreateVrepPath(_fbb, _o, _rehasher); -} - -inline flatbuffers::Offset CreateVrepPath(flatbuffers::FlatBufferBuilder &_fbb, const VrepPathT *_o, const flatbuffers::rehasher_function_t *_rehasher) { - (void)_rehasher; - (void)_o; - struct _VectorArgs { flatbuffers::FlatBufferBuilder *__fbb; const VrepPathT* __o; const flatbuffers::rehasher_function_t *__rehasher; } _va = { &_fbb, _o, _rehasher}; (void)_va; - auto _controlPoints = _o->controlPoints.size() ? _fbb.CreateVector> (_o->controlPoints.size(), [](size_t i, _VectorArgs *__va) { return CreateVrepControlPoint(*__va->__fbb, __va->__o->controlPoints[i].get(), __va->__rehasher); }, &_va ) : 0; - return grl::flatbuffer::CreateVrepPath( - _fbb, - _controlPoints); -} - -inline const grl::flatbuffer::VrepPath *GetVrepPath(const void *buf) { - return flatbuffers::GetRoot(buf); -} - -inline const grl::flatbuffer::VrepPath *GetSizePrefixedVrepPath(const void *buf) { - return flatbuffers::GetSizePrefixedRoot(buf); -} - -inline bool VerifyVrepPathBuffer( - flatbuffers::Verifier &verifier) { - return verifier.VerifyBuffer(nullptr); -} - -inline bool VerifySizePrefixedVrepPathBuffer( - flatbuffers::Verifier &verifier) { - return verifier.VerifySizePrefixedBuffer(nullptr); -} - -inline void FinishVrepPathBuffer( - flatbuffers::FlatBufferBuilder &fbb, - flatbuffers::Offset root) { - fbb.Finish(root); -} - -inline void FinishSizePrefixedVrepPathBuffer( - flatbuffers::FlatBufferBuilder &fbb, - flatbuffers::Offset root) { - fbb.FinishSizePrefixed(root); -} - -inline std::unique_ptr UnPackVrepPath( - const void *buf, - const flatbuffers::resolver_function_t *res = nullptr) { - return std::unique_ptr(GetVrepPath(buf)->UnPack(res)); -} - -} // namespace flatbuffer -} // namespace grl - -#endif // FLATBUFFERS_GENERATED_VREPPATH_GRL_FLATBUFFER_H_ diff --git a/include/grl/flatbuffer/grl/__init__.py b/include/grl/flatbuffer/grl/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/include/grl/flatbuffer/grl/flatbuffer/ArmControlSeries.java b/include/grl/flatbuffer/grl/flatbuffer/ArmControlSeries.java deleted file mode 100644 index 74d88f7..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/ArmControlSeries.java +++ /dev/null @@ -1,40 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class ArmControlSeries extends Table { - public static ArmControlSeries getRootAsArmControlSeries(ByteBuffer _bb) { return getRootAsArmControlSeries(_bb, new ArmControlSeries()); } - public static ArmControlSeries getRootAsArmControlSeries(ByteBuffer _bb, ArmControlSeries obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } - public static boolean ArmControlSeriesBufferHasIdentifier(ByteBuffer _bb) { return __has_identifier(_bb, "armc"); } - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public ArmControlSeries __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - public ArmControlState states(int j) { return states(new ArmControlState(), j); } - public ArmControlState states(ArmControlState obj, int j) { int o = __offset(4); return o != 0 ? obj.__assign(__indirect(__vector(o) + j * 4), bb) : null; } - public int statesLength() { int o = __offset(4); return o != 0 ? __vector_len(o) : 0; } - - public static int createArmControlSeries(FlatBufferBuilder builder, - int statesOffset) { - builder.startObject(1); - ArmControlSeries.addStates(builder, statesOffset); - return ArmControlSeries.endArmControlSeries(builder); - } - - public static void startArmControlSeries(FlatBufferBuilder builder) { builder.startObject(1); } - public static void addStates(FlatBufferBuilder builder, int statesOffset) { builder.addOffset(0, statesOffset, 0); } - public static int createStatesVector(FlatBufferBuilder builder, int[] data) { builder.startVector(4, data.length, 4); for (int i = data.length - 1; i >= 0; i--) builder.addOffset(data[i]); return builder.endVector(); } - public static void startStatesVector(FlatBufferBuilder builder, int numElems) { builder.startVector(4, numElems, 4); } - public static int endArmControlSeries(FlatBufferBuilder builder) { - int o = builder.endObject(); - return o; - } - public static void finishArmControlSeriesBuffer(FlatBufferBuilder builder, int offset) { builder.finish(offset, "armc"); } - public static void finishSizePrefixedArmControlSeriesBuffer(FlatBufferBuilder builder, int offset) { builder.finishSizePrefixed(offset, "armc"); } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/ArmControlSeries.py b/include/grl/flatbuffer/grl/flatbuffer/ArmControlSeries.py deleted file mode 100644 index 0e768ab..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/ArmControlSeries.py +++ /dev/null @@ -1,44 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class ArmControlSeries(object): - __slots__ = ['_tab'] - - @classmethod - def GetRootAsArmControlSeries(cls, buf, offset): - n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) - x = ArmControlSeries() - x.Init(buf, n + offset) - return x - - # ArmControlSeries - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - - # ArmControlSeries - def States(self, j): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - x = self._tab.Vector(o) - x += flatbuffers.number_types.UOffsetTFlags.py_type(j) * 4 - x = self._tab.Indirect(x) - from .ArmControlState import ArmControlState - obj = ArmControlState() - obj.Init(self._tab.Bytes, x) - return obj - return None - - # ArmControlSeries - def StatesLength(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - return self._tab.VectorLen(o) - return 0 - -def ArmControlSeriesStart(builder): builder.StartObject(1) -def ArmControlSeriesAddStates(builder, states): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(states), 0) -def ArmControlSeriesStartStatesVector(builder, numElems): return builder.StartVector(4, numElems, 4) -def ArmControlSeriesEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/ArmControlState.java b/include/grl/flatbuffer/grl/flatbuffer/ArmControlState.java deleted file mode 100644 index dcc9a2a..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/ArmControlState.java +++ /dev/null @@ -1,51 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class ArmControlState extends Table { - public static ArmControlState getRootAsArmControlState(ByteBuffer _bb) { return getRootAsArmControlState(_bb, new ArmControlState()); } - public static ArmControlState getRootAsArmControlState(ByteBuffer _bb, ArmControlState obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public ArmControlState __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - public String name() { int o = __offset(4); return o != 0 ? __string(o + bb_pos) : null; } - public ByteBuffer nameAsByteBuffer() { return __vector_as_bytebuffer(4, 1); } - public ByteBuffer nameInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 4, 1); } - public long sequenceNumber() { int o = __offset(6); return o != 0 ? bb.getLong(o + bb_pos) : 0L; } - public double timeStamp() { int o = __offset(8); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } - public byte stateType() { int o = __offset(10); return o != 0 ? bb.get(o + bb_pos) : 0; } - public Table state(Table obj) { int o = __offset(12); return o != 0 ? __union(obj, o) : null; } - - public static int createArmControlState(FlatBufferBuilder builder, - int nameOffset, - long sequenceNumber, - double timeStamp, - byte state_type, - int stateOffset) { - builder.startObject(5); - ArmControlState.addTimeStamp(builder, timeStamp); - ArmControlState.addSequenceNumber(builder, sequenceNumber); - ArmControlState.addState(builder, stateOffset); - ArmControlState.addName(builder, nameOffset); - ArmControlState.addStateType(builder, state_type); - return ArmControlState.endArmControlState(builder); - } - - public static void startArmControlState(FlatBufferBuilder builder) { builder.startObject(5); } - public static void addName(FlatBufferBuilder builder, int nameOffset) { builder.addOffset(0, nameOffset, 0); } - public static void addSequenceNumber(FlatBufferBuilder builder, long sequenceNumber) { builder.addLong(1, sequenceNumber, 0L); } - public static void addTimeStamp(FlatBufferBuilder builder, double timeStamp) { builder.addDouble(2, timeStamp, 0.0); } - public static void addStateType(FlatBufferBuilder builder, byte stateType) { builder.addByte(3, stateType, 0); } - public static void addState(FlatBufferBuilder builder, int stateOffset) { builder.addOffset(4, stateOffset, 0); } - public static int endArmControlState(FlatBufferBuilder builder) { - int o = builder.endObject(); - return o; - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/ArmControlState.py b/include/grl/flatbuffer/grl/flatbuffer/ArmControlState.py deleted file mode 100644 index abb9979..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/ArmControlState.py +++ /dev/null @@ -1,65 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class ArmControlState(object): - __slots__ = ['_tab'] - - @classmethod - def GetRootAsArmControlState(cls, buf, offset): - n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) - x = ArmControlState() - x.Init(buf, n + offset) - return x - - # ArmControlState - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - - # ArmControlState - def Name(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - return self._tab.String(o + self._tab.Pos) - return bytes() - - # ArmControlState - def SequenceNumber(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Int64Flags, o + self._tab.Pos) - return 0 - - # ArmControlState - def TimeStamp(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) - return 0.0 - - # ArmControlState - def StateType(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Uint8Flags, o + self._tab.Pos) - return 0 - - # ArmControlState - def State(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(12)) - if o != 0: - from flatbuffers.table import Table - obj = Table(bytearray(), 0) - self._tab.Union(obj, o) - return obj - return None - -def ArmControlStateStart(builder): builder.StartObject(5) -def ArmControlStateAddName(builder, name): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(name), 0) -def ArmControlStateAddSequenceNumber(builder, sequenceNumber): builder.PrependInt64Slot(1, sequenceNumber, 0) -def ArmControlStateAddTimeStamp(builder, timeStamp): builder.PrependFloat64Slot(2, timeStamp, 0.0) -def ArmControlStateAddStateType(builder, stateType): builder.PrependUint8Slot(3, stateType, 0) -def ArmControlStateAddState(builder, state): builder.PrependUOffsetTRelativeSlot(4, flatbuffers.number_types.UOffsetTFlags.py_type(state), 0) -def ArmControlStateEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/ArmState.java b/include/grl/flatbuffer/grl/flatbuffer/ArmState.java deleted file mode 100644 index 80885a9..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/ArmState.java +++ /dev/null @@ -1,21 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -public final class ArmState { - private ArmState() { } - public static final byte NONE = 0; - public static final byte StartArm = 1; - public static final byte StopArm = 2; - public static final byte PauseArm = 3; - public static final byte ShutdownArm = 4; - public static final byte TeachArm = 5; - public static final byte MoveArmTrajectory = 6; - public static final byte MoveArmJointServo = 7; - public static final byte MoveArmCartesianServo = 8; - - public static final String[] names = { "NONE", "StartArm", "StopArm", "PauseArm", "ShutdownArm", "TeachArm", "MoveArmTrajectory", "MoveArmJointServo", "MoveArmCartesianServo", }; - - public static String name(int e) { return names[e]; } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/ArmState.py b/include/grl/flatbuffer/grl/flatbuffer/ArmState.py deleted file mode 100644 index e056f75..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/ArmState.py +++ /dev/null @@ -1,15 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -class ArmState(object): - NONE = 0 - StartArm = 1 - StopArm = 2 - PauseArm = 3 - ShutdownArm = 4 - TeachArm = 5 - MoveArmTrajectory = 6 - MoveArmJointServo = 7 - MoveArmCartesianServo = 8 - diff --git a/include/grl/flatbuffer/grl/flatbuffer/CartesianImpedenceControlMode.java b/include/grl/flatbuffer/grl/flatbuffer/CartesianImpedenceControlMode.java deleted file mode 100644 index 360b7e9..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/CartesianImpedenceControlMode.java +++ /dev/null @@ -1,69 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class CartesianImpedenceControlMode extends Table { - public static CartesianImpedenceControlMode getRootAsCartesianImpedenceControlMode(ByteBuffer _bb) { return getRootAsCartesianImpedenceControlMode(_bb, new CartesianImpedenceControlMode()); } - public static CartesianImpedenceControlMode getRootAsCartesianImpedenceControlMode(ByteBuffer _bb, CartesianImpedenceControlMode obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public CartesianImpedenceControlMode __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - /** - * actual stiffness to set rot:[nm/rad] - */ - public EulerPose stiffness() { return stiffness(new EulerPose()); } - public EulerPose stiffness(EulerPose obj) { int o = __offset(4); return o != 0 ? obj.__assign(o + bb_pos, bb) : null; } - /** - * actual damping to set - */ - public EulerPose damping() { return damping(new EulerPose()); } - public EulerPose damping(EulerPose obj) { int o = __offset(6); return o != 0 ? obj.__assign(o + bb_pos, bb) : null; } - /** - * [Nm/rad] must be => 0.0 - */ - public double nullspaceStiffness() { int o = __offset(8); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } - /** - * must be between 0.3-1.0 suggested is 0.7 - */ - public double nullspaceDamping() { int o = __offset(10); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } - /** - * maximum deviation from set goal in mm and radians - */ - public EulerPose maxPathDeviation() { return maxPathDeviation(new EulerPose()); } - public EulerPose maxPathDeviation(EulerPose obj) { int o = __offset(12); return o != 0 ? obj.__assign(o + bb_pos, bb) : null; } - /** - * trans: [mm/s] rot: [rad/s] - */ - public EulerPose maxCartesianVelocity() { return maxCartesianVelocity(new EulerPose()); } - public EulerPose maxCartesianVelocity(EulerPose obj) { int o = __offset(14); return o != 0 ? obj.__assign(o + bb_pos, bb) : null; } - /** - * xyz: Newtons rpy:Nm (all >=0) - */ - public EulerPose maxControlForce() { return maxControlForce(new EulerPose()); } - public EulerPose maxControlForce(EulerPose obj) { int o = __offset(16); return o != 0 ? obj.__assign(o + bb_pos, bb) : null; } - /** - * stop if max control force is exceeded - */ - public boolean maxControlForceExceededStop() { int o = __offset(18); return o != 0 ? 0!=bb.get(o + bb_pos) : false; } - - public static void startCartesianImpedenceControlMode(FlatBufferBuilder builder) { builder.startObject(8); } - public static void addStiffness(FlatBufferBuilder builder, int stiffnessOffset) { builder.addStruct(0, stiffnessOffset, 0); } - public static void addDamping(FlatBufferBuilder builder, int dampingOffset) { builder.addStruct(1, dampingOffset, 0); } - public static void addNullspaceStiffness(FlatBufferBuilder builder, double nullspaceStiffness) { builder.addDouble(2, nullspaceStiffness, 0.0); } - public static void addNullspaceDamping(FlatBufferBuilder builder, double nullspaceDamping) { builder.addDouble(3, nullspaceDamping, 0.0); } - public static void addMaxPathDeviation(FlatBufferBuilder builder, int maxPathDeviationOffset) { builder.addStruct(4, maxPathDeviationOffset, 0); } - public static void addMaxCartesianVelocity(FlatBufferBuilder builder, int maxCartesianVelocityOffset) { builder.addStruct(5, maxCartesianVelocityOffset, 0); } - public static void addMaxControlForce(FlatBufferBuilder builder, int maxControlForceOffset) { builder.addStruct(6, maxControlForceOffset, 0); } - public static void addMaxControlForceExceededStop(FlatBufferBuilder builder, boolean maxControlForceExceededStop) { builder.addBoolean(7, maxControlForceExceededStop, false); } - public static int endCartesianImpedenceControlMode(FlatBufferBuilder builder) { - int o = builder.endObject(); - return o; - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/CartesianImpedenceControlMode.py b/include/grl/flatbuffer/grl/flatbuffer/CartesianImpedenceControlMode.py deleted file mode 100644 index a74292d..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/CartesianImpedenceControlMode.py +++ /dev/null @@ -1,114 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class CartesianImpedenceControlMode(object): - __slots__ = ['_tab'] - - @classmethod - def GetRootAsCartesianImpedenceControlMode(cls, buf, offset): - n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) - x = CartesianImpedenceControlMode() - x.Init(buf, n + offset) - return x - - # CartesianImpedenceControlMode - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - -# /// actual stiffness to set rot:[nm/rad] - # CartesianImpedenceControlMode - def Stiffness(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - x = o + self._tab.Pos - from .EulerPose import EulerPose - obj = EulerPose() - obj.Init(self._tab.Bytes, x) - return obj - return None - -# /// actual damping to set - # CartesianImpedenceControlMode - def Damping(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) - if o != 0: - x = o + self._tab.Pos - from .EulerPose import EulerPose - obj = EulerPose() - obj.Init(self._tab.Bytes, x) - return obj - return None - -# /// [Nm/rad] must be => 0.0 - # CartesianImpedenceControlMode - def NullspaceStiffness(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) - return 0.0 - -# /// must be between 0.3-1.0 suggested is 0.7 - # CartesianImpedenceControlMode - def NullspaceDamping(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) - return 0.0 - -# /// maximum deviation from set goal in mm and radians - # CartesianImpedenceControlMode - def MaxPathDeviation(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(12)) - if o != 0: - x = o + self._tab.Pos - from .EulerPose import EulerPose - obj = EulerPose() - obj.Init(self._tab.Bytes, x) - return obj - return None - -# /// trans: [mm/s] rot: [rad/s] - # CartesianImpedenceControlMode - def MaxCartesianVelocity(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(14)) - if o != 0: - x = o + self._tab.Pos - from .EulerPose import EulerPose - obj = EulerPose() - obj.Init(self._tab.Bytes, x) - return obj - return None - -# /// xyz: Newtons rpy:Nm (all >=0) - # CartesianImpedenceControlMode - def MaxControlForce(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(16)) - if o != 0: - x = o + self._tab.Pos - from .EulerPose import EulerPose - obj = EulerPose() - obj.Init(self._tab.Bytes, x) - return obj - return None - -# /// stop if max control force is exceeded - # CartesianImpedenceControlMode - def MaxControlForceExceededStop(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(18)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.BoolFlags, o + self._tab.Pos) - return 0 - -def CartesianImpedenceControlModeStart(builder): builder.StartObject(8) -def CartesianImpedenceControlModeAddStiffness(builder, stiffness): builder.PrependStructSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(stiffness), 0) -def CartesianImpedenceControlModeAddDamping(builder, damping): builder.PrependStructSlot(1, flatbuffers.number_types.UOffsetTFlags.py_type(damping), 0) -def CartesianImpedenceControlModeAddNullspaceStiffness(builder, nullspaceStiffness): builder.PrependFloat64Slot(2, nullspaceStiffness, 0.0) -def CartesianImpedenceControlModeAddNullspaceDamping(builder, nullspaceDamping): builder.PrependFloat64Slot(3, nullspaceDamping, 0.0) -def CartesianImpedenceControlModeAddMaxPathDeviation(builder, maxPathDeviation): builder.PrependStructSlot(4, flatbuffers.number_types.UOffsetTFlags.py_type(maxPathDeviation), 0) -def CartesianImpedenceControlModeAddMaxCartesianVelocity(builder, maxCartesianVelocity): builder.PrependStructSlot(5, flatbuffers.number_types.UOffsetTFlags.py_type(maxCartesianVelocity), 0) -def CartesianImpedenceControlModeAddMaxControlForce(builder, maxControlForce): builder.PrependStructSlot(6, flatbuffers.number_types.UOffsetTFlags.py_type(maxControlForce), 0) -def CartesianImpedenceControlModeAddMaxControlForceExceededStop(builder, maxControlForceExceededStop): builder.PrependBoolSlot(7, maxControlForceExceededStop, 0) -def CartesianImpedenceControlModeEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/DeviceState.java b/include/grl/flatbuffer/grl/flatbuffer/DeviceState.java deleted file mode 100644 index d82dcd2..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/DeviceState.java +++ /dev/null @@ -1,15 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -public final class DeviceState { - private DeviceState() { } - public static final byte NONE = 0; - public static final byte KUKAiiwaState = 1; - public static final byte FusionTrackMessage = 2; - - public static final String[] names = { "NONE", "KUKAiiwaState", "FusionTrackMessage", }; - - public static String name(int e) { return names[e]; } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/DeviceState.py b/include/grl/flatbuffer/grl/flatbuffer/DeviceState.py deleted file mode 100644 index 64f92d2..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/DeviceState.py +++ /dev/null @@ -1,9 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -class DeviceState(object): - NONE = 0 - KUKAiiwaState = 1 - FusionTrackMessage = 2 - diff --git a/include/grl/flatbuffer/grl/flatbuffer/Disabled.java b/include/grl/flatbuffer/grl/flatbuffer/Disabled.java deleted file mode 100644 index 9b2f136..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/Disabled.java +++ /dev/null @@ -1,24 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class Disabled extends Table { - public static Disabled getRootAsDisabled(ByteBuffer _bb) { return getRootAsDisabled(_bb, new Disabled()); } - public static Disabled getRootAsDisabled(ByteBuffer _bb, Disabled obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public Disabled __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - - public static void startDisabled(FlatBufferBuilder builder) { builder.startObject(0); } - public static int endDisabled(FlatBufferBuilder builder) { - int o = builder.endObject(); - return o; - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/Disabled.py b/include/grl/flatbuffer/grl/flatbuffer/Disabled.py deleted file mode 100644 index a91db21..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/Disabled.py +++ /dev/null @@ -1,22 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class Disabled(object): - __slots__ = ['_tab'] - - @classmethod - def GetRootAsDisabled(cls, buf, offset): - n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) - x = Disabled() - x.Init(buf, n + offset) - return x - - # Disabled - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - -def DisabledStart(builder): builder.StartObject(0) -def DisabledEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/EClientCommandMode.java b/include/grl/flatbuffer/grl/flatbuffer/EClientCommandMode.java deleted file mode 100644 index c68bae8..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/EClientCommandMode.java +++ /dev/null @@ -1,19 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -/** - * Type of command being sent to the arm (Dimensonal units) - */ -public final class EClientCommandMode { - private EClientCommandMode() { } - public static final byte NO_COMMAND_MODE = 0; - public static final byte POSITION = 1; - public static final byte WRENCH = 2; - public static final byte TORQUE = 3; - - public static final String[] names = { "NO_COMMAND_MODE", "POSITION", "WRENCH", "TORQUE", }; - - public static String name(int e) { return names[e]; } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/EClientCommandMode.py b/include/grl/flatbuffer/grl/flatbuffer/EClientCommandMode.py deleted file mode 100644 index 92ff046..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/EClientCommandMode.py +++ /dev/null @@ -1,11 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -# /// Type of command being sent to the arm (Dimensonal units) -class EClientCommandMode(object): - NO_COMMAND_MODE = 0 - POSITION = 1 - WRENCH = 2 - TORQUE = 3 - diff --git a/include/grl/flatbuffer/grl/flatbuffer/EConnectionQuality.java b/include/grl/flatbuffer/grl/flatbuffer/EConnectionQuality.java deleted file mode 100644 index 830f5a7..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/EConnectionQuality.java +++ /dev/null @@ -1,16 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -public final class EConnectionQuality { - private EConnectionQuality() { } - public static final byte POOR = 0; - public static final byte FAIR = 1; - public static final byte GOOD = 2; - public static final byte EXCELLENT = 3; - - public static final String[] names = { "POOR", "FAIR", "GOOD", "EXCELLENT", }; - - public static String name(int e) { return names[e]; } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/EConnectionQuality.py b/include/grl/flatbuffer/grl/flatbuffer/EConnectionQuality.py deleted file mode 100644 index cc17e4b..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/EConnectionQuality.py +++ /dev/null @@ -1,10 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -class EConnectionQuality(object): - POOR = 0 - FAIR = 1 - GOOD = 2 - EXCELLENT = 3 - diff --git a/include/grl/flatbuffer/grl/flatbuffer/EControlMode.java b/include/grl/flatbuffer/grl/flatbuffer/EControlMode.java deleted file mode 100644 index 41c97a5..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/EControlMode.java +++ /dev/null @@ -1,16 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -public final class EControlMode { - private EControlMode() { } - public static final byte POSITION_CONTROL_MODE = 0; - public static final byte CART_IMP_CONTROL_MODE = 1; - public static final byte JOINT_IMP_CONTROL_MODE = 2; - public static final byte NO_CONTROL = 3; - - public static final String[] names = { "POSITION_CONTROL_MODE", "CART_IMP_CONTROL_MODE", "JOINT_IMP_CONTROL_MODE", "NO_CONTROL", }; - - public static String name(int e) { return names[e]; } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/EControlMode.py b/include/grl/flatbuffer/grl/flatbuffer/EControlMode.py deleted file mode 100644 index 2ea13d6..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/EControlMode.py +++ /dev/null @@ -1,10 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -class EControlMode(object): - POSITION_CONTROL_MODE = 0 - CART_IMP_CONTROL_MODE = 1 - JOINT_IMP_CONTROL_MODE = 2 - NO_CONTROL = 3 - diff --git a/include/grl/flatbuffer/grl/flatbuffer/EDriveState.java b/include/grl/flatbuffer/grl/flatbuffer/EDriveState.java deleted file mode 100644 index fca0cf4..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/EDriveState.java +++ /dev/null @@ -1,24 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -public final class EDriveState { - private EDriveState() { } - /** - * Driving mode currently unused - */ - public static final byte OFF = 1; - /** - * About to drive - */ - public static final byte TRANSITIONING = 2; - /** - * Actively commanding arm - */ - public static final byte ACTIVE = 3; - - public static final String[] names = { "OFF", "TRANSITIONING", "ACTIVE", }; - - public static String name(int e) { return names[e - OFF]; } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/EDriveState.py b/include/grl/flatbuffer/grl/flatbuffer/EDriveState.py deleted file mode 100644 index 4a504e1..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/EDriveState.py +++ /dev/null @@ -1,12 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -class EDriveState(object): -# /// Driving mode currently unused - OFF = 1 -# /// About to drive - TRANSITIONING = 2 -# /// Actively commanding arm - ACTIVE = 3 - diff --git a/include/grl/flatbuffer/grl/flatbuffer/EOperationMode.java b/include/grl/flatbuffer/grl/flatbuffer/EOperationMode.java deleted file mode 100644 index 30b049d..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/EOperationMode.java +++ /dev/null @@ -1,15 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -public final class EOperationMode { - private EOperationMode() { } - public static final byte TEST_MODE_1 = 0; - public static final byte TEST_MODE_2 = 1; - public static final byte AUTOMATIC_MODE = 2; - - public static final String[] names = { "TEST_MODE_1", "TEST_MODE_2", "AUTOMATIC_MODE", }; - - public static String name(int e) { return names[e]; } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/EOperationMode.py b/include/grl/flatbuffer/grl/flatbuffer/EOperationMode.py deleted file mode 100644 index 898ee70..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/EOperationMode.py +++ /dev/null @@ -1,9 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -class EOperationMode(object): - TEST_MODE_1 = 0 - TEST_MODE_2 = 1 - AUTOMATIC_MODE = 2 - diff --git a/include/grl/flatbuffer/grl/flatbuffer/EOverlayType.java b/include/grl/flatbuffer/grl/flatbuffer/EOverlayType.java deleted file mode 100644 index 61e5cf2..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/EOverlayType.java +++ /dev/null @@ -1,15 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -public final class EOverlayType { - private EOverlayType() { } - public static final byte NO_OVERLAY = 0; - public static final byte JOINT = 1; - public static final byte CARTESIAN = 2; - - public static final String[] names = { "NO_OVERLAY", "JOINT", "CARTESIAN", }; - - public static String name(int e) { return names[e]; } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/EOverlayType.py b/include/grl/flatbuffer/grl/flatbuffer/EOverlayType.py deleted file mode 100644 index b3e63f5..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/EOverlayType.py +++ /dev/null @@ -1,9 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -class EOverlayType(object): - NO_OVERLAY = 0 - JOINT = 1 - CARTESIAN = 2 - diff --git a/include/grl/flatbuffer/grl/flatbuffer/ESafetyState.java b/include/grl/flatbuffer/grl/flatbuffer/ESafetyState.java deleted file mode 100644 index 4c463ee..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/ESafetyState.java +++ /dev/null @@ -1,16 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -public final class ESafetyState { - private ESafetyState() { } - public static final byte NORMAL_OPERATION = 0; - public static final byte SAFETY_STOP_LEVEL_0 = 1; - public static final byte SAFETY_STOP_LEVEL_1 = 2; - public static final byte SAFETY_STOP_LEVEL_2 = 3; - - public static final String[] names = { "NORMAL_OPERATION", "SAFETY_STOP_LEVEL_0", "SAFETY_STOP_LEVEL_1", "SAFETY_STOP_LEVEL_2", }; - - public static String name(int e) { return names[e]; } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/ESafetyState.py b/include/grl/flatbuffer/grl/flatbuffer/ESafetyState.py deleted file mode 100644 index 82af46a..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/ESafetyState.py +++ /dev/null @@ -1,10 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -class ESafetyState(object): - NORMAL_OPERATION = 0 - SAFETY_STOP_LEVEL_0 = 1 - SAFETY_STOP_LEVEL_1 = 2 - SAFETY_STOP_LEVEL_2 = 3 - diff --git a/include/grl/flatbuffer/grl/flatbuffer/ESessionState.java b/include/grl/flatbuffer/grl/flatbuffer/ESessionState.java deleted file mode 100644 index 41fc63f..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/ESessionState.java +++ /dev/null @@ -1,32 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -public final class ESessionState { - private ESessionState() { } - /** - * No session - */ - public static final byte IDLE = 0; - /** - * Monitoring mode, receive state but connection too inconsistent to command - */ - public static final byte MONITORING_WAIT = 1; - /** - * Monitoring mode - */ - public static final byte MONITORING_READY = 2; - /** - * About to command (Overlay created in Java interface) - */ - public static final byte COMMANDING_WAIT = 3; - /** - * Actively commanding the arm with FRI - */ - public static final byte COMMANDING_ACTIVE = 4; - - public static final String[] names = { "IDLE", "MONITORING_WAIT", "MONITORING_READY", "COMMANDING_WAIT", "COMMANDING_ACTIVE", }; - - public static String name(int e) { return names[e]; } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/ESessionState.py b/include/grl/flatbuffer/grl/flatbuffer/ESessionState.py deleted file mode 100644 index efcb0cd..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/ESessionState.py +++ /dev/null @@ -1,16 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -class ESessionState(object): -# /// No session - IDLE = 0 -# /// Monitoring mode, receive state but connection too inconsistent to command - MONITORING_WAIT = 1 -# /// Monitoring mode - MONITORING_READY = 2 -# /// About to command (Overlay created in Java interface) - COMMANDING_WAIT = 3 -# /// Actively commanding the arm with FRI - COMMANDING_ACTIVE = 4 - diff --git a/include/grl/flatbuffer/grl/flatbuffer/EulerOrder.java b/include/grl/flatbuffer/grl/flatbuffer/EulerOrder.java deleted file mode 100644 index 5d507ef..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/EulerOrder.java +++ /dev/null @@ -1,23 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -public final class EulerOrder { - private EulerOrder() { } - public static final byte xyz = 0; - public static final byte yzx = 1; - public static final byte zxy = 2; - public static final byte xzy = 3; - public static final byte zyx = 4; - public static final byte yxz = 5; - public static final byte zxz = 6; - public static final byte xyx = 7; - public static final byte yzy = 8; - public static final byte xzx = 9; - public static final byte yxy = 10; - - public static final String[] names = { "xyz", "yzx", "zxy", "xzy", "zyx", "yxz", "zxz", "xyx", "yzy", "xzx", "yxy", }; - - public static String name(int e) { return names[e]; } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/EulerOrder.py b/include/grl/flatbuffer/grl/flatbuffer/EulerOrder.py deleted file mode 100644 index c95a5ec..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/EulerOrder.py +++ /dev/null @@ -1,17 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -class EulerOrder(object): - xyz = 0 - yzx = 1 - zxy = 2 - xzy = 3 - zyx = 4 - yxz = 5 - zxz = 6 - xyx = 7 - yzy = 8 - xzx = 9 - yxy = 10 - diff --git a/include/grl/flatbuffer/grl/flatbuffer/EulerPose.java b/include/grl/flatbuffer/grl/flatbuffer/EulerPose.java deleted file mode 100644 index 70141bc..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/EulerPose.java +++ /dev/null @@ -1,35 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class EulerPose extends Struct { - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public EulerPose __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - public Vector3d position() { return position(new Vector3d()); } - public Vector3d position(Vector3d obj) { return obj.__assign(bb_pos + 0, bb); } - public EulerRotation rotation() { return rotation(new EulerRotation()); } - public EulerRotation rotation(EulerRotation obj) { return obj.__assign(bb_pos + 24, bb); } - - public static int createEulerPose(FlatBufferBuilder builder, double position_x, double position_y, double position_z, double rotation_r1, double rotation_r2, double rotation_r3, byte rotation_eulerOrder) { - builder.prep(8, 56); - builder.prep(8, 32); - builder.pad(7); - builder.putByte(rotation_eulerOrder); - builder.putDouble(rotation_r3); - builder.putDouble(rotation_r2); - builder.putDouble(rotation_r1); - builder.prep(8, 24); - builder.putDouble(position_z); - builder.putDouble(position_y); - builder.putDouble(position_x); - return builder.offset(); - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/EulerPose.py b/include/grl/flatbuffer/grl/flatbuffer/EulerPose.py deleted file mode 100644 index 54497ad..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/EulerPose.py +++ /dev/null @@ -1,37 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class EulerPose(object): - __slots__ = ['_tab'] - - # EulerPose - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - - # EulerPose - def Position(self, obj): - obj.Init(self._tab.Bytes, self._tab.Pos + 0) - return obj - - # EulerPose - def Rotation(self, obj): - obj.Init(self._tab.Bytes, self._tab.Pos + 24) - return obj - - -def CreateEulerPose(builder, position_x, position_y, position_z, rotation_r1, rotation_r2, rotation_r3, rotation_eulerOrder): - builder.Prep(8, 56) - builder.Prep(8, 32) - builder.Pad(7) - builder.PrependInt8(rotation_eulerOrder) - builder.PrependFloat64(rotation_r3) - builder.PrependFloat64(rotation_r2) - builder.PrependFloat64(rotation_r1) - builder.Prep(8, 24) - builder.PrependFloat64(position_z) - builder.PrependFloat64(position_y) - builder.PrependFloat64(position_x) - return builder.Offset() diff --git a/include/grl/flatbuffer/grl/flatbuffer/EulerPoseParams.java b/include/grl/flatbuffer/grl/flatbuffer/EulerPoseParams.java deleted file mode 100644 index ec8a1a2..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/EulerPoseParams.java +++ /dev/null @@ -1,30 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class EulerPoseParams extends Table { - public static EulerPoseParams getRootAsEulerPoseParams(ByteBuffer _bb) { return getRootAsEulerPoseParams(_bb, new EulerPoseParams()); } - public static EulerPoseParams getRootAsEulerPoseParams(ByteBuffer _bb, EulerPoseParams obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public EulerPoseParams __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - public Vector3d position() { return position(new Vector3d()); } - public Vector3d position(Vector3d obj) { int o = __offset(4); return o != 0 ? obj.__assign(o + bb_pos, bb) : null; } - public EulerRotation rotation() { return rotation(new EulerRotation()); } - public EulerRotation rotation(EulerRotation obj) { int o = __offset(6); return o != 0 ? obj.__assign(o + bb_pos, bb) : null; } - - public static void startEulerPoseParams(FlatBufferBuilder builder) { builder.startObject(2); } - public static void addPosition(FlatBufferBuilder builder, int positionOffset) { builder.addStruct(0, positionOffset, 0); } - public static void addRotation(FlatBufferBuilder builder, int rotationOffset) { builder.addStruct(1, rotationOffset, 0); } - public static int endEulerPoseParams(FlatBufferBuilder builder) { - int o = builder.endObject(); - return o; - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/EulerPoseParams.py b/include/grl/flatbuffer/grl/flatbuffer/EulerPoseParams.py deleted file mode 100644 index 39dd403..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/EulerPoseParams.py +++ /dev/null @@ -1,46 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class EulerPoseParams(object): - __slots__ = ['_tab'] - - @classmethod - def GetRootAsEulerPoseParams(cls, buf, offset): - n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) - x = EulerPoseParams() - x.Init(buf, n + offset) - return x - - # EulerPoseParams - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - - # EulerPoseParams - def Position(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - x = o + self._tab.Pos - from .Vector3d import Vector3d - obj = Vector3d() - obj.Init(self._tab.Bytes, x) - return obj - return None - - # EulerPoseParams - def Rotation(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) - if o != 0: - x = o + self._tab.Pos - from .EulerRotation import EulerRotation - obj = EulerRotation() - obj.Init(self._tab.Bytes, x) - return obj - return None - -def EulerPoseParamsStart(builder): builder.StartObject(2) -def EulerPoseParamsAddPosition(builder, position): builder.PrependStructSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(position), 0) -def EulerPoseParamsAddRotation(builder, rotation): builder.PrependStructSlot(1, flatbuffers.number_types.UOffsetTFlags.py_type(rotation), 0) -def EulerPoseParamsEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/EulerRotation.java b/include/grl/flatbuffer/grl/flatbuffer/EulerRotation.java deleted file mode 100644 index e7c810f..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/EulerRotation.java +++ /dev/null @@ -1,30 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class EulerRotation extends Struct { - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public EulerRotation __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - public double r1() { return bb.getDouble(bb_pos + 0); } - public double r2() { return bb.getDouble(bb_pos + 8); } - public double r3() { return bb.getDouble(bb_pos + 16); } - public byte eulerOrder() { return bb.get(bb_pos + 24); } - - public static int createEulerRotation(FlatBufferBuilder builder, double r1, double r2, double r3, byte eulerOrder) { - builder.prep(8, 32); - builder.pad(7); - builder.putByte(eulerOrder); - builder.putDouble(r3); - builder.putDouble(r2); - builder.putDouble(r1); - return builder.offset(); - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/EulerRotation.py b/include/grl/flatbuffer/grl/flatbuffer/EulerRotation.py deleted file mode 100644 index c8353a5..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/EulerRotation.py +++ /dev/null @@ -1,30 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class EulerRotation(object): - __slots__ = ['_tab'] - - # EulerRotation - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - - # EulerRotation - def R1(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(0)) - # EulerRotation - def R2(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(8)) - # EulerRotation - def R3(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(16)) - # EulerRotation - def EulerOrder(self): return self._tab.Get(flatbuffers.number_types.Int8Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(24)) - -def CreateEulerRotation(builder, r1, r2, r3, eulerOrder): - builder.Prep(8, 32) - builder.Pad(7) - builder.PrependInt8(eulerOrder) - builder.PrependFloat64(r3) - builder.PrependFloat64(r2) - builder.PrependFloat64(r1) - return builder.Offset() diff --git a/include/grl/flatbuffer/grl/flatbuffer/EulerRotationParams.java b/include/grl/flatbuffer/grl/flatbuffer/EulerRotationParams.java deleted file mode 100644 index b6c8b5d..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/EulerRotationParams.java +++ /dev/null @@ -1,45 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class EulerRotationParams extends Table { - public static EulerRotationParams getRootAsEulerRotationParams(ByteBuffer _bb) { return getRootAsEulerRotationParams(_bb, new EulerRotationParams()); } - public static EulerRotationParams getRootAsEulerRotationParams(ByteBuffer _bb, EulerRotationParams obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public EulerRotationParams __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - public double r1() { int o = __offset(4); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } - public double r2() { int o = __offset(6); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } - public double r3() { int o = __offset(8); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } - public byte eulerOrder() { int o = __offset(10); return o != 0 ? bb.get(o + bb_pos) : 0; } - - public static int createEulerRotationParams(FlatBufferBuilder builder, - double r1, - double r2, - double r3, - byte eulerOrder) { - builder.startObject(4); - EulerRotationParams.addR3(builder, r3); - EulerRotationParams.addR2(builder, r2); - EulerRotationParams.addR1(builder, r1); - EulerRotationParams.addEulerOrder(builder, eulerOrder); - return EulerRotationParams.endEulerRotationParams(builder); - } - - public static void startEulerRotationParams(FlatBufferBuilder builder) { builder.startObject(4); } - public static void addR1(FlatBufferBuilder builder, double r1) { builder.addDouble(0, r1, 0.0); } - public static void addR2(FlatBufferBuilder builder, double r2) { builder.addDouble(1, r2, 0.0); } - public static void addR3(FlatBufferBuilder builder, double r3) { builder.addDouble(2, r3, 0.0); } - public static void addEulerOrder(FlatBufferBuilder builder, byte eulerOrder) { builder.addByte(3, eulerOrder, 0); } - public static int endEulerRotationParams(FlatBufferBuilder builder) { - int o = builder.endObject(); - return o; - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/EulerRotationParams.py b/include/grl/flatbuffer/grl/flatbuffer/EulerRotationParams.py deleted file mode 100644 index e1ec4df..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/EulerRotationParams.py +++ /dev/null @@ -1,54 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class EulerRotationParams(object): - __slots__ = ['_tab'] - - @classmethod - def GetRootAsEulerRotationParams(cls, buf, offset): - n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) - x = EulerRotationParams() - x.Init(buf, n + offset) - return x - - # EulerRotationParams - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - - # EulerRotationParams - def R1(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) - return 0.0 - - # EulerRotationParams - def R2(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) - return 0.0 - - # EulerRotationParams - def R3(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) - return 0.0 - - # EulerRotationParams - def EulerOrder(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Int8Flags, o + self._tab.Pos) - return 0 - -def EulerRotationParamsStart(builder): builder.StartObject(4) -def EulerRotationParamsAddR1(builder, r1): builder.PrependFloat64Slot(0, r1, 0.0) -def EulerRotationParamsAddR2(builder, r2): builder.PrependFloat64Slot(1, r2, 0.0) -def EulerRotationParamsAddR3(builder, r3): builder.PrependFloat64Slot(2, r3, 0.0) -def EulerRotationParamsAddEulerOrder(builder, eulerOrder): builder.PrependInt8Slot(3, eulerOrder, 0) -def EulerRotationParamsEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/EulerTranslationParams.java b/include/grl/flatbuffer/grl/flatbuffer/EulerTranslationParams.java deleted file mode 100644 index 6a12b1f..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/EulerTranslationParams.java +++ /dev/null @@ -1,41 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class EulerTranslationParams extends Table { - public static EulerTranslationParams getRootAsEulerTranslationParams(ByteBuffer _bb) { return getRootAsEulerTranslationParams(_bb, new EulerTranslationParams()); } - public static EulerTranslationParams getRootAsEulerTranslationParams(ByteBuffer _bb, EulerTranslationParams obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public EulerTranslationParams __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - public double x() { int o = __offset(4); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } - public double y() { int o = __offset(6); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } - public double z() { int o = __offset(8); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } - - public static int createEulerTranslationParams(FlatBufferBuilder builder, - double x, - double y, - double z) { - builder.startObject(3); - EulerTranslationParams.addZ(builder, z); - EulerTranslationParams.addY(builder, y); - EulerTranslationParams.addX(builder, x); - return EulerTranslationParams.endEulerTranslationParams(builder); - } - - public static void startEulerTranslationParams(FlatBufferBuilder builder) { builder.startObject(3); } - public static void addX(FlatBufferBuilder builder, double x) { builder.addDouble(0, x, 0.0); } - public static void addY(FlatBufferBuilder builder, double y) { builder.addDouble(1, y, 0.0); } - public static void addZ(FlatBufferBuilder builder, double z) { builder.addDouble(2, z, 0.0); } - public static int endEulerTranslationParams(FlatBufferBuilder builder) { - int o = builder.endObject(); - return o; - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/EulerTranslationParams.py b/include/grl/flatbuffer/grl/flatbuffer/EulerTranslationParams.py deleted file mode 100644 index eca6b41..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/EulerTranslationParams.py +++ /dev/null @@ -1,46 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class EulerTranslationParams(object): - __slots__ = ['_tab'] - - @classmethod - def GetRootAsEulerTranslationParams(cls, buf, offset): - n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) - x = EulerTranslationParams() - x.Init(buf, n + offset) - return x - - # EulerTranslationParams - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - - # EulerTranslationParams - def X(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) - return 0.0 - - # EulerTranslationParams - def Y(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) - return 0.0 - - # EulerTranslationParams - def Z(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) - return 0.0 - -def EulerTranslationParamsStart(builder): builder.StartObject(3) -def EulerTranslationParamsAddX(builder, x): builder.PrependFloat64Slot(0, x, 0.0) -def EulerTranslationParamsAddY(builder, y): builder.PrependFloat64Slot(1, y, 0.0) -def EulerTranslationParamsAddZ(builder, z): builder.PrependFloat64Slot(2, z, 0.0) -def EulerTranslationParamsEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/EulerXYZd.java b/include/grl/flatbuffer/grl/flatbuffer/EulerXYZd.java deleted file mode 100644 index 17084ff..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/EulerXYZd.java +++ /dev/null @@ -1,27 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class EulerXYZd extends Struct { - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public EulerXYZd __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - public double rx() { return bb.getDouble(bb_pos + 0); } - public double ry() { return bb.getDouble(bb_pos + 8); } - public double rz() { return bb.getDouble(bb_pos + 16); } - - public static int createEulerXYZd(FlatBufferBuilder builder, double rx, double ry, double rz) { - builder.prep(8, 24); - builder.putDouble(rz); - builder.putDouble(ry); - builder.putDouble(rx); - return builder.offset(); - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/EulerXYZd.py b/include/grl/flatbuffer/grl/flatbuffer/EulerXYZd.py deleted file mode 100644 index cb2120b..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/EulerXYZd.py +++ /dev/null @@ -1,26 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class EulerXYZd(object): - __slots__ = ['_tab'] - - # EulerXYZd - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - - # EulerXYZd - def Rx(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(0)) - # EulerXYZd - def Ry(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(8)) - # EulerXYZd - def Rz(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(16)) - -def CreateEulerXYZd(builder, rx, ry, rz): - builder.Prep(8, 24) - builder.PrependFloat64(rz) - builder.PrependFloat64(ry) - builder.PrependFloat64(rx) - return builder.Offset() diff --git a/include/grl/flatbuffer/grl/flatbuffer/FRI.java b/include/grl/flatbuffer/grl/flatbuffer/FRI.java deleted file mode 100644 index 8a43874..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/FRI.java +++ /dev/null @@ -1,97 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class FRI extends Table { - public static FRI getRootAsFRI(ByteBuffer _bb) { return getRootAsFRI(_bb, new FRI()); } - public static FRI getRootAsFRI(ByteBuffer _bb, FRI obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public FRI __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - public byte overlayType() { int o = __offset(4); return o != 0 ? bb.get(o + bb_pos) : 1; } - /** - * Set the value for the send period of the connection from the KUKA controller to the remote side in [ms]. - * This means, the KUKA controller will send cyclic FRI messages every sendPeriod milliseconds to the remote side. - * - * - * Parameters: - * sendPeriod - the send period in milliseconds, 1 <= sendPeriod <= 100. - * Note: The recommended value for good performance should be between 1-5 milliseconds. - */ - public int sendPeriodMillisec() { int o = __offset(6); return o != 0 ? bb.getInt(o + bb_pos) : 4; } - /** - * Set the receive multiplier of the cycle time from the remote side to the KUKA controller. - * This multiplier defines the value of the receivePeriod which is calculated: - * receivePeriod = receiveMultiplier * sendPeriod - * - * The KUKA controller will expect a FRI response message every receivePeriod milliseconds from the remote side. - * - * The receivePeriod has to be within the range of: - * 1 <= receivePeriod <= 100. - */ - public int setReceiveMultiplier() { int o = __offset(8); return o != 0 ? bb.getInt(o + bb_pos) : 5; } - public boolean updatePortOnRemote() { int o = __offset(10); return o != 0 ? 0!=bb.get(o + bb_pos) : false; } - /** - * Set the port ID of the socket at the controller side. - * Note: Do not change this port ID, unless your application requires different port IDs on both ends of the FRI channel. - * For changing the FRI port ID on both sides, it is sufficient to call setPortOnRemote(int). - * Values of controllerPortID: - * "-1" - The configuration of setPortOnRemote(int) is used. This is the default. - * recommended range of port IDs: 30200 <= controllerPortID < 30210 - */ - public short portOnRemote() { int o = __offset(12); return o != 0 ? bb.getShort(o + bb_pos) : 0; } - public boolean updatePortOnController() { int o = __offset(14); return o != 0 ? 0!=bb.get(o + bb_pos) : false; } - /** - * Set the port ID of the FRI channel at the remote side. - * By default, this port ID is used on both sides of the FRI channel, unless specified otherwise by setPortOnController(int). - * - * Values of portID: - * - * default port ID: 30200 - * recommended range of port IDs: 30200 <= portID < 30210 - * Since the FRI channel utilizes UDP as connection layer, make sure, that your network topology (firewall, network services) are chosen accordingly. - * - * Parameters: - * portID - the port ID > 0 (also known as UDP port number) - */ - public short portOnController() { int o = __offset(16); return o != 0 ? bb.getShort(o + bb_pos) : 0; } - - public static int createFRI(FlatBufferBuilder builder, - byte overlayType, - int sendPeriodMillisec, - int setReceiveMultiplier, - boolean updatePortOnRemote, - short portOnRemote, - boolean updatePortOnController, - short portOnController) { - builder.startObject(7); - FRI.addSetReceiveMultiplier(builder, setReceiveMultiplier); - FRI.addSendPeriodMillisec(builder, sendPeriodMillisec); - FRI.addPortOnController(builder, portOnController); - FRI.addPortOnRemote(builder, portOnRemote); - FRI.addUpdatePortOnController(builder, updatePortOnController); - FRI.addUpdatePortOnRemote(builder, updatePortOnRemote); - FRI.addOverlayType(builder, overlayType); - return FRI.endFRI(builder); - } - - public static void startFRI(FlatBufferBuilder builder) { builder.startObject(7); } - public static void addOverlayType(FlatBufferBuilder builder, byte overlayType) { builder.addByte(0, overlayType, 1); } - public static void addSendPeriodMillisec(FlatBufferBuilder builder, int sendPeriodMillisec) { builder.addInt(1, sendPeriodMillisec, 4); } - public static void addSetReceiveMultiplier(FlatBufferBuilder builder, int setReceiveMultiplier) { builder.addInt(2, setReceiveMultiplier, 5); } - public static void addUpdatePortOnRemote(FlatBufferBuilder builder, boolean updatePortOnRemote) { builder.addBoolean(3, updatePortOnRemote, false); } - public static void addPortOnRemote(FlatBufferBuilder builder, short portOnRemote) { builder.addShort(4, portOnRemote, 0); } - public static void addUpdatePortOnController(FlatBufferBuilder builder, boolean updatePortOnController) { builder.addBoolean(5, updatePortOnController, false); } - public static void addPortOnController(FlatBufferBuilder builder, short portOnController) { builder.addShort(6, portOnController, 0); } - public static int endFRI(FlatBufferBuilder builder) { - int o = builder.endObject(); - return o; - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/FRI.py b/include/grl/flatbuffer/grl/flatbuffer/FRI.py deleted file mode 100644 index 96844c6..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/FRI.py +++ /dev/null @@ -1,110 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class FRI(object): - __slots__ = ['_tab'] - - @classmethod - def GetRootAsFRI(cls, buf, offset): - n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) - x = FRI() - x.Init(buf, n + offset) - return x - - # FRI - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - - # FRI - def OverlayType(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Int8Flags, o + self._tab.Pos) - return 1 - -# /// Set the value for the send period of the connection from the KUKA controller to the remote side in [ms]. -# /// This means, the KUKA controller will send cyclic FRI messages every sendPeriod milliseconds to the remote side. -# /// -# /// -# /// Parameters: -# /// sendPeriod - the send period in milliseconds, 1 <= sendPeriod <= 100. -# /// Note: The recommended value for good performance should be between 1-5 milliseconds. - # FRI - def SendPeriodMillisec(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Int32Flags, o + self._tab.Pos) - return 4 - -# /// Set the receive multiplier of the cycle time from the remote side to the KUKA controller. -# /// This multiplier defines the value of the receivePeriod which is calculated: -# /// receivePeriod = receiveMultiplier * sendPeriod -# /// -# /// The KUKA controller will expect a FRI response message every receivePeriod milliseconds from the remote side. -# /// -# /// The receivePeriod has to be within the range of: -# /// 1 <= receivePeriod <= 100. - # FRI - def SetReceiveMultiplier(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Int32Flags, o + self._tab.Pos) - return 5 - - # FRI - def UpdatePortOnRemote(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.BoolFlags, o + self._tab.Pos) - return 0 - -# /// Set the port ID of the socket at the controller side. -# /// Note: Do not change this port ID, unless your application requires different port IDs on both ends of the FRI channel. -# /// For changing the FRI port ID on both sides, it is sufficient to call setPortOnRemote(int). -# /// Values of controllerPortID: -# /// "-1" - The configuration of setPortOnRemote(int) is used. This is the default. -# /// recommended range of port IDs: 30200 <= controllerPortID < 30210 - # FRI - def PortOnRemote(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(12)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Int16Flags, o + self._tab.Pos) - return 0 - - # FRI - def UpdatePortOnController(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(14)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.BoolFlags, o + self._tab.Pos) - return 0 - -# /// Set the port ID of the FRI channel at the remote side. -# /// By default, this port ID is used on both sides of the FRI channel, unless specified otherwise by setPortOnController(int). -# /// -# /// Values of portID: -# /// -# /// default port ID: 30200 -# /// recommended range of port IDs: 30200 <= portID < 30210 -# /// Since the FRI channel utilizes UDP as connection layer, make sure, that your network topology (firewall, network services) are chosen accordingly. -# /// -# /// Parameters: -# /// portID - the port ID > 0 (also known as UDP port number) - # FRI - def PortOnController(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(16)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Int16Flags, o + self._tab.Pos) - return 0 - -def FRIStart(builder): builder.StartObject(7) -def FRIAddOverlayType(builder, overlayType): builder.PrependInt8Slot(0, overlayType, 1) -def FRIAddSendPeriodMillisec(builder, sendPeriodMillisec): builder.PrependInt32Slot(1, sendPeriodMillisec, 4) -def FRIAddSetReceiveMultiplier(builder, setReceiveMultiplier): builder.PrependInt32Slot(2, setReceiveMultiplier, 5) -def FRIAddUpdatePortOnRemote(builder, updatePortOnRemote): builder.PrependBoolSlot(3, updatePortOnRemote, 0) -def FRIAddPortOnRemote(builder, portOnRemote): builder.PrependInt16Slot(4, portOnRemote, 0) -def FRIAddUpdatePortOnController(builder, updatePortOnController): builder.PrependBoolSlot(5, updatePortOnController, 0) -def FRIAddPortOnController(builder, portOnController): builder.PrependInt16Slot(6, portOnController, 0) -def FRIEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/FRIMessageLog.java b/include/grl/flatbuffer/grl/flatbuffer/FRIMessageLog.java deleted file mode 100644 index 3268eb8..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/FRIMessageLog.java +++ /dev/null @@ -1,112 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class FRIMessageLog extends Table { - public static FRIMessageLog getRootAsFRIMessageLog(ByteBuffer _bb) { return getRootAsFRIMessageLog(_bb, new FRIMessageLog()); } - public static FRIMessageLog getRootAsFRIMessageLog(ByteBuffer _bb, FRIMessageLog obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public FRIMessageLog __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - public byte sessionState() { int o = __offset(4); return o != 0 ? bb.get(o + bb_pos) : 0; } - public byte connectionQuality() { int o = __offset(6); return o != 0 ? bb.get(o + bb_pos) : 0; } - public byte controlMode() { int o = __offset(8); return o != 0 ? bb.get(o + bb_pos) : 0; } - public int messageIdentifier() { int o = __offset(10); return o != 0 ? bb.getInt(o + bb_pos) : 0; } - public int sequenceCounter() { int o = __offset(12); return o != 0 ? bb.getInt(o + bb_pos) : 0; } - public int reflectedSequenceCounter() { int o = __offset(14); return o != 0 ? bb.getInt(o + bb_pos) : 0; } - public double measuredJointPosition(int j) { int o = __offset(16); return o != 0 ? bb.getDouble(__vector(o) + j * 8) : 0; } - public int measuredJointPositionLength() { int o = __offset(16); return o != 0 ? __vector_len(o) : 0; } - public ByteBuffer measuredJointPositionAsByteBuffer() { return __vector_as_bytebuffer(16, 8); } - public ByteBuffer measuredJointPositionInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 16, 8); } - public double measuredTorque(int j) { int o = __offset(18); return o != 0 ? bb.getDouble(__vector(o) + j * 8) : 0; } - public int measuredTorqueLength() { int o = __offset(18); return o != 0 ? __vector_len(o) : 0; } - public ByteBuffer measuredTorqueAsByteBuffer() { return __vector_as_bytebuffer(18, 8); } - public ByteBuffer measuredTorqueInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 18, 8); } - public double commandedJointPosition(int j) { int o = __offset(20); return o != 0 ? bb.getDouble(__vector(o) + j * 8) : 0; } - public int commandedJointPositionLength() { int o = __offset(20); return o != 0 ? __vector_len(o) : 0; } - public ByteBuffer commandedJointPositionAsByteBuffer() { return __vector_as_bytebuffer(20, 8); } - public ByteBuffer commandedJointPositionInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 20, 8); } - public double commandedTorque(int j) { int o = __offset(22); return o != 0 ? bb.getDouble(__vector(o) + j * 8) : 0; } - public int commandedTorqueLength() { int o = __offset(22); return o != 0 ? __vector_len(o) : 0; } - public ByteBuffer commandedTorqueAsByteBuffer() { return __vector_as_bytebuffer(22, 8); } - public ByteBuffer commandedTorqueInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 22, 8); } - public double externalTorque(int j) { int o = __offset(24); return o != 0 ? bb.getDouble(__vector(o) + j * 8) : 0; } - public int externalTorqueLength() { int o = __offset(24); return o != 0 ? __vector_len(o) : 0; } - public ByteBuffer externalTorqueAsByteBuffer() { return __vector_as_bytebuffer(24, 8); } - public ByteBuffer externalTorqueInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 24, 8); } - public double jointStateInterpolated(int j) { int o = __offset(26); return o != 0 ? bb.getDouble(__vector(o) + j * 8) : 0; } - public int jointStateInterpolatedLength() { int o = __offset(26); return o != 0 ? __vector_len(o) : 0; } - public ByteBuffer jointStateInterpolatedAsByteBuffer() { return __vector_as_bytebuffer(26, 8); } - public ByteBuffer jointStateInterpolatedInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 26, 8); } - public TimeEvent timeStamp() { return timeStamp(new TimeEvent()); } - public TimeEvent timeStamp(TimeEvent obj) { int o = __offset(28); return o != 0 ? obj.__assign(__indirect(o + bb_pos), bb) : null; } - - public static int createFRIMessageLog(FlatBufferBuilder builder, - byte sessionState, - byte connectionQuality, - byte controlMode, - int messageIdentifier, - int sequenceCounter, - int reflectedSequenceCounter, - int measuredJointPositionOffset, - int measuredTorqueOffset, - int commandedJointPositionOffset, - int commandedTorqueOffset, - int externalTorqueOffset, - int jointStateInterpolatedOffset, - int timeStampOffset) { - builder.startObject(13); - FRIMessageLog.addTimeStamp(builder, timeStampOffset); - FRIMessageLog.addJointStateInterpolated(builder, jointStateInterpolatedOffset); - FRIMessageLog.addExternalTorque(builder, externalTorqueOffset); - FRIMessageLog.addCommandedTorque(builder, commandedTorqueOffset); - FRIMessageLog.addCommandedJointPosition(builder, commandedJointPositionOffset); - FRIMessageLog.addMeasuredTorque(builder, measuredTorqueOffset); - FRIMessageLog.addMeasuredJointPosition(builder, measuredJointPositionOffset); - FRIMessageLog.addReflectedSequenceCounter(builder, reflectedSequenceCounter); - FRIMessageLog.addSequenceCounter(builder, sequenceCounter); - FRIMessageLog.addMessageIdentifier(builder, messageIdentifier); - FRIMessageLog.addControlMode(builder, controlMode); - FRIMessageLog.addConnectionQuality(builder, connectionQuality); - FRIMessageLog.addSessionState(builder, sessionState); - return FRIMessageLog.endFRIMessageLog(builder); - } - - public static void startFRIMessageLog(FlatBufferBuilder builder) { builder.startObject(13); } - public static void addSessionState(FlatBufferBuilder builder, byte sessionState) { builder.addByte(0, sessionState, 0); } - public static void addConnectionQuality(FlatBufferBuilder builder, byte connectionQuality) { builder.addByte(1, connectionQuality, 0); } - public static void addControlMode(FlatBufferBuilder builder, byte controlMode) { builder.addByte(2, controlMode, 0); } - public static void addMessageIdentifier(FlatBufferBuilder builder, int messageIdentifier) { builder.addInt(3, messageIdentifier, 0); } - public static void addSequenceCounter(FlatBufferBuilder builder, int sequenceCounter) { builder.addInt(4, sequenceCounter, 0); } - public static void addReflectedSequenceCounter(FlatBufferBuilder builder, int reflectedSequenceCounter) { builder.addInt(5, reflectedSequenceCounter, 0); } - public static void addMeasuredJointPosition(FlatBufferBuilder builder, int measuredJointPositionOffset) { builder.addOffset(6, measuredJointPositionOffset, 0); } - public static int createMeasuredJointPositionVector(FlatBufferBuilder builder, double[] data) { builder.startVector(8, data.length, 8); for (int i = data.length - 1; i >= 0; i--) builder.addDouble(data[i]); return builder.endVector(); } - public static void startMeasuredJointPositionVector(FlatBufferBuilder builder, int numElems) { builder.startVector(8, numElems, 8); } - public static void addMeasuredTorque(FlatBufferBuilder builder, int measuredTorqueOffset) { builder.addOffset(7, measuredTorqueOffset, 0); } - public static int createMeasuredTorqueVector(FlatBufferBuilder builder, double[] data) { builder.startVector(8, data.length, 8); for (int i = data.length - 1; i >= 0; i--) builder.addDouble(data[i]); return builder.endVector(); } - public static void startMeasuredTorqueVector(FlatBufferBuilder builder, int numElems) { builder.startVector(8, numElems, 8); } - public static void addCommandedJointPosition(FlatBufferBuilder builder, int commandedJointPositionOffset) { builder.addOffset(8, commandedJointPositionOffset, 0); } - public static int createCommandedJointPositionVector(FlatBufferBuilder builder, double[] data) { builder.startVector(8, data.length, 8); for (int i = data.length - 1; i >= 0; i--) builder.addDouble(data[i]); return builder.endVector(); } - public static void startCommandedJointPositionVector(FlatBufferBuilder builder, int numElems) { builder.startVector(8, numElems, 8); } - public static void addCommandedTorque(FlatBufferBuilder builder, int commandedTorqueOffset) { builder.addOffset(9, commandedTorqueOffset, 0); } - public static int createCommandedTorqueVector(FlatBufferBuilder builder, double[] data) { builder.startVector(8, data.length, 8); for (int i = data.length - 1; i >= 0; i--) builder.addDouble(data[i]); return builder.endVector(); } - public static void startCommandedTorqueVector(FlatBufferBuilder builder, int numElems) { builder.startVector(8, numElems, 8); } - public static void addExternalTorque(FlatBufferBuilder builder, int externalTorqueOffset) { builder.addOffset(10, externalTorqueOffset, 0); } - public static int createExternalTorqueVector(FlatBufferBuilder builder, double[] data) { builder.startVector(8, data.length, 8); for (int i = data.length - 1; i >= 0; i--) builder.addDouble(data[i]); return builder.endVector(); } - public static void startExternalTorqueVector(FlatBufferBuilder builder, int numElems) { builder.startVector(8, numElems, 8); } - public static void addJointStateInterpolated(FlatBufferBuilder builder, int jointStateInterpolatedOffset) { builder.addOffset(11, jointStateInterpolatedOffset, 0); } - public static int createJointStateInterpolatedVector(FlatBufferBuilder builder, double[] data) { builder.startVector(8, data.length, 8); for (int i = data.length - 1; i >= 0; i--) builder.addDouble(data[i]); return builder.endVector(); } - public static void startJointStateInterpolatedVector(FlatBufferBuilder builder, int numElems) { builder.startVector(8, numElems, 8); } - public static void addTimeStamp(FlatBufferBuilder builder, int timeStampOffset) { builder.addOffset(12, timeStampOffset, 0); } - public static int endFRIMessageLog(FlatBufferBuilder builder) { - int o = builder.endObject(); - return o; - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/FRIMessageLog.py b/include/grl/flatbuffer/grl/flatbuffer/FRIMessageLog.py deleted file mode 100644 index c8b9336..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/FRIMessageLog.py +++ /dev/null @@ -1,226 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class FRIMessageLog(object): - __slots__ = ['_tab'] - - @classmethod - def GetRootAsFRIMessageLog(cls, buf, offset): - n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) - x = FRIMessageLog() - x.Init(buf, n + offset) - return x - - # FRIMessageLog - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - - # FRIMessageLog - def SessionState(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Int8Flags, o + self._tab.Pos) - return 0 - - # FRIMessageLog - def ConnectionQuality(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Int8Flags, o + self._tab.Pos) - return 0 - - # FRIMessageLog - def ControlMode(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Int8Flags, o + self._tab.Pos) - return 0 - - # FRIMessageLog - def MessageIdentifier(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Int32Flags, o + self._tab.Pos) - return 0 - - # FRIMessageLog - def SequenceCounter(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(12)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Int32Flags, o + self._tab.Pos) - return 0 - - # FRIMessageLog - def ReflectedSequenceCounter(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(14)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Int32Flags, o + self._tab.Pos) - return 0 - - # FRIMessageLog - def MeasuredJointPosition(self, j): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(16)) - if o != 0: - a = self._tab.Vector(o) - return self._tab.Get(flatbuffers.number_types.Float64Flags, a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 8)) - return 0 - - # FRIMessageLog - def MeasuredJointPositionAsNumpy(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(16)) - if o != 0: - return self._tab.GetVectorAsNumpy(flatbuffers.number_types.Float64Flags, o) - return 0 - - # FRIMessageLog - def MeasuredJointPositionLength(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(16)) - if o != 0: - return self._tab.VectorLen(o) - return 0 - - # FRIMessageLog - def MeasuredTorque(self, j): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(18)) - if o != 0: - a = self._tab.Vector(o) - return self._tab.Get(flatbuffers.number_types.Float64Flags, a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 8)) - return 0 - - # FRIMessageLog - def MeasuredTorqueAsNumpy(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(18)) - if o != 0: - return self._tab.GetVectorAsNumpy(flatbuffers.number_types.Float64Flags, o) - return 0 - - # FRIMessageLog - def MeasuredTorqueLength(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(18)) - if o != 0: - return self._tab.VectorLen(o) - return 0 - - # FRIMessageLog - def CommandedJointPosition(self, j): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(20)) - if o != 0: - a = self._tab.Vector(o) - return self._tab.Get(flatbuffers.number_types.Float64Flags, a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 8)) - return 0 - - # FRIMessageLog - def CommandedJointPositionAsNumpy(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(20)) - if o != 0: - return self._tab.GetVectorAsNumpy(flatbuffers.number_types.Float64Flags, o) - return 0 - - # FRIMessageLog - def CommandedJointPositionLength(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(20)) - if o != 0: - return self._tab.VectorLen(o) - return 0 - - # FRIMessageLog - def CommandedTorque(self, j): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(22)) - if o != 0: - a = self._tab.Vector(o) - return self._tab.Get(flatbuffers.number_types.Float64Flags, a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 8)) - return 0 - - # FRIMessageLog - def CommandedTorqueAsNumpy(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(22)) - if o != 0: - return self._tab.GetVectorAsNumpy(flatbuffers.number_types.Float64Flags, o) - return 0 - - # FRIMessageLog - def CommandedTorqueLength(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(22)) - if o != 0: - return self._tab.VectorLen(o) - return 0 - - # FRIMessageLog - def ExternalTorque(self, j): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(24)) - if o != 0: - a = self._tab.Vector(o) - return self._tab.Get(flatbuffers.number_types.Float64Flags, a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 8)) - return 0 - - # FRIMessageLog - def ExternalTorqueAsNumpy(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(24)) - if o != 0: - return self._tab.GetVectorAsNumpy(flatbuffers.number_types.Float64Flags, o) - return 0 - - # FRIMessageLog - def ExternalTorqueLength(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(24)) - if o != 0: - return self._tab.VectorLen(o) - return 0 - - # FRIMessageLog - def JointStateInterpolated(self, j): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(26)) - if o != 0: - a = self._tab.Vector(o) - return self._tab.Get(flatbuffers.number_types.Float64Flags, a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 8)) - return 0 - - # FRIMessageLog - def JointStateInterpolatedAsNumpy(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(26)) - if o != 0: - return self._tab.GetVectorAsNumpy(flatbuffers.number_types.Float64Flags, o) - return 0 - - # FRIMessageLog - def JointStateInterpolatedLength(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(26)) - if o != 0: - return self._tab.VectorLen(o) - return 0 - - # FRIMessageLog - def TimeStamp(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(28)) - if o != 0: - x = self._tab.Indirect(o + self._tab.Pos) - from .TimeEvent import TimeEvent - obj = TimeEvent() - obj.Init(self._tab.Bytes, x) - return obj - return None - -def FRIMessageLogStart(builder): builder.StartObject(13) -def FRIMessageLogAddSessionState(builder, sessionState): builder.PrependInt8Slot(0, sessionState, 0) -def FRIMessageLogAddConnectionQuality(builder, connectionQuality): builder.PrependInt8Slot(1, connectionQuality, 0) -def FRIMessageLogAddControlMode(builder, controlMode): builder.PrependInt8Slot(2, controlMode, 0) -def FRIMessageLogAddMessageIdentifier(builder, messageIdentifier): builder.PrependInt32Slot(3, messageIdentifier, 0) -def FRIMessageLogAddSequenceCounter(builder, sequenceCounter): builder.PrependInt32Slot(4, sequenceCounter, 0) -def FRIMessageLogAddReflectedSequenceCounter(builder, reflectedSequenceCounter): builder.PrependInt32Slot(5, reflectedSequenceCounter, 0) -def FRIMessageLogAddMeasuredJointPosition(builder, measuredJointPosition): builder.PrependUOffsetTRelativeSlot(6, flatbuffers.number_types.UOffsetTFlags.py_type(measuredJointPosition), 0) -def FRIMessageLogStartMeasuredJointPositionVector(builder, numElems): return builder.StartVector(8, numElems, 8) -def FRIMessageLogAddMeasuredTorque(builder, measuredTorque): builder.PrependUOffsetTRelativeSlot(7, flatbuffers.number_types.UOffsetTFlags.py_type(measuredTorque), 0) -def FRIMessageLogStartMeasuredTorqueVector(builder, numElems): return builder.StartVector(8, numElems, 8) -def FRIMessageLogAddCommandedJointPosition(builder, commandedJointPosition): builder.PrependUOffsetTRelativeSlot(8, flatbuffers.number_types.UOffsetTFlags.py_type(commandedJointPosition), 0) -def FRIMessageLogStartCommandedJointPositionVector(builder, numElems): return builder.StartVector(8, numElems, 8) -def FRIMessageLogAddCommandedTorque(builder, commandedTorque): builder.PrependUOffsetTRelativeSlot(9, flatbuffers.number_types.UOffsetTFlags.py_type(commandedTorque), 0) -def FRIMessageLogStartCommandedTorqueVector(builder, numElems): return builder.StartVector(8, numElems, 8) -def FRIMessageLogAddExternalTorque(builder, externalTorque): builder.PrependUOffsetTRelativeSlot(10, flatbuffers.number_types.UOffsetTFlags.py_type(externalTorque), 0) -def FRIMessageLogStartExternalTorqueVector(builder, numElems): return builder.StartVector(8, numElems, 8) -def FRIMessageLogAddJointStateInterpolated(builder, jointStateInterpolated): builder.PrependUOffsetTRelativeSlot(11, flatbuffers.number_types.UOffsetTFlags.py_type(jointStateInterpolated), 0) -def FRIMessageLogStartJointStateInterpolatedVector(builder, numElems): return builder.StartVector(8, numElems, 8) -def FRIMessageLogAddTimeStamp(builder, timeStamp): builder.PrependUOffsetTRelativeSlot(12, flatbuffers.number_types.UOffsetTFlags.py_type(timeStamp), 0) -def FRIMessageLogEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/FRITimeStamp.java b/include/grl/flatbuffer/grl/flatbuffer/FRITimeStamp.java deleted file mode 100644 index 8604f8c..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/FRITimeStamp.java +++ /dev/null @@ -1,37 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class FRITimeStamp extends Table { - public static FRITimeStamp getRootAsFRITimeStamp(ByteBuffer _bb) { return getRootAsFRITimeStamp(_bb, new FRITimeStamp()); } - public static FRITimeStamp getRootAsFRITimeStamp(ByteBuffer _bb, FRITimeStamp obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public FRITimeStamp __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - public int sec() { int o = __offset(4); return o != 0 ? bb.getInt(o + bb_pos) : 0; } - public int nanosec() { int o = __offset(6); return o != 0 ? bb.getInt(o + bb_pos) : 0; } - - public static int createFRITimeStamp(FlatBufferBuilder builder, - int sec, - int nanosec) { - builder.startObject(2); - FRITimeStamp.addNanosec(builder, nanosec); - FRITimeStamp.addSec(builder, sec); - return FRITimeStamp.endFRITimeStamp(builder); - } - - public static void startFRITimeStamp(FlatBufferBuilder builder) { builder.startObject(2); } - public static void addSec(FlatBufferBuilder builder, int sec) { builder.addInt(0, sec, 0); } - public static void addNanosec(FlatBufferBuilder builder, int nanosec) { builder.addInt(1, nanosec, 0); } - public static int endFRITimeStamp(FlatBufferBuilder builder) { - int o = builder.endObject(); - return o; - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/FRITimeStamp.py b/include/grl/flatbuffer/grl/flatbuffer/FRITimeStamp.py deleted file mode 100644 index 1b7c07e..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/FRITimeStamp.py +++ /dev/null @@ -1,38 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class FRITimeStamp(object): - __slots__ = ['_tab'] - - @classmethod - def GetRootAsFRITimeStamp(cls, buf, offset): - n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) - x = FRITimeStamp() - x.Init(buf, n + offset) - return x - - # FRITimeStamp - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - - # FRITimeStamp - def Sec(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Int32Flags, o + self._tab.Pos) - return 0 - - # FRITimeStamp - def Nanosec(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Int32Flags, o + self._tab.Pos) - return 0 - -def FRITimeStampStart(builder): builder.StartObject(2) -def FRITimeStampAddSec(builder, sec): builder.PrependInt32Slot(0, sec, 0) -def FRITimeStampAddNanosec(builder, nanosec): builder.PrependInt32Slot(1, nanosec, 0) -def FRITimeStampEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/FusionTrackFrame.java b/include/grl/flatbuffer/grl/flatbuffer/FusionTrackFrame.java deleted file mode 100644 index 58548e6..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/FusionTrackFrame.java +++ /dev/null @@ -1,179 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -/** - * Data for one frame capture - * On the Atracsys FusionTrack optical tracker - * look at ftkInterface.h for details - * Frame class is defined in FusionTrack.hpp - */ -public final class FusionTrackFrame extends Table { - public static FusionTrackFrame getRootAsFusionTrackFrame(ByteBuffer _bb) { return getRootAsFusionTrackFrame(_bb, new FusionTrackFrame()); } - public static FusionTrackFrame getRootAsFusionTrackFrame(ByteBuffer _bb, FusionTrackFrame obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public FusionTrackFrame __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - public double timestamp() { int o = __offset(4); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } - public long serialNumber() { int o = __offset(6); return o != 0 ? bb.getLong(o + bb_pos) : 0L; } - public long hardwareTimestampUS() { int o = __offset(8); return o != 0 ? bb.getLong(o + bb_pos) : 0L; } - public long desynchroUS() { int o = __offset(10); return o != 0 ? bb.getLong(o + bb_pos) : 0L; } - public long counter() { int o = __offset(12); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } - public long format() { int o = __offset(14); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } - public long width() { int o = __offset(16); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } - public long height() { int o = __offset(18); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } - public int imageStrideInBytes() { int o = __offset(20); return o != 0 ? bb.getInt(o + bb_pos) : 0; } - public long imageHeaderVersion() { int o = __offset(22); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } - public int imageHeaderStatus() { int o = __offset(24); return o != 0 ? bb.getInt(o + bb_pos) : 0; } - public String imageLeftPixels() { int o = __offset(26); return o != 0 ? __string(o + bb_pos) : null; } - public ByteBuffer imageLeftPixelsAsByteBuffer() { return __vector_as_bytebuffer(26, 1); } - public ByteBuffer imageLeftPixelsInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 26, 1); } - public long imageLeftPixelsVersion() { int o = __offset(28); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } - public int imageLeftStatus() { int o = __offset(30); return o != 0 ? bb.getInt(o + bb_pos) : 0; } - public String imageRightPixels() { int o = __offset(32); return o != 0 ? __string(o + bb_pos) : null; } - public ByteBuffer imageRightPixelsAsByteBuffer() { return __vector_as_bytebuffer(32, 1); } - public ByteBuffer imageRightPixelsInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 32, 1); } - public long imageRightPixelsVersion() { int o = __offset(34); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } - public int imageRightStatus() { int o = __offset(36); return o != 0 ? bb.getInt(o + bb_pos) : 0; } - public ftkRegionOfInterest regionsOfInterestLeft(int j) { return regionsOfInterestLeft(new ftkRegionOfInterest(), j); } - public ftkRegionOfInterest regionsOfInterestLeft(ftkRegionOfInterest obj, int j) { int o = __offset(38); return o != 0 ? obj.__assign(__indirect(__vector(o) + j * 4), bb) : null; } - public int regionsOfInterestLeftLength() { int o = __offset(38); return o != 0 ? __vector_len(o) : 0; } - public long regionsOfInterestLeftVersion() { int o = __offset(40); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } - public int regionsOfInterestLeftStatus() { int o = __offset(42); return o != 0 ? bb.getInt(o + bb_pos) : 0; } - public ftkRegionOfInterest regionsOfInterestRight(int j) { return regionsOfInterestRight(new ftkRegionOfInterest(), j); } - public ftkRegionOfInterest regionsOfInterestRight(ftkRegionOfInterest obj, int j) { int o = __offset(44); return o != 0 ? obj.__assign(__indirect(__vector(o) + j * 4), bb) : null; } - public int regionsOfInterestRightLength() { int o = __offset(44); return o != 0 ? __vector_len(o) : 0; } - public long regionsOfInterestRightVersion() { int o = __offset(46); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } - public int regionsOfInterestRightStatus() { int o = __offset(48); return o != 0 ? bb.getInt(o + bb_pos) : 0; } - public ftk3DFiducial threeDFiducials(int j) { return threeDFiducials(new ftk3DFiducial(), j); } - public ftk3DFiducial threeDFiducials(ftk3DFiducial obj, int j) { int o = __offset(50); return o != 0 ? obj.__assign(__indirect(__vector(o) + j * 4), bb) : null; } - public int threeDFiducialsLength() { int o = __offset(50); return o != 0 ? __vector_len(o) : 0; } - public long threeDFiducialsVersion() { int o = __offset(52); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } - public int threeDFiducialsStatus() { int o = __offset(54); return o != 0 ? bb.getInt(o + bb_pos) : 0; } - public ftkMarker markers(int j) { return markers(new ftkMarker(), j); } - public ftkMarker markers(ftkMarker obj, int j) { int o = __offset(56); return o != 0 ? obj.__assign(__indirect(__vector(o) + j * 4), bb) : null; } - public int markersLength() { int o = __offset(56); return o != 0 ? __vector_len(o) : 0; } - public long markersVersion() { int o = __offset(58); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } - public int markersStatus() { int o = __offset(60); return o != 0 ? bb.getInt(o + bb_pos) : 0; } - public int deviceType() { int o = __offset(62); return o != 0 ? bb.getInt(o + bb_pos) : 0; } - public long ftkError() { int o = __offset(64); return o != 0 ? bb.getLong(o + bb_pos) : 0L; } - - public static int createFusionTrackFrame(FlatBufferBuilder builder, - double timestamp, - long serialNumber, - long hardwareTimestampUS, - long desynchroUS, - long counter, - long format, - long width, - long height, - int imageStrideInBytes, - long imageHeaderVersion, - int imageHeaderStatus, - int imageLeftPixelsOffset, - long imageLeftPixelsVersion, - int imageLeftStatus, - int imageRightPixelsOffset, - long imageRightPixelsVersion, - int imageRightStatus, - int regionsOfInterestLeftOffset, - long regionsOfInterestLeftVersion, - int regionsOfInterestLeftStatus, - int regionsOfInterestRightOffset, - long regionsOfInterestRightVersion, - int regionsOfInterestRightStatus, - int threeDFiducialsOffset, - long threeDFiducialsVersion, - int threeDFiducialsStatus, - int markersOffset, - long markersVersion, - int markersStatus, - int deviceType, - long ftkError) { - builder.startObject(31); - FusionTrackFrame.addFtkError(builder, ftkError); - FusionTrackFrame.addDesynchroUS(builder, desynchroUS); - FusionTrackFrame.addHardwareTimestampUS(builder, hardwareTimestampUS); - FusionTrackFrame.addSerialNumber(builder, serialNumber); - FusionTrackFrame.addTimestamp(builder, timestamp); - FusionTrackFrame.addDeviceType(builder, deviceType); - FusionTrackFrame.addMarkersStatus(builder, markersStatus); - FusionTrackFrame.addMarkersVersion(builder, markersVersion); - FusionTrackFrame.addMarkers(builder, markersOffset); - FusionTrackFrame.addThreeDFiducialsStatus(builder, threeDFiducialsStatus); - FusionTrackFrame.addThreeDFiducialsVersion(builder, threeDFiducialsVersion); - FusionTrackFrame.addThreeDFiducials(builder, threeDFiducialsOffset); - FusionTrackFrame.addRegionsOfInterestRightStatus(builder, regionsOfInterestRightStatus); - FusionTrackFrame.addRegionsOfInterestRightVersion(builder, regionsOfInterestRightVersion); - FusionTrackFrame.addRegionsOfInterestRight(builder, regionsOfInterestRightOffset); - FusionTrackFrame.addRegionsOfInterestLeftStatus(builder, regionsOfInterestLeftStatus); - FusionTrackFrame.addRegionsOfInterestLeftVersion(builder, regionsOfInterestLeftVersion); - FusionTrackFrame.addRegionsOfInterestLeft(builder, regionsOfInterestLeftOffset); - FusionTrackFrame.addImageRightStatus(builder, imageRightStatus); - FusionTrackFrame.addImageRightPixelsVersion(builder, imageRightPixelsVersion); - FusionTrackFrame.addImageRightPixels(builder, imageRightPixelsOffset); - FusionTrackFrame.addImageLeftStatus(builder, imageLeftStatus); - FusionTrackFrame.addImageLeftPixelsVersion(builder, imageLeftPixelsVersion); - FusionTrackFrame.addImageLeftPixels(builder, imageLeftPixelsOffset); - FusionTrackFrame.addImageHeaderStatus(builder, imageHeaderStatus); - FusionTrackFrame.addImageHeaderVersion(builder, imageHeaderVersion); - FusionTrackFrame.addImageStrideInBytes(builder, imageStrideInBytes); - FusionTrackFrame.addHeight(builder, height); - FusionTrackFrame.addWidth(builder, width); - FusionTrackFrame.addFormat(builder, format); - FusionTrackFrame.addCounter(builder, counter); - return FusionTrackFrame.endFusionTrackFrame(builder); - } - - public static void startFusionTrackFrame(FlatBufferBuilder builder) { builder.startObject(31); } - public static void addTimestamp(FlatBufferBuilder builder, double timestamp) { builder.addDouble(0, timestamp, 0.0); } - public static void addSerialNumber(FlatBufferBuilder builder, long serialNumber) { builder.addLong(1, serialNumber, 0L); } - public static void addHardwareTimestampUS(FlatBufferBuilder builder, long hardwareTimestampUS) { builder.addLong(2, hardwareTimestampUS, 0L); } - public static void addDesynchroUS(FlatBufferBuilder builder, long desynchroUS) { builder.addLong(3, desynchroUS, 0L); } - public static void addCounter(FlatBufferBuilder builder, long counter) { builder.addInt(4, (int)counter, (int)0L); } - public static void addFormat(FlatBufferBuilder builder, long format) { builder.addInt(5, (int)format, (int)0L); } - public static void addWidth(FlatBufferBuilder builder, long width) { builder.addInt(6, (int)width, (int)0L); } - public static void addHeight(FlatBufferBuilder builder, long height) { builder.addInt(7, (int)height, (int)0L); } - public static void addImageStrideInBytes(FlatBufferBuilder builder, int imageStrideInBytes) { builder.addInt(8, imageStrideInBytes, 0); } - public static void addImageHeaderVersion(FlatBufferBuilder builder, long imageHeaderVersion) { builder.addInt(9, (int)imageHeaderVersion, (int)0L); } - public static void addImageHeaderStatus(FlatBufferBuilder builder, int imageHeaderStatus) { builder.addInt(10, imageHeaderStatus, 0); } - public static void addImageLeftPixels(FlatBufferBuilder builder, int imageLeftPixelsOffset) { builder.addOffset(11, imageLeftPixelsOffset, 0); } - public static void addImageLeftPixelsVersion(FlatBufferBuilder builder, long imageLeftPixelsVersion) { builder.addInt(12, (int)imageLeftPixelsVersion, (int)0L); } - public static void addImageLeftStatus(FlatBufferBuilder builder, int imageLeftStatus) { builder.addInt(13, imageLeftStatus, 0); } - public static void addImageRightPixels(FlatBufferBuilder builder, int imageRightPixelsOffset) { builder.addOffset(14, imageRightPixelsOffset, 0); } - public static void addImageRightPixelsVersion(FlatBufferBuilder builder, long imageRightPixelsVersion) { builder.addInt(15, (int)imageRightPixelsVersion, (int)0L); } - public static void addImageRightStatus(FlatBufferBuilder builder, int imageRightStatus) { builder.addInt(16, imageRightStatus, 0); } - public static void addRegionsOfInterestLeft(FlatBufferBuilder builder, int regionsOfInterestLeftOffset) { builder.addOffset(17, regionsOfInterestLeftOffset, 0); } - public static int createRegionsOfInterestLeftVector(FlatBufferBuilder builder, int[] data) { builder.startVector(4, data.length, 4); for (int i = data.length - 1; i >= 0; i--) builder.addOffset(data[i]); return builder.endVector(); } - public static void startRegionsOfInterestLeftVector(FlatBufferBuilder builder, int numElems) { builder.startVector(4, numElems, 4); } - public static void addRegionsOfInterestLeftVersion(FlatBufferBuilder builder, long regionsOfInterestLeftVersion) { builder.addInt(18, (int)regionsOfInterestLeftVersion, (int)0L); } - public static void addRegionsOfInterestLeftStatus(FlatBufferBuilder builder, int regionsOfInterestLeftStatus) { builder.addInt(19, regionsOfInterestLeftStatus, 0); } - public static void addRegionsOfInterestRight(FlatBufferBuilder builder, int regionsOfInterestRightOffset) { builder.addOffset(20, regionsOfInterestRightOffset, 0); } - public static int createRegionsOfInterestRightVector(FlatBufferBuilder builder, int[] data) { builder.startVector(4, data.length, 4); for (int i = data.length - 1; i >= 0; i--) builder.addOffset(data[i]); return builder.endVector(); } - public static void startRegionsOfInterestRightVector(FlatBufferBuilder builder, int numElems) { builder.startVector(4, numElems, 4); } - public static void addRegionsOfInterestRightVersion(FlatBufferBuilder builder, long regionsOfInterestRightVersion) { builder.addInt(21, (int)regionsOfInterestRightVersion, (int)0L); } - public static void addRegionsOfInterestRightStatus(FlatBufferBuilder builder, int regionsOfInterestRightStatus) { builder.addInt(22, regionsOfInterestRightStatus, 0); } - public static void addThreeDFiducials(FlatBufferBuilder builder, int threeDFiducialsOffset) { builder.addOffset(23, threeDFiducialsOffset, 0); } - public static int createThreeDFiducialsVector(FlatBufferBuilder builder, int[] data) { builder.startVector(4, data.length, 4); for (int i = data.length - 1; i >= 0; i--) builder.addOffset(data[i]); return builder.endVector(); } - public static void startThreeDFiducialsVector(FlatBufferBuilder builder, int numElems) { builder.startVector(4, numElems, 4); } - public static void addThreeDFiducialsVersion(FlatBufferBuilder builder, long threeDFiducialsVersion) { builder.addInt(24, (int)threeDFiducialsVersion, (int)0L); } - public static void addThreeDFiducialsStatus(FlatBufferBuilder builder, int threeDFiducialsStatus) { builder.addInt(25, threeDFiducialsStatus, 0); } - public static void addMarkers(FlatBufferBuilder builder, int markersOffset) { builder.addOffset(26, markersOffset, 0); } - public static int createMarkersVector(FlatBufferBuilder builder, int[] data) { builder.startVector(4, data.length, 4); for (int i = data.length - 1; i >= 0; i--) builder.addOffset(data[i]); return builder.endVector(); } - public static void startMarkersVector(FlatBufferBuilder builder, int numElems) { builder.startVector(4, numElems, 4); } - public static void addMarkersVersion(FlatBufferBuilder builder, long markersVersion) { builder.addInt(27, (int)markersVersion, (int)0L); } - public static void addMarkersStatus(FlatBufferBuilder builder, int markersStatus) { builder.addInt(28, markersStatus, 0); } - public static void addDeviceType(FlatBufferBuilder builder, int deviceType) { builder.addInt(29, deviceType, 0); } - public static void addFtkError(FlatBufferBuilder builder, long ftkError) { builder.addLong(30, ftkError, 0L); } - public static int endFusionTrackFrame(FlatBufferBuilder builder) { - int o = builder.endObject(); - return o; - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/FusionTrackFrame.py b/include/grl/flatbuffer/grl/flatbuffer/FusionTrackFrame.py deleted file mode 100644 index 9150bfd..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/FusionTrackFrame.py +++ /dev/null @@ -1,330 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -# /// Data for one frame capture -# /// On the Atracsys FusionTrack optical tracker -# /// look at ftkInterface.h for details -# /// Frame class is defined in FusionTrack.hpp -class FusionTrackFrame(object): - __slots__ = ['_tab'] - - @classmethod - def GetRootAsFusionTrackFrame(cls, buf, offset): - n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) - x = FusionTrackFrame() - x.Init(buf, n + offset) - return x - - # FusionTrackFrame - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - - # FusionTrackFrame - def Timestamp(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) - return 0.0 - - # FusionTrackFrame - def SerialNumber(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Uint64Flags, o + self._tab.Pos) - return 0 - - # FusionTrackFrame - def HardwareTimestampUS(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Uint64Flags, o + self._tab.Pos) - return 0 - - # FusionTrackFrame - def DesynchroUS(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Uint64Flags, o + self._tab.Pos) - return 0 - - # FusionTrackFrame - def Counter(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(12)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) - return 0 - - # FusionTrackFrame - def Format(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(14)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) - return 0 - - # FusionTrackFrame - def Width(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(16)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) - return 0 - - # FusionTrackFrame - def Height(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(18)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) - return 0 - - # FusionTrackFrame - def ImageStrideInBytes(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(20)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Int32Flags, o + self._tab.Pos) - return 0 - - # FusionTrackFrame - def ImageHeaderVersion(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(22)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) - return 0 - - # FusionTrackFrame - def ImageHeaderStatus(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(24)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Int32Flags, o + self._tab.Pos) - return 0 - - # FusionTrackFrame - def ImageLeftPixels(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(26)) - if o != 0: - return self._tab.String(o + self._tab.Pos) - return bytes() - - # FusionTrackFrame - def ImageLeftPixelsVersion(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(28)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) - return 0 - - # FusionTrackFrame - def ImageLeftStatus(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(30)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Int32Flags, o + self._tab.Pos) - return 0 - - # FusionTrackFrame - def ImageRightPixels(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(32)) - if o != 0: - return self._tab.String(o + self._tab.Pos) - return bytes() - - # FusionTrackFrame - def ImageRightPixelsVersion(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(34)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) - return 0 - - # FusionTrackFrame - def ImageRightStatus(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(36)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Int32Flags, o + self._tab.Pos) - return 0 - - # FusionTrackFrame - def RegionsOfInterestLeft(self, j): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(38)) - if o != 0: - x = self._tab.Vector(o) - x += flatbuffers.number_types.UOffsetTFlags.py_type(j) * 4 - x = self._tab.Indirect(x) - from .ftkRegionOfInterest import ftkRegionOfInterest - obj = ftkRegionOfInterest() - obj.Init(self._tab.Bytes, x) - return obj - return None - - # FusionTrackFrame - def RegionsOfInterestLeftLength(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(38)) - if o != 0: - return self._tab.VectorLen(o) - return 0 - - # FusionTrackFrame - def RegionsOfInterestLeftVersion(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(40)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) - return 0 - - # FusionTrackFrame - def RegionsOfInterestLeftStatus(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(42)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Int32Flags, o + self._tab.Pos) - return 0 - - # FusionTrackFrame - def RegionsOfInterestRight(self, j): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(44)) - if o != 0: - x = self._tab.Vector(o) - x += flatbuffers.number_types.UOffsetTFlags.py_type(j) * 4 - x = self._tab.Indirect(x) - from .ftkRegionOfInterest import ftkRegionOfInterest - obj = ftkRegionOfInterest() - obj.Init(self._tab.Bytes, x) - return obj - return None - - # FusionTrackFrame - def RegionsOfInterestRightLength(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(44)) - if o != 0: - return self._tab.VectorLen(o) - return 0 - - # FusionTrackFrame - def RegionsOfInterestRightVersion(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(46)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) - return 0 - - # FusionTrackFrame - def RegionsOfInterestRightStatus(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(48)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Int32Flags, o + self._tab.Pos) - return 0 - - # FusionTrackFrame - def ThreeDFiducials(self, j): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(50)) - if o != 0: - x = self._tab.Vector(o) - x += flatbuffers.number_types.UOffsetTFlags.py_type(j) * 4 - x = self._tab.Indirect(x) - from .ftk3DFiducial import ftk3DFiducial - obj = ftk3DFiducial() - obj.Init(self._tab.Bytes, x) - return obj - return None - - # FusionTrackFrame - def ThreeDFiducialsLength(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(50)) - if o != 0: - return self._tab.VectorLen(o) - return 0 - - # FusionTrackFrame - def ThreeDFiducialsVersion(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(52)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) - return 0 - - # FusionTrackFrame - def ThreeDFiducialsStatus(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(54)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Int32Flags, o + self._tab.Pos) - return 0 - - # FusionTrackFrame - def Markers(self, j): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(56)) - if o != 0: - x = self._tab.Vector(o) - x += flatbuffers.number_types.UOffsetTFlags.py_type(j) * 4 - x = self._tab.Indirect(x) - from .ftkMarker import ftkMarker - obj = ftkMarker() - obj.Init(self._tab.Bytes, x) - return obj - return None - - # FusionTrackFrame - def MarkersLength(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(56)) - if o != 0: - return self._tab.VectorLen(o) - return 0 - - # FusionTrackFrame - def MarkersVersion(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(58)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) - return 0 - - # FusionTrackFrame - def MarkersStatus(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(60)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Int32Flags, o + self._tab.Pos) - return 0 - - # FusionTrackFrame - def DeviceType(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(62)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Int32Flags, o + self._tab.Pos) - return 0 - - # FusionTrackFrame - def FtkError(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(64)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Int64Flags, o + self._tab.Pos) - return 0 - -def FusionTrackFrameStart(builder): builder.StartObject(31) -def FusionTrackFrameAddTimestamp(builder, timestamp): builder.PrependFloat64Slot(0, timestamp, 0.0) -def FusionTrackFrameAddSerialNumber(builder, serialNumber): builder.PrependUint64Slot(1, serialNumber, 0) -def FusionTrackFrameAddHardwareTimestampUS(builder, hardwareTimestampUS): builder.PrependUint64Slot(2, hardwareTimestampUS, 0) -def FusionTrackFrameAddDesynchroUS(builder, desynchroUS): builder.PrependUint64Slot(3, desynchroUS, 0) -def FusionTrackFrameAddCounter(builder, counter): builder.PrependUint32Slot(4, counter, 0) -def FusionTrackFrameAddFormat(builder, format): builder.PrependUint32Slot(5, format, 0) -def FusionTrackFrameAddWidth(builder, width): builder.PrependUint32Slot(6, width, 0) -def FusionTrackFrameAddHeight(builder, height): builder.PrependUint32Slot(7, height, 0) -def FusionTrackFrameAddImageStrideInBytes(builder, imageStrideInBytes): builder.PrependInt32Slot(8, imageStrideInBytes, 0) -def FusionTrackFrameAddImageHeaderVersion(builder, imageHeaderVersion): builder.PrependUint32Slot(9, imageHeaderVersion, 0) -def FusionTrackFrameAddImageHeaderStatus(builder, imageHeaderStatus): builder.PrependInt32Slot(10, imageHeaderStatus, 0) -def FusionTrackFrameAddImageLeftPixels(builder, imageLeftPixels): builder.PrependUOffsetTRelativeSlot(11, flatbuffers.number_types.UOffsetTFlags.py_type(imageLeftPixels), 0) -def FusionTrackFrameAddImageLeftPixelsVersion(builder, imageLeftPixelsVersion): builder.PrependUint32Slot(12, imageLeftPixelsVersion, 0) -def FusionTrackFrameAddImageLeftStatus(builder, imageLeftStatus): builder.PrependInt32Slot(13, imageLeftStatus, 0) -def FusionTrackFrameAddImageRightPixels(builder, imageRightPixels): builder.PrependUOffsetTRelativeSlot(14, flatbuffers.number_types.UOffsetTFlags.py_type(imageRightPixels), 0) -def FusionTrackFrameAddImageRightPixelsVersion(builder, imageRightPixelsVersion): builder.PrependUint32Slot(15, imageRightPixelsVersion, 0) -def FusionTrackFrameAddImageRightStatus(builder, imageRightStatus): builder.PrependInt32Slot(16, imageRightStatus, 0) -def FusionTrackFrameAddRegionsOfInterestLeft(builder, regionsOfInterestLeft): builder.PrependUOffsetTRelativeSlot(17, flatbuffers.number_types.UOffsetTFlags.py_type(regionsOfInterestLeft), 0) -def FusionTrackFrameStartRegionsOfInterestLeftVector(builder, numElems): return builder.StartVector(4, numElems, 4) -def FusionTrackFrameAddRegionsOfInterestLeftVersion(builder, regionsOfInterestLeftVersion): builder.PrependUint32Slot(18, regionsOfInterestLeftVersion, 0) -def FusionTrackFrameAddRegionsOfInterestLeftStatus(builder, regionsOfInterestLeftStatus): builder.PrependInt32Slot(19, regionsOfInterestLeftStatus, 0) -def FusionTrackFrameAddRegionsOfInterestRight(builder, regionsOfInterestRight): builder.PrependUOffsetTRelativeSlot(20, flatbuffers.number_types.UOffsetTFlags.py_type(regionsOfInterestRight), 0) -def FusionTrackFrameStartRegionsOfInterestRightVector(builder, numElems): return builder.StartVector(4, numElems, 4) -def FusionTrackFrameAddRegionsOfInterestRightVersion(builder, regionsOfInterestRightVersion): builder.PrependUint32Slot(21, regionsOfInterestRightVersion, 0) -def FusionTrackFrameAddRegionsOfInterestRightStatus(builder, regionsOfInterestRightStatus): builder.PrependInt32Slot(22, regionsOfInterestRightStatus, 0) -def FusionTrackFrameAddThreeDFiducials(builder, threeDFiducials): builder.PrependUOffsetTRelativeSlot(23, flatbuffers.number_types.UOffsetTFlags.py_type(threeDFiducials), 0) -def FusionTrackFrameStartThreeDFiducialsVector(builder, numElems): return builder.StartVector(4, numElems, 4) -def FusionTrackFrameAddThreeDFiducialsVersion(builder, threeDFiducialsVersion): builder.PrependUint32Slot(24, threeDFiducialsVersion, 0) -def FusionTrackFrameAddThreeDFiducialsStatus(builder, threeDFiducialsStatus): builder.PrependInt32Slot(25, threeDFiducialsStatus, 0) -def FusionTrackFrameAddMarkers(builder, markers): builder.PrependUOffsetTRelativeSlot(26, flatbuffers.number_types.UOffsetTFlags.py_type(markers), 0) -def FusionTrackFrameStartMarkersVector(builder, numElems): return builder.StartVector(4, numElems, 4) -def FusionTrackFrameAddMarkersVersion(builder, markersVersion): builder.PrependUint32Slot(27, markersVersion, 0) -def FusionTrackFrameAddMarkersStatus(builder, markersStatus): builder.PrependInt32Slot(28, markersStatus, 0) -def FusionTrackFrameAddDeviceType(builder, deviceType): builder.PrependInt32Slot(29, deviceType, 0) -def FusionTrackFrameAddFtkError(builder, ftkError): builder.PrependInt64Slot(30, ftkError, 0) -def FusionTrackFrameEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/FusionTrackMessage.java b/include/grl/flatbuffer/grl/flatbuffer/FusionTrackMessage.java deleted file mode 100644 index 1fa352d..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/FusionTrackMessage.java +++ /dev/null @@ -1,48 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class FusionTrackMessage extends Table { - public static FusionTrackMessage getRootAsFusionTrackMessage(ByteBuffer _bb) { return getRootAsFusionTrackMessage(_bb, new FusionTrackMessage()); } - public static FusionTrackMessage getRootAsFusionTrackMessage(ByteBuffer _bb, FusionTrackMessage obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public FusionTrackMessage __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - public double timestamp() { int o = __offset(4); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } - public FusionTrackParameters parameters() { return parameters(new FusionTrackParameters()); } - public FusionTrackParameters parameters(FusionTrackParameters obj) { int o = __offset(6); return o != 0 ? obj.__assign(__indirect(o + bb_pos), bb) : null; } - public TimeEvent timeEvent() { return timeEvent(new TimeEvent()); } - public TimeEvent timeEvent(TimeEvent obj) { int o = __offset(8); return o != 0 ? obj.__assign(__indirect(o + bb_pos), bb) : null; } - public FusionTrackFrame frame() { return frame(new FusionTrackFrame()); } - public FusionTrackFrame frame(FusionTrackFrame obj) { int o = __offset(10); return o != 0 ? obj.__assign(__indirect(o + bb_pos), bb) : null; } - - public static int createFusionTrackMessage(FlatBufferBuilder builder, - double timestamp, - int parametersOffset, - int timeEventOffset, - int frameOffset) { - builder.startObject(4); - FusionTrackMessage.addTimestamp(builder, timestamp); - FusionTrackMessage.addFrame(builder, frameOffset); - FusionTrackMessage.addTimeEvent(builder, timeEventOffset); - FusionTrackMessage.addParameters(builder, parametersOffset); - return FusionTrackMessage.endFusionTrackMessage(builder); - } - - public static void startFusionTrackMessage(FlatBufferBuilder builder) { builder.startObject(4); } - public static void addTimestamp(FlatBufferBuilder builder, double timestamp) { builder.addDouble(0, timestamp, 0.0); } - public static void addParameters(FlatBufferBuilder builder, int parametersOffset) { builder.addOffset(1, parametersOffset, 0); } - public static void addTimeEvent(FlatBufferBuilder builder, int timeEventOffset) { builder.addOffset(2, timeEventOffset, 0); } - public static void addFrame(FlatBufferBuilder builder, int frameOffset) { builder.addOffset(3, frameOffset, 0); } - public static int endFusionTrackMessage(FlatBufferBuilder builder) { - int o = builder.endObject(); - return o; - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/FusionTrackMessage.py b/include/grl/flatbuffer/grl/flatbuffer/FusionTrackMessage.py deleted file mode 100644 index 2b085c2..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/FusionTrackMessage.py +++ /dev/null @@ -1,66 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class FusionTrackMessage(object): - __slots__ = ['_tab'] - - @classmethod - def GetRootAsFusionTrackMessage(cls, buf, offset): - n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) - x = FusionTrackMessage() - x.Init(buf, n + offset) - return x - - # FusionTrackMessage - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - - # FusionTrackMessage - def Timestamp(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) - return 0.0 - - # FusionTrackMessage - def Parameters(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) - if o != 0: - x = self._tab.Indirect(o + self._tab.Pos) - from .FusionTrackParameters import FusionTrackParameters - obj = FusionTrackParameters() - obj.Init(self._tab.Bytes, x) - return obj - return None - - # FusionTrackMessage - def TimeEvent(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) - if o != 0: - x = self._tab.Indirect(o + self._tab.Pos) - from .TimeEvent import TimeEvent - obj = TimeEvent() - obj.Init(self._tab.Bytes, x) - return obj - return None - - # FusionTrackMessage - def Frame(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) - if o != 0: - x = self._tab.Indirect(o + self._tab.Pos) - from .FusionTrackFrame import FusionTrackFrame - obj = FusionTrackFrame() - obj.Init(self._tab.Bytes, x) - return obj - return None - -def FusionTrackMessageStart(builder): builder.StartObject(4) -def FusionTrackMessageAddTimestamp(builder, timestamp): builder.PrependFloat64Slot(0, timestamp, 0.0) -def FusionTrackMessageAddParameters(builder, parameters): builder.PrependUOffsetTRelativeSlot(1, flatbuffers.number_types.UOffsetTFlags.py_type(parameters), 0) -def FusionTrackMessageAddTimeEvent(builder, timeEvent): builder.PrependUOffsetTRelativeSlot(2, flatbuffers.number_types.UOffsetTFlags.py_type(timeEvent), 0) -def FusionTrackMessageAddFrame(builder, frame): builder.PrependUOffsetTRelativeSlot(3, flatbuffers.number_types.UOffsetTFlags.py_type(frame), 0) -def FusionTrackMessageEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/FusionTrackParameters.java b/include/grl/flatbuffer/grl/flatbuffer/FusionTrackParameters.java deleted file mode 100644 index c50156d..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/FusionTrackParameters.java +++ /dev/null @@ -1,145 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class FusionTrackParameters extends Table { - public static FusionTrackParameters getRootAsFusionTrackParameters(ByteBuffer _bb) { return getRootAsFusionTrackParameters(_bb, new FusionTrackParameters()); } - public static FusionTrackParameters getRootAsFusionTrackParameters(ByteBuffer _bb, FusionTrackParameters obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public FusionTrackParameters __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - /** - * Name for this connection / FusionTrack driver instance - * useful for debugging and when multiple data sources are used - */ - public String name() { int o = __offset(4); return o != 0 ? __string(o + bb_pos) : null; } - public ByteBuffer nameAsByteBuffer() { return __vector_as_bytebuffer(4, 1); } - public ByteBuffer nameInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 4, 1); } - /** - * Name for the clock on the FusionTrack - * Useful for timing calculations and debugging. - */ - public String deviceClockID() { int o = __offset(6); return o != 0 ? __string(o + bb_pos) : null; } - public ByteBuffer deviceClockIDAsByteBuffer() { return __vector_as_bytebuffer(6, 1); } - public ByteBuffer deviceClockIDInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 6, 1); } - /** - * Name for the local clock on which this driver runs - * Useful for timing calculations and debugging. - */ - public String localClockID() { int o = __offset(8); return o != 0 ? __string(o + bb_pos) : null; } - public ByteBuffer localClockIDAsByteBuffer() { return __vector_as_bytebuffer(8, 1); } - public ByteBuffer localClockIDInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 8, 1); } - /** - * dimensions of the markers that may be present - */ - public ftkGeometry geometries(int j) { return geometries(new ftkGeometry(), j); } - public ftkGeometry geometries(ftkGeometry obj, int j) { int o = __offset(10); return o != 0 ? obj.__assign(__indirect(__vector(o) + j * 4), bb) : null; } - public int geometriesLength() { int o = __offset(10); return o != 0 ? __vector_len(o) : 0; } - /** - * Geometries aka fiducials aka markers to be loaded from ini files. - * The data loaded should not repeat IDs from MarkerIDs. - */ - public String geometryFilenames(int j) { int o = __offset(12); return o != 0 ? __string(__vector(o) + j * 4) : null; } - public int geometryFilenamesLength() { int o = __offset(12); return o != 0 ? __vector_len(o) : 0; } - /** - * Path to the directory with the marker ini files listed above - * Uses the default current working directory if empty - * geometryDir:[string]; - */ - public String geometryDir() { int o = __offset(14); return o != 0 ? __string(o + bb_pos) : null; } - public ByteBuffer geometryDirAsByteBuffer() { return __vector_as_bytebuffer(14, 1); } - public ByteBuffer geometryDirInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 14, 1); } - /** - * Optional list of optical tracker device ids to expect - * will be loaded automatically if empty - */ - public long TrackerDeviceIDs(int j) { int o = __offset(16); return o != 0 ? bb.getLong(__vector(o) + j * 8) : 0; } - public int TrackerDeviceIDsLength() { int o = __offset(16); return o != 0 ? __vector_len(o) : 0; } - public ByteBuffer TrackerDeviceIDsAsByteBuffer() { return __vector_as_bytebuffer(16, 8); } - public ByteBuffer TrackerDeviceIDsInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 16, 8); } - /** - * Marker geometry unique integer IDs - */ - public long markerIDs(int j) { int o = __offset(18); return o != 0 ? bb.getLong(__vector(o) + j * 8) : 0; } - public int markerIDsLength() { int o = __offset(18); return o != 0 ? __vector_len(o) : 0; } - public ByteBuffer markerIDsAsByteBuffer() { return __vector_as_bytebuffer(18, 8); } - public ByteBuffer markerIDsInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 18, 8); } - /** - * Optional Marker geometry names with one for each ID, none otherwise - */ - public String markerNames(int j) { int o = __offset(20); return o != 0 ? __string(__vector(o) + j * 4) : null; } - public int markerNamesLength() { int o = __offset(20); return o != 0 ? __vector_len(o) : 0; } - public long mDeviceSerialNumbers(int j) { int o = __offset(22); return o != 0 ? bb.getLong(__vector(o) + j * 8) : 0; } - public int mDeviceSerialNumbersLength() { int o = __offset(22); return o != 0 ? __vector_len(o) : 0; } - public ByteBuffer mDeviceSerialNumbersAsByteBuffer() { return __vector_as_bytebuffer(22, 8); } - public ByteBuffer mDeviceSerialNumbersInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 22, 8); } - public int mDeviceTypes(int j) { int o = __offset(24); return o != 0 ? bb.get(__vector(o) + j * 1) & 0xFF : 0; } - public int mDeviceTypesLength() { int o = __offset(24); return o != 0 ? __vector_len(o) : 0; } - public ByteBuffer mDeviceTypesAsByteBuffer() { return __vector_as_bytebuffer(24, 1); } - public ByteBuffer mDeviceTypesInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 24, 1); } - - public static int createFusionTrackParameters(FlatBufferBuilder builder, - int nameOffset, - int deviceClockIDOffset, - int localClockIDOffset, - int geometriesOffset, - int geometryFilenamesOffset, - int geometryDirOffset, - int TrackerDeviceIDsOffset, - int markerIDsOffset, - int markerNamesOffset, - int m_deviceSerialNumbersOffset, - int m_device_typesOffset) { - builder.startObject(11); - FusionTrackParameters.addMDeviceTypes(builder, m_device_typesOffset); - FusionTrackParameters.addMDeviceSerialNumbers(builder, m_deviceSerialNumbersOffset); - FusionTrackParameters.addMarkerNames(builder, markerNamesOffset); - FusionTrackParameters.addMarkerIDs(builder, markerIDsOffset); - FusionTrackParameters.addTrackerDeviceIDs(builder, TrackerDeviceIDsOffset); - FusionTrackParameters.addGeometryDir(builder, geometryDirOffset); - FusionTrackParameters.addGeometryFilenames(builder, geometryFilenamesOffset); - FusionTrackParameters.addGeometries(builder, geometriesOffset); - FusionTrackParameters.addLocalClockID(builder, localClockIDOffset); - FusionTrackParameters.addDeviceClockID(builder, deviceClockIDOffset); - FusionTrackParameters.addName(builder, nameOffset); - return FusionTrackParameters.endFusionTrackParameters(builder); - } - - public static void startFusionTrackParameters(FlatBufferBuilder builder) { builder.startObject(11); } - public static void addName(FlatBufferBuilder builder, int nameOffset) { builder.addOffset(0, nameOffset, 0); } - public static void addDeviceClockID(FlatBufferBuilder builder, int deviceClockIDOffset) { builder.addOffset(1, deviceClockIDOffset, 0); } - public static void addLocalClockID(FlatBufferBuilder builder, int localClockIDOffset) { builder.addOffset(2, localClockIDOffset, 0); } - public static void addGeometries(FlatBufferBuilder builder, int geometriesOffset) { builder.addOffset(3, geometriesOffset, 0); } - public static int createGeometriesVector(FlatBufferBuilder builder, int[] data) { builder.startVector(4, data.length, 4); for (int i = data.length - 1; i >= 0; i--) builder.addOffset(data[i]); return builder.endVector(); } - public static void startGeometriesVector(FlatBufferBuilder builder, int numElems) { builder.startVector(4, numElems, 4); } - public static void addGeometryFilenames(FlatBufferBuilder builder, int geometryFilenamesOffset) { builder.addOffset(4, geometryFilenamesOffset, 0); } - public static int createGeometryFilenamesVector(FlatBufferBuilder builder, int[] data) { builder.startVector(4, data.length, 4); for (int i = data.length - 1; i >= 0; i--) builder.addOffset(data[i]); return builder.endVector(); } - public static void startGeometryFilenamesVector(FlatBufferBuilder builder, int numElems) { builder.startVector(4, numElems, 4); } - public static void addGeometryDir(FlatBufferBuilder builder, int geometryDirOffset) { builder.addOffset(5, geometryDirOffset, 0); } - public static void addTrackerDeviceIDs(FlatBufferBuilder builder, int TrackerDeviceIDsOffset) { builder.addOffset(6, TrackerDeviceIDsOffset, 0); } - public static int createTrackerDeviceIDsVector(FlatBufferBuilder builder, long[] data) { builder.startVector(8, data.length, 8); for (int i = data.length - 1; i >= 0; i--) builder.addLong(data[i]); return builder.endVector(); } - public static void startTrackerDeviceIDsVector(FlatBufferBuilder builder, int numElems) { builder.startVector(8, numElems, 8); } - public static void addMarkerIDs(FlatBufferBuilder builder, int markerIDsOffset) { builder.addOffset(7, markerIDsOffset, 0); } - public static int createMarkerIDsVector(FlatBufferBuilder builder, long[] data) { builder.startVector(8, data.length, 8); for (int i = data.length - 1; i >= 0; i--) builder.addLong(data[i]); return builder.endVector(); } - public static void startMarkerIDsVector(FlatBufferBuilder builder, int numElems) { builder.startVector(8, numElems, 8); } - public static void addMarkerNames(FlatBufferBuilder builder, int markerNamesOffset) { builder.addOffset(8, markerNamesOffset, 0); } - public static int createMarkerNamesVector(FlatBufferBuilder builder, int[] data) { builder.startVector(4, data.length, 4); for (int i = data.length - 1; i >= 0; i--) builder.addOffset(data[i]); return builder.endVector(); } - public static void startMarkerNamesVector(FlatBufferBuilder builder, int numElems) { builder.startVector(4, numElems, 4); } - public static void addMDeviceSerialNumbers(FlatBufferBuilder builder, int mDeviceSerialNumbersOffset) { builder.addOffset(9, mDeviceSerialNumbersOffset, 0); } - public static int createMDeviceSerialNumbersVector(FlatBufferBuilder builder, long[] data) { builder.startVector(8, data.length, 8); for (int i = data.length - 1; i >= 0; i--) builder.addLong(data[i]); return builder.endVector(); } - public static void startMDeviceSerialNumbersVector(FlatBufferBuilder builder, int numElems) { builder.startVector(8, numElems, 8); } - public static void addMDeviceTypes(FlatBufferBuilder builder, int mDeviceTypesOffset) { builder.addOffset(10, mDeviceTypesOffset, 0); } - public static int createMDeviceTypesVector(FlatBufferBuilder builder, byte[] data) { builder.startVector(1, data.length, 1); for (int i = data.length - 1; i >= 0; i--) builder.addByte(data[i]); return builder.endVector(); } - public static void startMDeviceTypesVector(FlatBufferBuilder builder, int numElems) { builder.startVector(1, numElems, 1); } - public static int endFusionTrackParameters(FlatBufferBuilder builder) { - int o = builder.endObject(); - return o; - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/FusionTrackParameters.py b/include/grl/flatbuffer/grl/flatbuffer/FusionTrackParameters.py deleted file mode 100644 index 1354fe8..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/FusionTrackParameters.py +++ /dev/null @@ -1,222 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class FusionTrackParameters(object): - __slots__ = ['_tab'] - - @classmethod - def GetRootAsFusionTrackParameters(cls, buf, offset): - n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) - x = FusionTrackParameters() - x.Init(buf, n + offset) - return x - - # FusionTrackParameters - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - -# /// Name for this connection / FusionTrack driver instance -# /// useful for debugging and when multiple data sources are used - # FusionTrackParameters - def Name(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - return self._tab.String(o + self._tab.Pos) - return bytes() - -# /// Name for the clock on the FusionTrack -# /// Useful for timing calculations and debugging. - # FusionTrackParameters - def DeviceClockID(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) - if o != 0: - return self._tab.String(o + self._tab.Pos) - return bytes() - -# /// Name for the local clock on which this driver runs -# /// Useful for timing calculations and debugging. - # FusionTrackParameters - def LocalClockID(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) - if o != 0: - return self._tab.String(o + self._tab.Pos) - return bytes() - -# /// dimensions of the markers that may be present - # FusionTrackParameters - def Geometries(self, j): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) - if o != 0: - x = self._tab.Vector(o) - x += flatbuffers.number_types.UOffsetTFlags.py_type(j) * 4 - x = self._tab.Indirect(x) - from .ftkGeometry import ftkGeometry - obj = ftkGeometry() - obj.Init(self._tab.Bytes, x) - return obj - return None - - # FusionTrackParameters - def GeometriesLength(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) - if o != 0: - return self._tab.VectorLen(o) - return 0 - -# /// Geometries aka fiducials aka markers to be loaded from ini files. -# /// The data loaded should not repeat IDs from MarkerIDs. - # FusionTrackParameters - def GeometryFilenames(self, j): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(12)) - if o != 0: - a = self._tab.Vector(o) - return self._tab.String(a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 4)) - return "" - - # FusionTrackParameters - def GeometryFilenamesLength(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(12)) - if o != 0: - return self._tab.VectorLen(o) - return 0 - -# /// Path to the directory with the marker ini files listed above -# /// Uses the default current working directory if empty -# /// geometryDir:[string]; - # FusionTrackParameters - def GeometryDir(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(14)) - if o != 0: - return self._tab.String(o + self._tab.Pos) - return bytes() - -# /// Optional list of optical tracker device ids to expect -# /// will be loaded automatically if empty - # FusionTrackParameters - def TrackerDeviceIDs(self, j): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(16)) - if o != 0: - a = self._tab.Vector(o) - return self._tab.Get(flatbuffers.number_types.Uint64Flags, a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 8)) - return 0 - - # FusionTrackParameters - def TrackerDeviceIDsAsNumpy(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(16)) - if o != 0: - return self._tab.GetVectorAsNumpy(flatbuffers.number_types.Uint64Flags, o) - return 0 - - # FusionTrackParameters - def TrackerDeviceIDsLength(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(16)) - if o != 0: - return self._tab.VectorLen(o) - return 0 - -# /// Marker geometry unique integer IDs - # FusionTrackParameters - def MarkerIDs(self, j): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(18)) - if o != 0: - a = self._tab.Vector(o) - return self._tab.Get(flatbuffers.number_types.Uint64Flags, a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 8)) - return 0 - - # FusionTrackParameters - def MarkerIDsAsNumpy(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(18)) - if o != 0: - return self._tab.GetVectorAsNumpy(flatbuffers.number_types.Uint64Flags, o) - return 0 - - # FusionTrackParameters - def MarkerIDsLength(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(18)) - if o != 0: - return self._tab.VectorLen(o) - return 0 - -# /// Optional Marker geometry names with one for each ID, none otherwise - # FusionTrackParameters - def MarkerNames(self, j): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(20)) - if o != 0: - a = self._tab.Vector(o) - return self._tab.String(a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 4)) - return "" - - # FusionTrackParameters - def MarkerNamesLength(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(20)) - if o != 0: - return self._tab.VectorLen(o) - return 0 - - # FusionTrackParameters - def MDeviceSerialNumbers(self, j): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(22)) - if o != 0: - a = self._tab.Vector(o) - return self._tab.Get(flatbuffers.number_types.Uint64Flags, a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 8)) - return 0 - - # FusionTrackParameters - def MDeviceSerialNumbersAsNumpy(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(22)) - if o != 0: - return self._tab.GetVectorAsNumpy(flatbuffers.number_types.Uint64Flags, o) - return 0 - - # FusionTrackParameters - def MDeviceSerialNumbersLength(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(22)) - if o != 0: - return self._tab.VectorLen(o) - return 0 - - # FusionTrackParameters - def MDeviceTypes(self, j): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(24)) - if o != 0: - a = self._tab.Vector(o) - return self._tab.Get(flatbuffers.number_types.Uint8Flags, a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 1)) - return 0 - - # FusionTrackParameters - def MDeviceTypesAsNumpy(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(24)) - if o != 0: - return self._tab.GetVectorAsNumpy(flatbuffers.number_types.Uint8Flags, o) - return 0 - - # FusionTrackParameters - def MDeviceTypesLength(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(24)) - if o != 0: - return self._tab.VectorLen(o) - return 0 - -def FusionTrackParametersStart(builder): builder.StartObject(11) -def FusionTrackParametersAddName(builder, name): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(name), 0) -def FusionTrackParametersAddDeviceClockID(builder, deviceClockID): builder.PrependUOffsetTRelativeSlot(1, flatbuffers.number_types.UOffsetTFlags.py_type(deviceClockID), 0) -def FusionTrackParametersAddLocalClockID(builder, localClockID): builder.PrependUOffsetTRelativeSlot(2, flatbuffers.number_types.UOffsetTFlags.py_type(localClockID), 0) -def FusionTrackParametersAddGeometries(builder, geometries): builder.PrependUOffsetTRelativeSlot(3, flatbuffers.number_types.UOffsetTFlags.py_type(geometries), 0) -def FusionTrackParametersStartGeometriesVector(builder, numElems): return builder.StartVector(4, numElems, 4) -def FusionTrackParametersAddGeometryFilenames(builder, geometryFilenames): builder.PrependUOffsetTRelativeSlot(4, flatbuffers.number_types.UOffsetTFlags.py_type(geometryFilenames), 0) -def FusionTrackParametersStartGeometryFilenamesVector(builder, numElems): return builder.StartVector(4, numElems, 4) -def FusionTrackParametersAddGeometryDir(builder, geometryDir): builder.PrependUOffsetTRelativeSlot(5, flatbuffers.number_types.UOffsetTFlags.py_type(geometryDir), 0) -def FusionTrackParametersAddTrackerDeviceIDs(builder, TrackerDeviceIDs): builder.PrependUOffsetTRelativeSlot(6, flatbuffers.number_types.UOffsetTFlags.py_type(TrackerDeviceIDs), 0) -def FusionTrackParametersStartTrackerDeviceIDsVector(builder, numElems): return builder.StartVector(8, numElems, 8) -def FusionTrackParametersAddMarkerIDs(builder, markerIDs): builder.PrependUOffsetTRelativeSlot(7, flatbuffers.number_types.UOffsetTFlags.py_type(markerIDs), 0) -def FusionTrackParametersStartMarkerIDsVector(builder, numElems): return builder.StartVector(8, numElems, 8) -def FusionTrackParametersAddMarkerNames(builder, markerNames): builder.PrependUOffsetTRelativeSlot(8, flatbuffers.number_types.UOffsetTFlags.py_type(markerNames), 0) -def FusionTrackParametersStartMarkerNamesVector(builder, numElems): return builder.StartVector(4, numElems, 4) -def FusionTrackParametersAddMDeviceSerialNumbers(builder, mDeviceSerialNumbers): builder.PrependUOffsetTRelativeSlot(9, flatbuffers.number_types.UOffsetTFlags.py_type(mDeviceSerialNumbers), 0) -def FusionTrackParametersStartMDeviceSerialNumbersVector(builder, numElems): return builder.StartVector(8, numElems, 8) -def FusionTrackParametersAddMDeviceTypes(builder, mDeviceTypes): builder.PrependUOffsetTRelativeSlot(10, flatbuffers.number_types.UOffsetTFlags.py_type(mDeviceTypes), 0) -def FusionTrackParametersStartMDeviceTypesVector(builder, numElems): return builder.StartVector(1, numElems, 1) -def FusionTrackParametersEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/Inertia.java b/include/grl/flatbuffer/grl/flatbuffer/Inertia.java deleted file mode 100644 index 7311a09..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/Inertia.java +++ /dev/null @@ -1,47 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class Inertia extends Struct { - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public Inertia __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - public double mass() { return bb.getDouble(bb_pos + 0); } - public Pose pose() { return pose(new Pose()); } - public Pose pose(Pose obj) { return obj.__assign(bb_pos + 8, bb); } - public double ixx() { return bb.getDouble(bb_pos + 64); } - public double ixy() { return bb.getDouble(bb_pos + 72); } - public double ixz() { return bb.getDouble(bb_pos + 80); } - public double iyy() { return bb.getDouble(bb_pos + 88); } - public double iyz() { return bb.getDouble(bb_pos + 96); } - public double izz() { return bb.getDouble(bb_pos + 104); } - - public static int createInertia(FlatBufferBuilder builder, double mass, double pose_position_x, double pose_position_y, double pose_position_z, double pose_orientation_x, double pose_orientation_y, double pose_orientation_z, double pose_orientation_w, double ixx, double ixy, double ixz, double iyy, double iyz, double izz) { - builder.prep(8, 112); - builder.putDouble(izz); - builder.putDouble(iyz); - builder.putDouble(iyy); - builder.putDouble(ixz); - builder.putDouble(ixy); - builder.putDouble(ixx); - builder.prep(8, 56); - builder.prep(8, 32); - builder.putDouble(pose_orientation_w); - builder.putDouble(pose_orientation_z); - builder.putDouble(pose_orientation_y); - builder.putDouble(pose_orientation_x); - builder.prep(8, 24); - builder.putDouble(pose_position_z); - builder.putDouble(pose_position_y); - builder.putDouble(pose_position_x); - builder.putDouble(mass); - return builder.offset(); - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/Inertia.py b/include/grl/flatbuffer/grl/flatbuffer/Inertia.py deleted file mode 100644 index 9b0121a..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/Inertia.py +++ /dev/null @@ -1,53 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class Inertia(object): - __slots__ = ['_tab'] - - # Inertia - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - - # Inertia - def Mass(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(0)) - # Inertia - def Pose(self, obj): - obj.Init(self._tab.Bytes, self._tab.Pos + 8) - return obj - - # Inertia - def Ixx(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(64)) - # Inertia - def Ixy(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(72)) - # Inertia - def Ixz(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(80)) - # Inertia - def Iyy(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(88)) - # Inertia - def Iyz(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(96)) - # Inertia - def Izz(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(104)) - -def CreateInertia(builder, mass, pose_position_x, pose_position_y, pose_position_z, pose_orientation_x, pose_orientation_y, pose_orientation_z, pose_orientation_w, ixx, ixy, ixz, iyy, iyz, izz): - builder.Prep(8, 112) - builder.PrependFloat64(izz) - builder.PrependFloat64(iyz) - builder.PrependFloat64(iyy) - builder.PrependFloat64(ixz) - builder.PrependFloat64(ixy) - builder.PrependFloat64(ixx) - builder.Prep(8, 56) - builder.Prep(8, 32) - builder.PrependFloat64(pose_orientation_w) - builder.PrependFloat64(pose_orientation_z) - builder.PrependFloat64(pose_orientation_y) - builder.PrependFloat64(pose_orientation_x) - builder.Prep(8, 24) - builder.PrependFloat64(pose_position_z) - builder.PrependFloat64(pose_position_y) - builder.PrependFloat64(pose_position_x) - builder.PrependFloat64(mass) - return builder.Offset() diff --git a/include/grl/flatbuffer/grl/flatbuffer/JointImpedenceControlMode.java b/include/grl/flatbuffer/grl/flatbuffer/JointImpedenceControlMode.java deleted file mode 100644 index 0e2510c..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/JointImpedenceControlMode.java +++ /dev/null @@ -1,47 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class JointImpedenceControlMode extends Table { - public static JointImpedenceControlMode getRootAsJointImpedenceControlMode(ByteBuffer _bb) { return getRootAsJointImpedenceControlMode(_bb, new JointImpedenceControlMode()); } - public static JointImpedenceControlMode getRootAsJointImpedenceControlMode(ByteBuffer _bb, JointImpedenceControlMode obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public JointImpedenceControlMode __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - public double stiffness(int j) { int o = __offset(4); return o != 0 ? bb.getDouble(__vector(o) + j * 8) : 0; } - public int stiffnessLength() { int o = __offset(4); return o != 0 ? __vector_len(o) : 0; } - public ByteBuffer stiffnessAsByteBuffer() { return __vector_as_bytebuffer(4, 8); } - public ByteBuffer stiffnessInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 4, 8); } - public double damping(int j) { int o = __offset(6); return o != 0 ? bb.getDouble(__vector(o) + j * 8) : 0; } - public int dampingLength() { int o = __offset(6); return o != 0 ? __vector_len(o) : 0; } - public ByteBuffer dampingAsByteBuffer() { return __vector_as_bytebuffer(6, 8); } - public ByteBuffer dampingInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 6, 8); } - - public static int createJointImpedenceControlMode(FlatBufferBuilder builder, - int stiffnessOffset, - int dampingOffset) { - builder.startObject(2); - JointImpedenceControlMode.addDamping(builder, dampingOffset); - JointImpedenceControlMode.addStiffness(builder, stiffnessOffset); - return JointImpedenceControlMode.endJointImpedenceControlMode(builder); - } - - public static void startJointImpedenceControlMode(FlatBufferBuilder builder) { builder.startObject(2); } - public static void addStiffness(FlatBufferBuilder builder, int stiffnessOffset) { builder.addOffset(0, stiffnessOffset, 0); } - public static int createStiffnessVector(FlatBufferBuilder builder, double[] data) { builder.startVector(8, data.length, 8); for (int i = data.length - 1; i >= 0; i--) builder.addDouble(data[i]); return builder.endVector(); } - public static void startStiffnessVector(FlatBufferBuilder builder, int numElems) { builder.startVector(8, numElems, 8); } - public static void addDamping(FlatBufferBuilder builder, int dampingOffset) { builder.addOffset(1, dampingOffset, 0); } - public static int createDampingVector(FlatBufferBuilder builder, double[] data) { builder.startVector(8, data.length, 8); for (int i = data.length - 1; i >= 0; i--) builder.addDouble(data[i]); return builder.endVector(); } - public static void startDampingVector(FlatBufferBuilder builder, int numElems) { builder.startVector(8, numElems, 8); } - public static int endJointImpedenceControlMode(FlatBufferBuilder builder) { - int o = builder.endObject(); - return o; - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/JointImpedenceControlMode.py b/include/grl/flatbuffer/grl/flatbuffer/JointImpedenceControlMode.py deleted file mode 100644 index 85f4824..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/JointImpedenceControlMode.py +++ /dev/null @@ -1,70 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class JointImpedenceControlMode(object): - __slots__ = ['_tab'] - - @classmethod - def GetRootAsJointImpedenceControlMode(cls, buf, offset): - n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) - x = JointImpedenceControlMode() - x.Init(buf, n + offset) - return x - - # JointImpedenceControlMode - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - - # JointImpedenceControlMode - def Stiffness(self, j): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - a = self._tab.Vector(o) - return self._tab.Get(flatbuffers.number_types.Float64Flags, a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 8)) - return 0 - - # JointImpedenceControlMode - def StiffnessAsNumpy(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - return self._tab.GetVectorAsNumpy(flatbuffers.number_types.Float64Flags, o) - return 0 - - # JointImpedenceControlMode - def StiffnessLength(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - return self._tab.VectorLen(o) - return 0 - - # JointImpedenceControlMode - def Damping(self, j): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) - if o != 0: - a = self._tab.Vector(o) - return self._tab.Get(flatbuffers.number_types.Float64Flags, a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 8)) - return 0 - - # JointImpedenceControlMode - def DampingAsNumpy(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) - if o != 0: - return self._tab.GetVectorAsNumpy(flatbuffers.number_types.Float64Flags, o) - return 0 - - # JointImpedenceControlMode - def DampingLength(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) - if o != 0: - return self._tab.VectorLen(o) - return 0 - -def JointImpedenceControlModeStart(builder): builder.StartObject(2) -def JointImpedenceControlModeAddStiffness(builder, stiffness): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(stiffness), 0) -def JointImpedenceControlModeStartStiffnessVector(builder, numElems): return builder.StartVector(8, numElems, 8) -def JointImpedenceControlModeAddDamping(builder, damping): builder.PrependUOffsetTRelativeSlot(1, flatbuffers.number_types.UOffsetTFlags.py_type(damping), 0) -def JointImpedenceControlModeStartDampingVector(builder, numElems): return builder.StartVector(8, numElems, 8) -def JointImpedenceControlModeEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/JointState.java b/include/grl/flatbuffer/grl/flatbuffer/JointState.java deleted file mode 100644 index 0380c56..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/JointState.java +++ /dev/null @@ -1,65 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class JointState extends Table { - public static JointState getRootAsJointState(ByteBuffer _bb) { return getRootAsJointState(_bb, new JointState()); } - public static JointState getRootAsJointState(ByteBuffer _bb, JointState obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public JointState __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - public double position(int j) { int o = __offset(4); return o != 0 ? bb.getDouble(__vector(o) + j * 8) : 0; } - public int positionLength() { int o = __offset(4); return o != 0 ? __vector_len(o) : 0; } - public ByteBuffer positionAsByteBuffer() { return __vector_as_bytebuffer(4, 8); } - public ByteBuffer positionInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 4, 8); } - public double velocity(int j) { int o = __offset(6); return o != 0 ? bb.getDouble(__vector(o) + j * 8) : 0; } - public int velocityLength() { int o = __offset(6); return o != 0 ? __vector_len(o) : 0; } - public ByteBuffer velocityAsByteBuffer() { return __vector_as_bytebuffer(6, 8); } - public ByteBuffer velocityInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 6, 8); } - public double acceleration(int j) { int o = __offset(8); return o != 0 ? bb.getDouble(__vector(o) + j * 8) : 0; } - public int accelerationLength() { int o = __offset(8); return o != 0 ? __vector_len(o) : 0; } - public ByteBuffer accelerationAsByteBuffer() { return __vector_as_bytebuffer(8, 8); } - public ByteBuffer accelerationInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 8, 8); } - public double torque(int j) { int o = __offset(10); return o != 0 ? bb.getDouble(__vector(o) + j * 8) : 0; } - public int torqueLength() { int o = __offset(10); return o != 0 ? __vector_len(o) : 0; } - public ByteBuffer torqueAsByteBuffer() { return __vector_as_bytebuffer(10, 8); } - public ByteBuffer torqueInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 10, 8); } - - public static int createJointState(FlatBufferBuilder builder, - int positionOffset, - int velocityOffset, - int accelerationOffset, - int torqueOffset) { - builder.startObject(4); - JointState.addTorque(builder, torqueOffset); - JointState.addAcceleration(builder, accelerationOffset); - JointState.addVelocity(builder, velocityOffset); - JointState.addPosition(builder, positionOffset); - return JointState.endJointState(builder); - } - - public static void startJointState(FlatBufferBuilder builder) { builder.startObject(4); } - public static void addPosition(FlatBufferBuilder builder, int positionOffset) { builder.addOffset(0, positionOffset, 0); } - public static int createPositionVector(FlatBufferBuilder builder, double[] data) { builder.startVector(8, data.length, 8); for (int i = data.length - 1; i >= 0; i--) builder.addDouble(data[i]); return builder.endVector(); } - public static void startPositionVector(FlatBufferBuilder builder, int numElems) { builder.startVector(8, numElems, 8); } - public static void addVelocity(FlatBufferBuilder builder, int velocityOffset) { builder.addOffset(1, velocityOffset, 0); } - public static int createVelocityVector(FlatBufferBuilder builder, double[] data) { builder.startVector(8, data.length, 8); for (int i = data.length - 1; i >= 0; i--) builder.addDouble(data[i]); return builder.endVector(); } - public static void startVelocityVector(FlatBufferBuilder builder, int numElems) { builder.startVector(8, numElems, 8); } - public static void addAcceleration(FlatBufferBuilder builder, int accelerationOffset) { builder.addOffset(2, accelerationOffset, 0); } - public static int createAccelerationVector(FlatBufferBuilder builder, double[] data) { builder.startVector(8, data.length, 8); for (int i = data.length - 1; i >= 0; i--) builder.addDouble(data[i]); return builder.endVector(); } - public static void startAccelerationVector(FlatBufferBuilder builder, int numElems) { builder.startVector(8, numElems, 8); } - public static void addTorque(FlatBufferBuilder builder, int torqueOffset) { builder.addOffset(3, torqueOffset, 0); } - public static int createTorqueVector(FlatBufferBuilder builder, double[] data) { builder.startVector(8, data.length, 8); for (int i = data.length - 1; i >= 0; i--) builder.addDouble(data[i]); return builder.endVector(); } - public static void startTorqueVector(FlatBufferBuilder builder, int numElems) { builder.startVector(8, numElems, 8); } - public static int endJointState(FlatBufferBuilder builder) { - int o = builder.endObject(); - return o; - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/JointState.py b/include/grl/flatbuffer/grl/flatbuffer/JointState.py deleted file mode 100644 index 4746e33..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/JointState.py +++ /dev/null @@ -1,118 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class JointState(object): - __slots__ = ['_tab'] - - @classmethod - def GetRootAsJointState(cls, buf, offset): - n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) - x = JointState() - x.Init(buf, n + offset) - return x - - # JointState - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - - # JointState - def Position(self, j): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - a = self._tab.Vector(o) - return self._tab.Get(flatbuffers.number_types.Float64Flags, a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 8)) - return 0 - - # JointState - def PositionAsNumpy(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - return self._tab.GetVectorAsNumpy(flatbuffers.number_types.Float64Flags, o) - return 0 - - # JointState - def PositionLength(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - return self._tab.VectorLen(o) - return 0 - - # JointState - def Velocity(self, j): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) - if o != 0: - a = self._tab.Vector(o) - return self._tab.Get(flatbuffers.number_types.Float64Flags, a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 8)) - return 0 - - # JointState - def VelocityAsNumpy(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) - if o != 0: - return self._tab.GetVectorAsNumpy(flatbuffers.number_types.Float64Flags, o) - return 0 - - # JointState - def VelocityLength(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) - if o != 0: - return self._tab.VectorLen(o) - return 0 - - # JointState - def Acceleration(self, j): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) - if o != 0: - a = self._tab.Vector(o) - return self._tab.Get(flatbuffers.number_types.Float64Flags, a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 8)) - return 0 - - # JointState - def AccelerationAsNumpy(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) - if o != 0: - return self._tab.GetVectorAsNumpy(flatbuffers.number_types.Float64Flags, o) - return 0 - - # JointState - def AccelerationLength(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) - if o != 0: - return self._tab.VectorLen(o) - return 0 - - # JointState - def Torque(self, j): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) - if o != 0: - a = self._tab.Vector(o) - return self._tab.Get(flatbuffers.number_types.Float64Flags, a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 8)) - return 0 - - # JointState - def TorqueAsNumpy(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) - if o != 0: - return self._tab.GetVectorAsNumpy(flatbuffers.number_types.Float64Flags, o) - return 0 - - # JointState - def TorqueLength(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) - if o != 0: - return self._tab.VectorLen(o) - return 0 - -def JointStateStart(builder): builder.StartObject(4) -def JointStateAddPosition(builder, position): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(position), 0) -def JointStateStartPositionVector(builder, numElems): return builder.StartVector(8, numElems, 8) -def JointStateAddVelocity(builder, velocity): builder.PrependUOffsetTRelativeSlot(1, flatbuffers.number_types.UOffsetTFlags.py_type(velocity), 0) -def JointStateStartVelocityVector(builder, numElems): return builder.StartVector(8, numElems, 8) -def JointStateAddAcceleration(builder, acceleration): builder.PrependUOffsetTRelativeSlot(2, flatbuffers.number_types.UOffsetTFlags.py_type(acceleration), 0) -def JointStateStartAccelerationVector(builder, numElems): return builder.StartVector(8, numElems, 8) -def JointStateAddTorque(builder, torque): builder.PrependUOffsetTRelativeSlot(3, flatbuffers.number_types.UOffsetTFlags.py_type(torque), 0) -def JointStateStartTorqueVector(builder, numElems): return builder.StartVector(8, numElems, 8) -def JointStateEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaArmConfiguration.java b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaArmConfiguration.java deleted file mode 100644 index 822026a..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaArmConfiguration.java +++ /dev/null @@ -1,119 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class KUKAiiwaArmConfiguration extends Table { - public static KUKAiiwaArmConfiguration getRootAsKUKAiiwaArmConfiguration(ByteBuffer _bb) { return getRootAsKUKAiiwaArmConfiguration(_bb, new KUKAiiwaArmConfiguration()); } - public static KUKAiiwaArmConfiguration getRootAsKUKAiiwaArmConfiguration(ByteBuffer _bb, KUKAiiwaArmConfiguration obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public KUKAiiwaArmConfiguration __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - public String name() { int o = __offset(4); return o != 0 ? __string(o + bb_pos) : null; } - public ByteBuffer nameAsByteBuffer() { return __vector_as_bytebuffer(4, 1); } - public ByteBuffer nameInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 4, 1); } - /** - * how commands will be sent to robot - */ - public byte commandInterface() { int o = __offset(6); return o != 0 ? bb.get(o + bb_pos) : 0; } - /** - * how robot state will be sent to driver - */ - public byte monitorInterface() { int o = __offset(8); return o != 0 ? bb.get(o + bb_pos) : 0; } - /** - * motion command mode: cartesian, wrench, torque commands - */ - public byte clientCommandMode() { int o = __offset(10); return o != 0 ? bb.get(o + bb_pos) : 0; } - /** - * The type of commands FRI will use: cartesian, joint - */ - public byte overlayType() { int o = __offset(12); return o != 0 ? bb.get(o + bb_pos) : 0; } - /** - * position, cartesian impedence, or joint impedence low level controller adjustments - */ - public byte controlMode() { int o = __offset(14); return o != 0 ? bb.get(o + bb_pos) : 0; } - public CartesianImpedenceControlMode setCartImpedance() { return setCartImpedance(new CartesianImpedenceControlMode()); } - public CartesianImpedenceControlMode setCartImpedance(CartesianImpedenceControlMode obj) { int o = __offset(16); return o != 0 ? obj.__assign(__indirect(o + bb_pos), bb) : null; } - public JointImpedenceControlMode setJointImpedance() { return setJointImpedance(new JointImpedenceControlMode()); } - public JointImpedenceControlMode setJointImpedance(JointImpedenceControlMode obj) { int o = __offset(18); return o != 0 ? obj.__assign(__indirect(o + bb_pos), bb) : null; } - public SmartServo smartServoConfig() { return smartServoConfig(new SmartServo()); } - public SmartServo smartServoConfig(SmartServo obj) { int o = __offset(20); return o != 0 ? obj.__assign(__indirect(o + bb_pos), bb) : null; } - public FRI FRIConfig() { return FRIConfig(new FRI()); } - public FRI FRIConfig(FRI obj) { int o = __offset(22); return o != 0 ? obj.__assign(__indirect(o + bb_pos), bb) : null; } - public LinkObject tools(int j) { return tools(new LinkObject(), j); } - public LinkObject tools(LinkObject obj, int j) { int o = __offset(24); return o != 0 ? obj.__assign(__indirect(__vector(o) + j * 4), bb) : null; } - public int toolsLength() { int o = __offset(24); return o != 0 ? __vector_len(o) : 0; } - /** - * set kuka tablet "processData" panel UI config strings - */ - public ProcessData processData(int j) { return processData(new ProcessData(), j); } - public ProcessData processData(ProcessData obj, int j) { int o = __offset(26); return o != 0 ? obj.__assign(__indirect(__vector(o) + j * 4), bb) : null; } - public int processDataLength() { int o = __offset(26); return o != 0 ? __vector_len(o) : 0; } - public String currentMotionCenter() { int o = __offset(28); return o != 0 ? __string(o + bb_pos) : null; } - public ByteBuffer currentMotionCenterAsByteBuffer() { return __vector_as_bytebuffer(28, 1); } - public ByteBuffer currentMotionCenterInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 28, 1); } - public boolean requestMonitorProcessData() { int o = __offset(30); return o != 0 ? 0!=bb.get(o + bb_pos) : false; } - - public static int createKUKAiiwaArmConfiguration(FlatBufferBuilder builder, - int nameOffset, - byte commandInterface, - byte monitorInterface, - byte clientCommandMode, - byte overlayType, - byte controlMode, - int setCartImpedanceOffset, - int setJointImpedanceOffset, - int smartServoConfigOffset, - int FRIConfigOffset, - int toolsOffset, - int processDataOffset, - int currentMotionCenterOffset, - boolean requestMonitorProcessData) { - builder.startObject(14); - KUKAiiwaArmConfiguration.addCurrentMotionCenter(builder, currentMotionCenterOffset); - KUKAiiwaArmConfiguration.addProcessData(builder, processDataOffset); - KUKAiiwaArmConfiguration.addTools(builder, toolsOffset); - KUKAiiwaArmConfiguration.addFRIConfig(builder, FRIConfigOffset); - KUKAiiwaArmConfiguration.addSmartServoConfig(builder, smartServoConfigOffset); - KUKAiiwaArmConfiguration.addSetJointImpedance(builder, setJointImpedanceOffset); - KUKAiiwaArmConfiguration.addSetCartImpedance(builder, setCartImpedanceOffset); - KUKAiiwaArmConfiguration.addName(builder, nameOffset); - KUKAiiwaArmConfiguration.addRequestMonitorProcessData(builder, requestMonitorProcessData); - KUKAiiwaArmConfiguration.addControlMode(builder, controlMode); - KUKAiiwaArmConfiguration.addOverlayType(builder, overlayType); - KUKAiiwaArmConfiguration.addClientCommandMode(builder, clientCommandMode); - KUKAiiwaArmConfiguration.addMonitorInterface(builder, monitorInterface); - KUKAiiwaArmConfiguration.addCommandInterface(builder, commandInterface); - return KUKAiiwaArmConfiguration.endKUKAiiwaArmConfiguration(builder); - } - - public static void startKUKAiiwaArmConfiguration(FlatBufferBuilder builder) { builder.startObject(14); } - public static void addName(FlatBufferBuilder builder, int nameOffset) { builder.addOffset(0, nameOffset, 0); } - public static void addCommandInterface(FlatBufferBuilder builder, byte commandInterface) { builder.addByte(1, commandInterface, 0); } - public static void addMonitorInterface(FlatBufferBuilder builder, byte monitorInterface) { builder.addByte(2, monitorInterface, 0); } - public static void addClientCommandMode(FlatBufferBuilder builder, byte clientCommandMode) { builder.addByte(3, clientCommandMode, 0); } - public static void addOverlayType(FlatBufferBuilder builder, byte overlayType) { builder.addByte(4, overlayType, 0); } - public static void addControlMode(FlatBufferBuilder builder, byte controlMode) { builder.addByte(5, controlMode, 0); } - public static void addSetCartImpedance(FlatBufferBuilder builder, int setCartImpedanceOffset) { builder.addOffset(6, setCartImpedanceOffset, 0); } - public static void addSetJointImpedance(FlatBufferBuilder builder, int setJointImpedanceOffset) { builder.addOffset(7, setJointImpedanceOffset, 0); } - public static void addSmartServoConfig(FlatBufferBuilder builder, int smartServoConfigOffset) { builder.addOffset(8, smartServoConfigOffset, 0); } - public static void addFRIConfig(FlatBufferBuilder builder, int FRIConfigOffset) { builder.addOffset(9, FRIConfigOffset, 0); } - public static void addTools(FlatBufferBuilder builder, int toolsOffset) { builder.addOffset(10, toolsOffset, 0); } - public static int createToolsVector(FlatBufferBuilder builder, int[] data) { builder.startVector(4, data.length, 4); for (int i = data.length - 1; i >= 0; i--) builder.addOffset(data[i]); return builder.endVector(); } - public static void startToolsVector(FlatBufferBuilder builder, int numElems) { builder.startVector(4, numElems, 4); } - public static void addProcessData(FlatBufferBuilder builder, int processDataOffset) { builder.addOffset(11, processDataOffset, 0); } - public static int createProcessDataVector(FlatBufferBuilder builder, int[] data) { builder.startVector(4, data.length, 4); for (int i = data.length - 1; i >= 0; i--) builder.addOffset(data[i]); return builder.endVector(); } - public static void startProcessDataVector(FlatBufferBuilder builder, int numElems) { builder.startVector(4, numElems, 4); } - public static void addCurrentMotionCenter(FlatBufferBuilder builder, int currentMotionCenterOffset) { builder.addOffset(12, currentMotionCenterOffset, 0); } - public static void addRequestMonitorProcessData(FlatBufferBuilder builder, boolean requestMonitorProcessData) { builder.addBoolean(13, requestMonitorProcessData, false); } - public static int endKUKAiiwaArmConfiguration(FlatBufferBuilder builder) { - int o = builder.endObject(); - return o; - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaArmConfiguration.py b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaArmConfiguration.py deleted file mode 100644 index 94ea9fb..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaArmConfiguration.py +++ /dev/null @@ -1,184 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class KUKAiiwaArmConfiguration(object): - __slots__ = ['_tab'] - - @classmethod - def GetRootAsKUKAiiwaArmConfiguration(cls, buf, offset): - n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) - x = KUKAiiwaArmConfiguration() - x.Init(buf, n + offset) - return x - - # KUKAiiwaArmConfiguration - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - - # KUKAiiwaArmConfiguration - def Name(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - return self._tab.String(o + self._tab.Pos) - return bytes() - -# /// how commands will be sent to robot - # KUKAiiwaArmConfiguration - def CommandInterface(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Int8Flags, o + self._tab.Pos) - return 0 - -# /// how robot state will be sent to driver - # KUKAiiwaArmConfiguration - def MonitorInterface(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Int8Flags, o + self._tab.Pos) - return 0 - -# /// motion command mode: cartesian, wrench, torque commands - # KUKAiiwaArmConfiguration - def ClientCommandMode(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Int8Flags, o + self._tab.Pos) - return 0 - -# /// The type of commands FRI will use: cartesian, joint - # KUKAiiwaArmConfiguration - def OverlayType(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(12)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Int8Flags, o + self._tab.Pos) - return 0 - -# /// position, cartesian impedence, or joint impedence low level controller adjustments - # KUKAiiwaArmConfiguration - def ControlMode(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(14)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Int8Flags, o + self._tab.Pos) - return 0 - - # KUKAiiwaArmConfiguration - def SetCartImpedance(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(16)) - if o != 0: - x = self._tab.Indirect(o + self._tab.Pos) - from .CartesianImpedenceControlMode import CartesianImpedenceControlMode - obj = CartesianImpedenceControlMode() - obj.Init(self._tab.Bytes, x) - return obj - return None - - # KUKAiiwaArmConfiguration - def SetJointImpedance(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(18)) - if o != 0: - x = self._tab.Indirect(o + self._tab.Pos) - from .JointImpedenceControlMode import JointImpedenceControlMode - obj = JointImpedenceControlMode() - obj.Init(self._tab.Bytes, x) - return obj - return None - - # KUKAiiwaArmConfiguration - def SmartServoConfig(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(20)) - if o != 0: - x = self._tab.Indirect(o + self._tab.Pos) - from .SmartServo import SmartServo - obj = SmartServo() - obj.Init(self._tab.Bytes, x) - return obj - return None - - # KUKAiiwaArmConfiguration - def FRIConfig(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(22)) - if o != 0: - x = self._tab.Indirect(o + self._tab.Pos) - from .FRI import FRI - obj = FRI() - obj.Init(self._tab.Bytes, x) - return obj - return None - - # KUKAiiwaArmConfiguration - def Tools(self, j): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(24)) - if o != 0: - x = self._tab.Vector(o) - x += flatbuffers.number_types.UOffsetTFlags.py_type(j) * 4 - x = self._tab.Indirect(x) - from .LinkObject import LinkObject - obj = LinkObject() - obj.Init(self._tab.Bytes, x) - return obj - return None - - # KUKAiiwaArmConfiguration - def ToolsLength(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(24)) - if o != 0: - return self._tab.VectorLen(o) - return 0 - -# /// set kuka tablet "processData" panel UI config strings - # KUKAiiwaArmConfiguration - def ProcessData(self, j): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(26)) - if o != 0: - x = self._tab.Vector(o) - x += flatbuffers.number_types.UOffsetTFlags.py_type(j) * 4 - x = self._tab.Indirect(x) - from .ProcessData import ProcessData - obj = ProcessData() - obj.Init(self._tab.Bytes, x) - return obj - return None - - # KUKAiiwaArmConfiguration - def ProcessDataLength(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(26)) - if o != 0: - return self._tab.VectorLen(o) - return 0 - - # KUKAiiwaArmConfiguration - def CurrentMotionCenter(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(28)) - if o != 0: - return self._tab.String(o + self._tab.Pos) - return bytes() - - # KUKAiiwaArmConfiguration - def RequestMonitorProcessData(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(30)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.BoolFlags, o + self._tab.Pos) - return 0 - -def KUKAiiwaArmConfigurationStart(builder): builder.StartObject(14) -def KUKAiiwaArmConfigurationAddName(builder, name): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(name), 0) -def KUKAiiwaArmConfigurationAddCommandInterface(builder, commandInterface): builder.PrependInt8Slot(1, commandInterface, 0) -def KUKAiiwaArmConfigurationAddMonitorInterface(builder, monitorInterface): builder.PrependInt8Slot(2, monitorInterface, 0) -def KUKAiiwaArmConfigurationAddClientCommandMode(builder, clientCommandMode): builder.PrependInt8Slot(3, clientCommandMode, 0) -def KUKAiiwaArmConfigurationAddOverlayType(builder, overlayType): builder.PrependInt8Slot(4, overlayType, 0) -def KUKAiiwaArmConfigurationAddControlMode(builder, controlMode): builder.PrependInt8Slot(5, controlMode, 0) -def KUKAiiwaArmConfigurationAddSetCartImpedance(builder, setCartImpedance): builder.PrependUOffsetTRelativeSlot(6, flatbuffers.number_types.UOffsetTFlags.py_type(setCartImpedance), 0) -def KUKAiiwaArmConfigurationAddSetJointImpedance(builder, setJointImpedance): builder.PrependUOffsetTRelativeSlot(7, flatbuffers.number_types.UOffsetTFlags.py_type(setJointImpedance), 0) -def KUKAiiwaArmConfigurationAddSmartServoConfig(builder, smartServoConfig): builder.PrependUOffsetTRelativeSlot(8, flatbuffers.number_types.UOffsetTFlags.py_type(smartServoConfig), 0) -def KUKAiiwaArmConfigurationAddFRIConfig(builder, FRIConfig): builder.PrependUOffsetTRelativeSlot(9, flatbuffers.number_types.UOffsetTFlags.py_type(FRIConfig), 0) -def KUKAiiwaArmConfigurationAddTools(builder, tools): builder.PrependUOffsetTRelativeSlot(10, flatbuffers.number_types.UOffsetTFlags.py_type(tools), 0) -def KUKAiiwaArmConfigurationStartToolsVector(builder, numElems): return builder.StartVector(4, numElems, 4) -def KUKAiiwaArmConfigurationAddProcessData(builder, processData): builder.PrependUOffsetTRelativeSlot(11, flatbuffers.number_types.UOffsetTFlags.py_type(processData), 0) -def KUKAiiwaArmConfigurationStartProcessDataVector(builder, numElems): return builder.StartVector(4, numElems, 4) -def KUKAiiwaArmConfigurationAddCurrentMotionCenter(builder, currentMotionCenter): builder.PrependUOffsetTRelativeSlot(12, flatbuffers.number_types.UOffsetTFlags.py_type(currentMotionCenter), 0) -def KUKAiiwaArmConfigurationAddRequestMonitorProcessData(builder, requestMonitorProcessData): builder.PrependBoolSlot(13, requestMonitorProcessData, 0) -def KUKAiiwaArmConfigurationEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaFusionTrackMessage.java b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaFusionTrackMessage.java deleted file mode 100644 index 7d956ff..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaFusionTrackMessage.java +++ /dev/null @@ -1,46 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class KUKAiiwaFusionTrackMessage extends Table { - public static KUKAiiwaFusionTrackMessage getRootAsKUKAiiwaFusionTrackMessage(ByteBuffer _bb) { return getRootAsKUKAiiwaFusionTrackMessage(_bb, new KUKAiiwaFusionTrackMessage()); } - public static KUKAiiwaFusionTrackMessage getRootAsKUKAiiwaFusionTrackMessage(ByteBuffer _bb, KUKAiiwaFusionTrackMessage obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public KUKAiiwaFusionTrackMessage __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - public double timestamp() { int o = __offset(4); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } - public TimeEvent timeEvent() { return timeEvent(new TimeEvent()); } - public TimeEvent timeEvent(TimeEvent obj) { int o = __offset(6); return o != 0 ? obj.__assign(__indirect(o + bb_pos), bb) : null; } - public byte deviceStateType() { int o = __offset(8); return o != 0 ? bb.get(o + bb_pos) : 0; } - public Table deviceState(Table obj) { int o = __offset(10); return o != 0 ? __union(obj, o) : null; } - - public static int createKUKAiiwaFusionTrackMessage(FlatBufferBuilder builder, - double timestamp, - int timeEventOffset, - byte deviceState_type, - int deviceStateOffset) { - builder.startObject(4); - KUKAiiwaFusionTrackMessage.addTimestamp(builder, timestamp); - KUKAiiwaFusionTrackMessage.addDeviceState(builder, deviceStateOffset); - KUKAiiwaFusionTrackMessage.addTimeEvent(builder, timeEventOffset); - KUKAiiwaFusionTrackMessage.addDeviceStateType(builder, deviceState_type); - return KUKAiiwaFusionTrackMessage.endKUKAiiwaFusionTrackMessage(builder); - } - - public static void startKUKAiiwaFusionTrackMessage(FlatBufferBuilder builder) { builder.startObject(4); } - public static void addTimestamp(FlatBufferBuilder builder, double timestamp) { builder.addDouble(0, timestamp, 0.0); } - public static void addTimeEvent(FlatBufferBuilder builder, int timeEventOffset) { builder.addOffset(1, timeEventOffset, 0); } - public static void addDeviceStateType(FlatBufferBuilder builder, byte deviceStateType) { builder.addByte(2, deviceStateType, 0); } - public static void addDeviceState(FlatBufferBuilder builder, int deviceStateOffset) { builder.addOffset(3, deviceStateOffset, 0); } - public static int endKUKAiiwaFusionTrackMessage(FlatBufferBuilder builder) { - int o = builder.endObject(); - return o; - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaFusionTrackMessage.py b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaFusionTrackMessage.py deleted file mode 100644 index 78bef25..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaFusionTrackMessage.py +++ /dev/null @@ -1,61 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class KUKAiiwaFusionTrackMessage(object): - __slots__ = ['_tab'] - - @classmethod - def GetRootAsKUKAiiwaFusionTrackMessage(cls, buf, offset): - n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) - x = KUKAiiwaFusionTrackMessage() - x.Init(buf, n + offset) - return x - - # KUKAiiwaFusionTrackMessage - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - - # KUKAiiwaFusionTrackMessage - def Timestamp(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) - return 0.0 - - # KUKAiiwaFusionTrackMessage - def TimeEvent(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) - if o != 0: - x = self._tab.Indirect(o + self._tab.Pos) - from .TimeEvent import TimeEvent - obj = TimeEvent() - obj.Init(self._tab.Bytes, x) - return obj - return None - - # KUKAiiwaFusionTrackMessage - def DeviceStateType(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Uint8Flags, o + self._tab.Pos) - return 0 - - # KUKAiiwaFusionTrackMessage - def DeviceState(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) - if o != 0: - from flatbuffers.table import Table - obj = Table(bytearray(), 0) - self._tab.Union(obj, o) - return obj - return None - -def KUKAiiwaFusionTrackMessageStart(builder): builder.StartObject(4) -def KUKAiiwaFusionTrackMessageAddTimestamp(builder, timestamp): builder.PrependFloat64Slot(0, timestamp, 0.0) -def KUKAiiwaFusionTrackMessageAddTimeEvent(builder, timeEvent): builder.PrependUOffsetTRelativeSlot(1, flatbuffers.number_types.UOffsetTFlags.py_type(timeEvent), 0) -def KUKAiiwaFusionTrackMessageAddDeviceStateType(builder, deviceStateType): builder.PrependUint8Slot(2, deviceStateType, 0) -def KUKAiiwaFusionTrackMessageAddDeviceState(builder, deviceState): builder.PrependUOffsetTRelativeSlot(3, flatbuffers.number_types.UOffsetTFlags.py_type(deviceState), 0) -def KUKAiiwaFusionTrackMessageEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaInterface.java b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaInterface.java deleted file mode 100644 index 2e85894..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaInterface.java +++ /dev/null @@ -1,16 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -public final class KUKAiiwaInterface { - private KUKAiiwaInterface() { } - public static final byte Disabled = 0; - public static final byte SmartServo = 1; - public static final byte DirectServo = 2; - public static final byte FRI = 3; - - public static final String[] names = { "Disabled", "SmartServo", "DirectServo", "FRI", }; - - public static String name(int e) { return names[e]; } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaInterface.py b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaInterface.py deleted file mode 100644 index 64848e0..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaInterface.py +++ /dev/null @@ -1,10 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -class KUKAiiwaInterface(object): - Disabled = 0 - SmartServo = 1 - DirectServo = 2 - FRI = 3 - diff --git a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaMonitorConfiguration.java b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaMonitorConfiguration.java deleted file mode 100644 index edce424..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaMonitorConfiguration.java +++ /dev/null @@ -1,63 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class KUKAiiwaMonitorConfiguration extends Table { - public static KUKAiiwaMonitorConfiguration getRootAsKUKAiiwaMonitorConfiguration(ByteBuffer _bb) { return getRootAsKUKAiiwaMonitorConfiguration(_bb, new KUKAiiwaMonitorConfiguration()); } - public static KUKAiiwaMonitorConfiguration getRootAsKUKAiiwaMonitorConfiguration(ByteBuffer _bb, KUKAiiwaMonitorConfiguration obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public KUKAiiwaMonitorConfiguration __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - public String hardwareVersion() { int o = __offset(4); return o != 0 ? __string(o + bb_pos) : null; } - public ByteBuffer hardwareVersionAsByteBuffer() { return __vector_as_bytebuffer(4, 1); } - public ByteBuffer hardwareVersionInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 4, 1); } - public double torqueSensorLimits(int j) { int o = __offset(6); return o != 0 ? bb.getDouble(__vector(o) + j * 8) : 0; } - public int torqueSensorLimitsLength() { int o = __offset(6); return o != 0 ? __vector_len(o) : 0; } - public ByteBuffer torqueSensorLimitsAsByteBuffer() { return __vector_as_bytebuffer(6, 8); } - public ByteBuffer torqueSensorLimitsInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 6, 8); } - public boolean isReadyToMove() { int o = __offset(8); return o != 0 ? 0!=bb.get(o + bb_pos) : false; } - public boolean isMastered() { int o = __offset(10); return o != 0 ? 0!=bb.get(o + bb_pos) : false; } - /** - * set kuka tablet "processData" panel UI config strings - */ - public ProcessData processData(int j) { return processData(new ProcessData(), j); } - public ProcessData processData(ProcessData obj, int j) { int o = __offset(12); return o != 0 ? obj.__assign(__indirect(__vector(o) + j * 4), bb) : null; } - public int processDataLength() { int o = __offset(12); return o != 0 ? __vector_len(o) : 0; } - - public static int createKUKAiiwaMonitorConfiguration(FlatBufferBuilder builder, - int hardwareVersionOffset, - int torqueSensorLimitsOffset, - boolean isReadyToMove, - boolean isMastered, - int processDataOffset) { - builder.startObject(5); - KUKAiiwaMonitorConfiguration.addProcessData(builder, processDataOffset); - KUKAiiwaMonitorConfiguration.addTorqueSensorLimits(builder, torqueSensorLimitsOffset); - KUKAiiwaMonitorConfiguration.addHardwareVersion(builder, hardwareVersionOffset); - KUKAiiwaMonitorConfiguration.addIsMastered(builder, isMastered); - KUKAiiwaMonitorConfiguration.addIsReadyToMove(builder, isReadyToMove); - return KUKAiiwaMonitorConfiguration.endKUKAiiwaMonitorConfiguration(builder); - } - - public static void startKUKAiiwaMonitorConfiguration(FlatBufferBuilder builder) { builder.startObject(5); } - public static void addHardwareVersion(FlatBufferBuilder builder, int hardwareVersionOffset) { builder.addOffset(0, hardwareVersionOffset, 0); } - public static void addTorqueSensorLimits(FlatBufferBuilder builder, int torqueSensorLimitsOffset) { builder.addOffset(1, torqueSensorLimitsOffset, 0); } - public static int createTorqueSensorLimitsVector(FlatBufferBuilder builder, double[] data) { builder.startVector(8, data.length, 8); for (int i = data.length - 1; i >= 0; i--) builder.addDouble(data[i]); return builder.endVector(); } - public static void startTorqueSensorLimitsVector(FlatBufferBuilder builder, int numElems) { builder.startVector(8, numElems, 8); } - public static void addIsReadyToMove(FlatBufferBuilder builder, boolean isReadyToMove) { builder.addBoolean(2, isReadyToMove, false); } - public static void addIsMastered(FlatBufferBuilder builder, boolean isMastered) { builder.addBoolean(3, isMastered, false); } - public static void addProcessData(FlatBufferBuilder builder, int processDataOffset) { builder.addOffset(4, processDataOffset, 0); } - public static int createProcessDataVector(FlatBufferBuilder builder, int[] data) { builder.startVector(4, data.length, 4); for (int i = data.length - 1; i >= 0; i--) builder.addOffset(data[i]); return builder.endVector(); } - public static void startProcessDataVector(FlatBufferBuilder builder, int numElems) { builder.startVector(4, numElems, 4); } - public static int endKUKAiiwaMonitorConfiguration(FlatBufferBuilder builder) { - int o = builder.endObject(); - return o; - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaMonitorConfiguration.py b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaMonitorConfiguration.py deleted file mode 100644 index 6c2201a..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaMonitorConfiguration.py +++ /dev/null @@ -1,93 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class KUKAiiwaMonitorConfiguration(object): - __slots__ = ['_tab'] - - @classmethod - def GetRootAsKUKAiiwaMonitorConfiguration(cls, buf, offset): - n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) - x = KUKAiiwaMonitorConfiguration() - x.Init(buf, n + offset) - return x - - # KUKAiiwaMonitorConfiguration - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - - # KUKAiiwaMonitorConfiguration - def HardwareVersion(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - return self._tab.String(o + self._tab.Pos) - return bytes() - - # KUKAiiwaMonitorConfiguration - def TorqueSensorLimits(self, j): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) - if o != 0: - a = self._tab.Vector(o) - return self._tab.Get(flatbuffers.number_types.Float64Flags, a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 8)) - return 0 - - # KUKAiiwaMonitorConfiguration - def TorqueSensorLimitsAsNumpy(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) - if o != 0: - return self._tab.GetVectorAsNumpy(flatbuffers.number_types.Float64Flags, o) - return 0 - - # KUKAiiwaMonitorConfiguration - def TorqueSensorLimitsLength(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) - if o != 0: - return self._tab.VectorLen(o) - return 0 - - # KUKAiiwaMonitorConfiguration - def IsReadyToMove(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.BoolFlags, o + self._tab.Pos) - return 0 - - # KUKAiiwaMonitorConfiguration - def IsMastered(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.BoolFlags, o + self._tab.Pos) - return 0 - -# /// set kuka tablet "processData" panel UI config strings - # KUKAiiwaMonitorConfiguration - def ProcessData(self, j): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(12)) - if o != 0: - x = self._tab.Vector(o) - x += flatbuffers.number_types.UOffsetTFlags.py_type(j) * 4 - x = self._tab.Indirect(x) - from .ProcessData import ProcessData - obj = ProcessData() - obj.Init(self._tab.Bytes, x) - return obj - return None - - # KUKAiiwaMonitorConfiguration - def ProcessDataLength(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(12)) - if o != 0: - return self._tab.VectorLen(o) - return 0 - -def KUKAiiwaMonitorConfigurationStart(builder): builder.StartObject(5) -def KUKAiiwaMonitorConfigurationAddHardwareVersion(builder, hardwareVersion): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(hardwareVersion), 0) -def KUKAiiwaMonitorConfigurationAddTorqueSensorLimits(builder, torqueSensorLimits): builder.PrependUOffsetTRelativeSlot(1, flatbuffers.number_types.UOffsetTFlags.py_type(torqueSensorLimits), 0) -def KUKAiiwaMonitorConfigurationStartTorqueSensorLimitsVector(builder, numElems): return builder.StartVector(8, numElems, 8) -def KUKAiiwaMonitorConfigurationAddIsReadyToMove(builder, isReadyToMove): builder.PrependBoolSlot(2, isReadyToMove, 0) -def KUKAiiwaMonitorConfigurationAddIsMastered(builder, isMastered): builder.PrependBoolSlot(3, isMastered, 0) -def KUKAiiwaMonitorConfigurationAddProcessData(builder, processData): builder.PrependUOffsetTRelativeSlot(4, flatbuffers.number_types.UOffsetTFlags.py_type(processData), 0) -def KUKAiiwaMonitorConfigurationStartProcessDataVector(builder, numElems): return builder.StartVector(4, numElems, 4) -def KUKAiiwaMonitorConfigurationEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaMonitorState.java b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaMonitorState.java deleted file mode 100644 index 524ab79..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaMonitorState.java +++ /dev/null @@ -1,57 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class KUKAiiwaMonitorState extends Table { - public static KUKAiiwaMonitorState getRootAsKUKAiiwaMonitorState(ByteBuffer _bb) { return getRootAsKUKAiiwaMonitorState(_bb, new KUKAiiwaMonitorState()); } - public static KUKAiiwaMonitorState getRootAsKUKAiiwaMonitorState(ByteBuffer _bb, KUKAiiwaMonitorState obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public KUKAiiwaMonitorState __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - public JointState measuredState() { return measuredState(new JointState()); } - public JointState measuredState(JointState obj) { int o = __offset(4); return o != 0 ? obj.__assign(__indirect(o + bb_pos), bb) : null; } - public Pose cartesianFlangePose() { return cartesianFlangePose(new Pose()); } - public Pose cartesianFlangePose(Pose obj) { int o = __offset(6); return o != 0 ? obj.__assign(o + bb_pos, bb) : null; } - public JointState jointStateReal() { return jointStateReal(new JointState()); } - public JointState jointStateReal(JointState obj) { int o = __offset(8); return o != 0 ? obj.__assign(__indirect(o + bb_pos), bb) : null; } - public JointState jointStateInterpolated() { return jointStateInterpolated(new JointState()); } - public JointState jointStateInterpolated(JointState obj) { int o = __offset(10); return o != 0 ? obj.__assign(__indirect(o + bb_pos), bb) : null; } - /** - * The state of the arm as calculated by kuka after - * subtracting the known weights of the arm - * and any attachments configured to be present. - * - * Most likely only contains torque. - * KukaState::ExternalTorque goes here - */ - public JointState externalState() { return externalState(new JointState()); } - public JointState externalState(JointState obj) { int o = __offset(12); return o != 0 ? obj.__assign(__indirect(o + bb_pos), bb) : null; } - /** - * KUKA::FRI::EOperationMode - */ - public byte operationMode() { int o = __offset(14); return o != 0 ? bb.get(o + bb_pos) : 0; } - public byte sessionState() { int o = __offset(16); return o != 0 ? bb.get(o + bb_pos) : 0; } - public Wrench CartesianWrench() { return CartesianWrench(new Wrench()); } - public Wrench CartesianWrench(Wrench obj) { int o = __offset(18); return o != 0 ? obj.__assign(o + bb_pos, bb) : null; } - - public static void startKUKAiiwaMonitorState(FlatBufferBuilder builder) { builder.startObject(8); } - public static void addMeasuredState(FlatBufferBuilder builder, int measuredStateOffset) { builder.addOffset(0, measuredStateOffset, 0); } - public static void addCartesianFlangePose(FlatBufferBuilder builder, int cartesianFlangePoseOffset) { builder.addStruct(1, cartesianFlangePoseOffset, 0); } - public static void addJointStateReal(FlatBufferBuilder builder, int jointStateRealOffset) { builder.addOffset(2, jointStateRealOffset, 0); } - public static void addJointStateInterpolated(FlatBufferBuilder builder, int jointStateInterpolatedOffset) { builder.addOffset(3, jointStateInterpolatedOffset, 0); } - public static void addExternalState(FlatBufferBuilder builder, int externalStateOffset) { builder.addOffset(4, externalStateOffset, 0); } - public static void addOperationMode(FlatBufferBuilder builder, byte operationMode) { builder.addByte(5, operationMode, 0); } - public static void addSessionState(FlatBufferBuilder builder, byte sessionState) { builder.addByte(6, sessionState, 0); } - public static void addCartesianWrench(FlatBufferBuilder builder, int CartesianWrenchOffset) { builder.addStruct(7, CartesianWrenchOffset, 0); } - public static int endKUKAiiwaMonitorState(FlatBufferBuilder builder) { - int o = builder.endObject(); - return o; - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaMonitorState.py b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaMonitorState.py deleted file mode 100644 index 601c1d1..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaMonitorState.py +++ /dev/null @@ -1,117 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class KUKAiiwaMonitorState(object): - __slots__ = ['_tab'] - - @classmethod - def GetRootAsKUKAiiwaMonitorState(cls, buf, offset): - n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) - x = KUKAiiwaMonitorState() - x.Init(buf, n + offset) - return x - - # KUKAiiwaMonitorState - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - - # KUKAiiwaMonitorState - def MeasuredState(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - x = self._tab.Indirect(o + self._tab.Pos) - from .JointState import JointState - obj = JointState() - obj.Init(self._tab.Bytes, x) - return obj - return None - - # KUKAiiwaMonitorState - def CartesianFlangePose(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) - if o != 0: - x = o + self._tab.Pos - from .Pose import Pose - obj = Pose() - obj.Init(self._tab.Bytes, x) - return obj - return None - - # KUKAiiwaMonitorState - def JointStateReal(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) - if o != 0: - x = self._tab.Indirect(o + self._tab.Pos) - from .JointState import JointState - obj = JointState() - obj.Init(self._tab.Bytes, x) - return obj - return None - - # KUKAiiwaMonitorState - def JointStateInterpolated(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) - if o != 0: - x = self._tab.Indirect(o + self._tab.Pos) - from .JointState import JointState - obj = JointState() - obj.Init(self._tab.Bytes, x) - return obj - return None - -# /// The state of the arm as calculated by kuka after -# /// subtracting the known weights of the arm -# /// and any attachments configured to be present. -# /// -# /// Most likely only contains torque. -# /// KukaState::ExternalTorque goes here - # KUKAiiwaMonitorState - def ExternalState(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(12)) - if o != 0: - x = self._tab.Indirect(o + self._tab.Pos) - from .JointState import JointState - obj = JointState() - obj.Init(self._tab.Bytes, x) - return obj - return None - -# /// KUKA::FRI::EOperationMode - # KUKAiiwaMonitorState - def OperationMode(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(14)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Int8Flags, o + self._tab.Pos) - return 0 - - # KUKAiiwaMonitorState - def SessionState(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(16)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Int8Flags, o + self._tab.Pos) - return 0 - - # KUKAiiwaMonitorState - def CartesianWrench(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(18)) - if o != 0: - x = o + self._tab.Pos - from .Wrench import Wrench - obj = Wrench() - obj.Init(self._tab.Bytes, x) - return obj - return None - -def KUKAiiwaMonitorStateStart(builder): builder.StartObject(8) -def KUKAiiwaMonitorStateAddMeasuredState(builder, measuredState): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(measuredState), 0) -def KUKAiiwaMonitorStateAddCartesianFlangePose(builder, cartesianFlangePose): builder.PrependStructSlot(1, flatbuffers.number_types.UOffsetTFlags.py_type(cartesianFlangePose), 0) -def KUKAiiwaMonitorStateAddJointStateReal(builder, jointStateReal): builder.PrependUOffsetTRelativeSlot(2, flatbuffers.number_types.UOffsetTFlags.py_type(jointStateReal), 0) -def KUKAiiwaMonitorStateAddJointStateInterpolated(builder, jointStateInterpolated): builder.PrependUOffsetTRelativeSlot(3, flatbuffers.number_types.UOffsetTFlags.py_type(jointStateInterpolated), 0) -def KUKAiiwaMonitorStateAddExternalState(builder, externalState): builder.PrependUOffsetTRelativeSlot(4, flatbuffers.number_types.UOffsetTFlags.py_type(externalState), 0) -def KUKAiiwaMonitorStateAddOperationMode(builder, operationMode): builder.PrependInt8Slot(5, operationMode, 0) -def KUKAiiwaMonitorStateAddSessionState(builder, sessionState): builder.PrependInt8Slot(6, sessionState, 0) -def KUKAiiwaMonitorStateAddCartesianWrench(builder, CartesianWrench): builder.PrependStructSlot(7, flatbuffers.number_types.UOffsetTFlags.py_type(CartesianWrench), 0) -def KUKAiiwaMonitorStateEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaState.java b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaState.java deleted file mode 100644 index 5e9d7f3..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaState.java +++ /dev/null @@ -1,93 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class KUKAiiwaState extends Table { - public static KUKAiiwaState getRootAsKUKAiiwaState(ByteBuffer _bb) { return getRootAsKUKAiiwaState(_bb, new KUKAiiwaState()); } - public static KUKAiiwaState getRootAsKUKAiiwaState(ByteBuffer _bb, KUKAiiwaState obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public KUKAiiwaState __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - public String name() { int o = __offset(4); return o != 0 ? __string(o + bb_pos) : null; } - public ByteBuffer nameAsByteBuffer() { return __vector_as_bytebuffer(4, 1); } - public ByteBuffer nameInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 4, 1); } - public String destination() { int o = __offset(6); return o != 0 ? __string(o + bb_pos) : null; } - public ByteBuffer destinationAsByteBuffer() { return __vector_as_bytebuffer(6, 1); } - public ByteBuffer destinationInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 6, 1); } - public String source() { int o = __offset(8); return o != 0 ? __string(o + bb_pos) : null; } - public ByteBuffer sourceAsByteBuffer() { return __vector_as_bytebuffer(8, 1); } - public ByteBuffer sourceInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 8, 1); } - public TimeEvent timeStamp() { return timeStamp(new TimeEvent()); } - public TimeEvent timeStamp(TimeEvent obj) { int o = __offset(10); return o != 0 ? obj.__assign(__indirect(o + bb_pos), bb) : null; } - public boolean setArmControlState() { int o = __offset(12); return o != 0 ? 0!=bb.get(o + bb_pos) : false; } - public ArmControlState armControlState() { return armControlState(new ArmControlState()); } - public ArmControlState armControlState(ArmControlState obj) { int o = __offset(14); return o != 0 ? obj.__assign(__indirect(o + bb_pos), bb) : null; } - public boolean setArmConfiguration() { int o = __offset(16); return o != 0 ? 0!=bb.get(o + bb_pos) : false; } - public KUKAiiwaArmConfiguration armConfiguration() { return armConfiguration(new KUKAiiwaArmConfiguration()); } - public KUKAiiwaArmConfiguration armConfiguration(KUKAiiwaArmConfiguration obj) { int o = __offset(18); return o != 0 ? obj.__assign(__indirect(o + bb_pos), bb) : null; } - public boolean hasMonitorState() { int o = __offset(20); return o != 0 ? 0!=bb.get(o + bb_pos) : false; } - public KUKAiiwaMonitorState monitorState() { return monitorState(new KUKAiiwaMonitorState()); } - public KUKAiiwaMonitorState monitorState(KUKAiiwaMonitorState obj) { int o = __offset(22); return o != 0 ? obj.__assign(__indirect(o + bb_pos), bb) : null; } - public boolean hasMonitorConfig() { int o = __offset(24); return o != 0 ? 0!=bb.get(o + bb_pos) : false; } - public KUKAiiwaMonitorConfiguration monitorConfig() { return monitorConfig(new KUKAiiwaMonitorConfiguration()); } - public KUKAiiwaMonitorConfiguration monitorConfig(KUKAiiwaMonitorConfiguration obj) { int o = __offset(26); return o != 0 ? obj.__assign(__indirect(o + bb_pos), bb) : null; } - public FRIMessageLog FRIMessage() { return FRIMessage(new FRIMessageLog()); } - public FRIMessageLog FRIMessage(FRIMessageLog obj) { int o = __offset(28); return o != 0 ? obj.__assign(__indirect(o + bb_pos), bb) : null; } - - public static int createKUKAiiwaState(FlatBufferBuilder builder, - int nameOffset, - int destinationOffset, - int sourceOffset, - int timeStampOffset, - boolean setArmControlState, - int armControlStateOffset, - boolean setArmConfiguration, - int armConfigurationOffset, - boolean hasMonitorState, - int monitorStateOffset, - boolean hasMonitorConfig, - int monitorConfigOffset, - int FRIMessageOffset) { - builder.startObject(13); - KUKAiiwaState.addFRIMessage(builder, FRIMessageOffset); - KUKAiiwaState.addMonitorConfig(builder, monitorConfigOffset); - KUKAiiwaState.addMonitorState(builder, monitorStateOffset); - KUKAiiwaState.addArmConfiguration(builder, armConfigurationOffset); - KUKAiiwaState.addArmControlState(builder, armControlStateOffset); - KUKAiiwaState.addTimeStamp(builder, timeStampOffset); - KUKAiiwaState.addSource(builder, sourceOffset); - KUKAiiwaState.addDestination(builder, destinationOffset); - KUKAiiwaState.addName(builder, nameOffset); - KUKAiiwaState.addHasMonitorConfig(builder, hasMonitorConfig); - KUKAiiwaState.addHasMonitorState(builder, hasMonitorState); - KUKAiiwaState.addSetArmConfiguration(builder, setArmConfiguration); - KUKAiiwaState.addSetArmControlState(builder, setArmControlState); - return KUKAiiwaState.endKUKAiiwaState(builder); - } - - public static void startKUKAiiwaState(FlatBufferBuilder builder) { builder.startObject(13); } - public static void addName(FlatBufferBuilder builder, int nameOffset) { builder.addOffset(0, nameOffset, 0); } - public static void addDestination(FlatBufferBuilder builder, int destinationOffset) { builder.addOffset(1, destinationOffset, 0); } - public static void addSource(FlatBufferBuilder builder, int sourceOffset) { builder.addOffset(2, sourceOffset, 0); } - public static void addTimeStamp(FlatBufferBuilder builder, int timeStampOffset) { builder.addOffset(3, timeStampOffset, 0); } - public static void addSetArmControlState(FlatBufferBuilder builder, boolean setArmControlState) { builder.addBoolean(4, setArmControlState, false); } - public static void addArmControlState(FlatBufferBuilder builder, int armControlStateOffset) { builder.addOffset(5, armControlStateOffset, 0); } - public static void addSetArmConfiguration(FlatBufferBuilder builder, boolean setArmConfiguration) { builder.addBoolean(6, setArmConfiguration, false); } - public static void addArmConfiguration(FlatBufferBuilder builder, int armConfigurationOffset) { builder.addOffset(7, armConfigurationOffset, 0); } - public static void addHasMonitorState(FlatBufferBuilder builder, boolean hasMonitorState) { builder.addBoolean(8, hasMonitorState, false); } - public static void addMonitorState(FlatBufferBuilder builder, int monitorStateOffset) { builder.addOffset(9, monitorStateOffset, 0); } - public static void addHasMonitorConfig(FlatBufferBuilder builder, boolean hasMonitorConfig) { builder.addBoolean(10, hasMonitorConfig, false); } - public static void addMonitorConfig(FlatBufferBuilder builder, int monitorConfigOffset) { builder.addOffset(11, monitorConfigOffset, 0); } - public static void addFRIMessage(FlatBufferBuilder builder, int FRIMessageOffset) { builder.addOffset(12, FRIMessageOffset, 0); } - public static int endKUKAiiwaState(FlatBufferBuilder builder) { - int o = builder.endObject(); - return o; - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaState.py b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaState.py deleted file mode 100644 index 69de57a..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaState.py +++ /dev/null @@ -1,150 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class KUKAiiwaState(object): - __slots__ = ['_tab'] - - @classmethod - def GetRootAsKUKAiiwaState(cls, buf, offset): - n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) - x = KUKAiiwaState() - x.Init(buf, n + offset) - return x - - # KUKAiiwaState - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - - # KUKAiiwaState - def Name(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - return self._tab.String(o + self._tab.Pos) - return bytes() - - # KUKAiiwaState - def Destination(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) - if o != 0: - return self._tab.String(o + self._tab.Pos) - return bytes() - - # KUKAiiwaState - def Source(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) - if o != 0: - return self._tab.String(o + self._tab.Pos) - return bytes() - - # KUKAiiwaState - def TimeStamp(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) - if o != 0: - x = self._tab.Indirect(o + self._tab.Pos) - from .TimeEvent import TimeEvent - obj = TimeEvent() - obj.Init(self._tab.Bytes, x) - return obj - return None - - # KUKAiiwaState - def SetArmControlState(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(12)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.BoolFlags, o + self._tab.Pos) - return 0 - - # KUKAiiwaState - def ArmControlState(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(14)) - if o != 0: - x = self._tab.Indirect(o + self._tab.Pos) - from .ArmControlState import ArmControlState - obj = ArmControlState() - obj.Init(self._tab.Bytes, x) - return obj - return None - - # KUKAiiwaState - def SetArmConfiguration(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(16)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.BoolFlags, o + self._tab.Pos) - return 0 - - # KUKAiiwaState - def ArmConfiguration(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(18)) - if o != 0: - x = self._tab.Indirect(o + self._tab.Pos) - from .KUKAiiwaArmConfiguration import KUKAiiwaArmConfiguration - obj = KUKAiiwaArmConfiguration() - obj.Init(self._tab.Bytes, x) - return obj - return None - - # KUKAiiwaState - def HasMonitorState(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(20)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.BoolFlags, o + self._tab.Pos) - return 0 - - # KUKAiiwaState - def MonitorState(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(22)) - if o != 0: - x = self._tab.Indirect(o + self._tab.Pos) - from .KUKAiiwaMonitorState import KUKAiiwaMonitorState - obj = KUKAiiwaMonitorState() - obj.Init(self._tab.Bytes, x) - return obj - return None - - # KUKAiiwaState - def HasMonitorConfig(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(24)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.BoolFlags, o + self._tab.Pos) - return 0 - - # KUKAiiwaState - def MonitorConfig(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(26)) - if o != 0: - x = self._tab.Indirect(o + self._tab.Pos) - from .KUKAiiwaMonitorConfiguration import KUKAiiwaMonitorConfiguration - obj = KUKAiiwaMonitorConfiguration() - obj.Init(self._tab.Bytes, x) - return obj - return None - - # KUKAiiwaState - def FRIMessage(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(28)) - if o != 0: - x = self._tab.Indirect(o + self._tab.Pos) - from .FRIMessageLog import FRIMessageLog - obj = FRIMessageLog() - obj.Init(self._tab.Bytes, x) - return obj - return None - -def KUKAiiwaStateStart(builder): builder.StartObject(13) -def KUKAiiwaStateAddName(builder, name): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(name), 0) -def KUKAiiwaStateAddDestination(builder, destination): builder.PrependUOffsetTRelativeSlot(1, flatbuffers.number_types.UOffsetTFlags.py_type(destination), 0) -def KUKAiiwaStateAddSource(builder, source): builder.PrependUOffsetTRelativeSlot(2, flatbuffers.number_types.UOffsetTFlags.py_type(source), 0) -def KUKAiiwaStateAddTimeStamp(builder, timeStamp): builder.PrependUOffsetTRelativeSlot(3, flatbuffers.number_types.UOffsetTFlags.py_type(timeStamp), 0) -def KUKAiiwaStateAddSetArmControlState(builder, setArmControlState): builder.PrependBoolSlot(4, setArmControlState, 0) -def KUKAiiwaStateAddArmControlState(builder, armControlState): builder.PrependUOffsetTRelativeSlot(5, flatbuffers.number_types.UOffsetTFlags.py_type(armControlState), 0) -def KUKAiiwaStateAddSetArmConfiguration(builder, setArmConfiguration): builder.PrependBoolSlot(6, setArmConfiguration, 0) -def KUKAiiwaStateAddArmConfiguration(builder, armConfiguration): builder.PrependUOffsetTRelativeSlot(7, flatbuffers.number_types.UOffsetTFlags.py_type(armConfiguration), 0) -def KUKAiiwaStateAddHasMonitorState(builder, hasMonitorState): builder.PrependBoolSlot(8, hasMonitorState, 0) -def KUKAiiwaStateAddMonitorState(builder, monitorState): builder.PrependUOffsetTRelativeSlot(9, flatbuffers.number_types.UOffsetTFlags.py_type(monitorState), 0) -def KUKAiiwaStateAddHasMonitorConfig(builder, hasMonitorConfig): builder.PrependBoolSlot(10, hasMonitorConfig, 0) -def KUKAiiwaStateAddMonitorConfig(builder, monitorConfig): builder.PrependUOffsetTRelativeSlot(11, flatbuffers.number_types.UOffsetTFlags.py_type(monitorConfig), 0) -def KUKAiiwaStateAddFRIMessage(builder, FRIMessage): builder.PrependUOffsetTRelativeSlot(12, flatbuffers.number_types.UOffsetTFlags.py_type(FRIMessage), 0) -def KUKAiiwaStateEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaStates.java b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaStates.java deleted file mode 100644 index 8930bdf..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaStates.java +++ /dev/null @@ -1,40 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class KUKAiiwaStates extends Table { - public static KUKAiiwaStates getRootAsKUKAiiwaStates(ByteBuffer _bb) { return getRootAsKUKAiiwaStates(_bb, new KUKAiiwaStates()); } - public static KUKAiiwaStates getRootAsKUKAiiwaStates(ByteBuffer _bb, KUKAiiwaStates obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } - public static boolean KUKAiiwaStatesBufferHasIdentifier(ByteBuffer _bb) { return __has_identifier(_bb, "iiwa"); } - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public KUKAiiwaStates __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - public KUKAiiwaState states(int j) { return states(new KUKAiiwaState(), j); } - public KUKAiiwaState states(KUKAiiwaState obj, int j) { int o = __offset(4); return o != 0 ? obj.__assign(__indirect(__vector(o) + j * 4), bb) : null; } - public int statesLength() { int o = __offset(4); return o != 0 ? __vector_len(o) : 0; } - - public static int createKUKAiiwaStates(FlatBufferBuilder builder, - int statesOffset) { - builder.startObject(1); - KUKAiiwaStates.addStates(builder, statesOffset); - return KUKAiiwaStates.endKUKAiiwaStates(builder); - } - - public static void startKUKAiiwaStates(FlatBufferBuilder builder) { builder.startObject(1); } - public static void addStates(FlatBufferBuilder builder, int statesOffset) { builder.addOffset(0, statesOffset, 0); } - public static int createStatesVector(FlatBufferBuilder builder, int[] data) { builder.startVector(4, data.length, 4); for (int i = data.length - 1; i >= 0; i--) builder.addOffset(data[i]); return builder.endVector(); } - public static void startStatesVector(FlatBufferBuilder builder, int numElems) { builder.startVector(4, numElems, 4); } - public static int endKUKAiiwaStates(FlatBufferBuilder builder) { - int o = builder.endObject(); - return o; - } - public static void finishKUKAiiwaStatesBuffer(FlatBufferBuilder builder, int offset) { builder.finish(offset, "iiwa"); } - public static void finishSizePrefixedKUKAiiwaStatesBuffer(FlatBufferBuilder builder, int offset) { builder.finishSizePrefixed(offset, "iiwa"); } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaStates.py b/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaStates.py deleted file mode 100644 index dfeab00..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/KUKAiiwaStates.py +++ /dev/null @@ -1,44 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class KUKAiiwaStates(object): - __slots__ = ['_tab'] - - @classmethod - def GetRootAsKUKAiiwaStates(cls, buf, offset): - n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) - x = KUKAiiwaStates() - x.Init(buf, n + offset) - return x - - # KUKAiiwaStates - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - - # KUKAiiwaStates - def States(self, j): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - x = self._tab.Vector(o) - x += flatbuffers.number_types.UOffsetTFlags.py_type(j) * 4 - x = self._tab.Indirect(x) - from .KUKAiiwaState import KUKAiiwaState - obj = KUKAiiwaState() - obj.Init(self._tab.Bytes, x) - return obj - return None - - # KUKAiiwaStates - def StatesLength(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - return self._tab.VectorLen(o) - return 0 - -def KUKAiiwaStatesStart(builder): builder.StartObject(1) -def KUKAiiwaStatesAddStates(builder, states): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(states), 0) -def KUKAiiwaStatesStartStatesVector(builder, numElems): return builder.StartVector(4, numElems, 4) -def KUKAiiwaStatesEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/LinkObject.java b/include/grl/flatbuffer/grl/flatbuffer/LinkObject.java deleted file mode 100644 index 6332388..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/LinkObject.java +++ /dev/null @@ -1,38 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class LinkObject extends Table { - public static LinkObject getRootAsLinkObject(ByteBuffer _bb) { return getRootAsLinkObject(_bb, new LinkObject()); } - public static LinkObject getRootAsLinkObject(ByteBuffer _bb, LinkObject obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public LinkObject __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - public String name() { int o = __offset(4); return o != 0 ? __string(o + bb_pos) : null; } - public ByteBuffer nameAsByteBuffer() { return __vector_as_bytebuffer(4, 1); } - public ByteBuffer nameInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 4, 1); } - public String parent() { int o = __offset(6); return o != 0 ? __string(o + bb_pos) : null; } - public ByteBuffer parentAsByteBuffer() { return __vector_as_bytebuffer(6, 1); } - public ByteBuffer parentInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 6, 1); } - public Pose pose() { return pose(new Pose()); } - public Pose pose(Pose obj) { int o = __offset(8); return o != 0 ? obj.__assign(o + bb_pos, bb) : null; } - public Inertia inertia() { return inertia(new Inertia()); } - public Inertia inertia(Inertia obj) { int o = __offset(10); return o != 0 ? obj.__assign(o + bb_pos, bb) : null; } - - public static void startLinkObject(FlatBufferBuilder builder) { builder.startObject(4); } - public static void addName(FlatBufferBuilder builder, int nameOffset) { builder.addOffset(0, nameOffset, 0); } - public static void addParent(FlatBufferBuilder builder, int parentOffset) { builder.addOffset(1, parentOffset, 0); } - public static void addPose(FlatBufferBuilder builder, int poseOffset) { builder.addStruct(2, poseOffset, 0); } - public static void addInertia(FlatBufferBuilder builder, int inertiaOffset) { builder.addStruct(3, inertiaOffset, 0); } - public static int endLinkObject(FlatBufferBuilder builder) { - int o = builder.endObject(); - return o; - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/LinkObject.py b/include/grl/flatbuffer/grl/flatbuffer/LinkObject.py deleted file mode 100644 index e61d17e..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/LinkObject.py +++ /dev/null @@ -1,62 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class LinkObject(object): - __slots__ = ['_tab'] - - @classmethod - def GetRootAsLinkObject(cls, buf, offset): - n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) - x = LinkObject() - x.Init(buf, n + offset) - return x - - # LinkObject - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - - # LinkObject - def Name(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - return self._tab.String(o + self._tab.Pos) - return bytes() - - # LinkObject - def Parent(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) - if o != 0: - return self._tab.String(o + self._tab.Pos) - return bytes() - - # LinkObject - def Pose(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) - if o != 0: - x = o + self._tab.Pos - from .Pose import Pose - obj = Pose() - obj.Init(self._tab.Bytes, x) - return obj - return None - - # LinkObject - def Inertia(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) - if o != 0: - x = o + self._tab.Pos - from .Inertia import Inertia - obj = Inertia() - obj.Init(self._tab.Bytes, x) - return obj - return None - -def LinkObjectStart(builder): builder.StartObject(4) -def LinkObjectAddName(builder, name): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(name), 0) -def LinkObjectAddParent(builder, parent): builder.PrependUOffsetTRelativeSlot(1, flatbuffers.number_types.UOffsetTFlags.py_type(parent), 0) -def LinkObjectAddPose(builder, pose): builder.PrependStructSlot(2, flatbuffers.number_types.UOffsetTFlags.py_type(pose), 0) -def LinkObjectAddInertia(builder, inertia): builder.PrependStructSlot(3, flatbuffers.number_types.UOffsetTFlags.py_type(inertia), 0) -def LinkObjectEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/LogKUKAiiwaFusionTrack.java b/include/grl/flatbuffer/grl/flatbuffer/LogKUKAiiwaFusionTrack.java deleted file mode 100644 index ace360d..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/LogKUKAiiwaFusionTrack.java +++ /dev/null @@ -1,40 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class LogKUKAiiwaFusionTrack extends Table { - public static LogKUKAiiwaFusionTrack getRootAsLogKUKAiiwaFusionTrack(ByteBuffer _bb) { return getRootAsLogKUKAiiwaFusionTrack(_bb, new LogKUKAiiwaFusionTrack()); } - public static LogKUKAiiwaFusionTrack getRootAsLogKUKAiiwaFusionTrack(ByteBuffer _bb, LogKUKAiiwaFusionTrack obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } - public static boolean LogKUKAiiwaFusionTrackBufferHasIdentifier(ByteBuffer _bb) { return __has_identifier(_bb, "flik"); } - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public LogKUKAiiwaFusionTrack __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - public KUKAiiwaFusionTrackMessage states(int j) { return states(new KUKAiiwaFusionTrackMessage(), j); } - public KUKAiiwaFusionTrackMessage states(KUKAiiwaFusionTrackMessage obj, int j) { int o = __offset(4); return o != 0 ? obj.__assign(__indirect(__vector(o) + j * 4), bb) : null; } - public int statesLength() { int o = __offset(4); return o != 0 ? __vector_len(o) : 0; } - - public static int createLogKUKAiiwaFusionTrack(FlatBufferBuilder builder, - int statesOffset) { - builder.startObject(1); - LogKUKAiiwaFusionTrack.addStates(builder, statesOffset); - return LogKUKAiiwaFusionTrack.endLogKUKAiiwaFusionTrack(builder); - } - - public static void startLogKUKAiiwaFusionTrack(FlatBufferBuilder builder) { builder.startObject(1); } - public static void addStates(FlatBufferBuilder builder, int statesOffset) { builder.addOffset(0, statesOffset, 0); } - public static int createStatesVector(FlatBufferBuilder builder, int[] data) { builder.startVector(4, data.length, 4); for (int i = data.length - 1; i >= 0; i--) builder.addOffset(data[i]); return builder.endVector(); } - public static void startStatesVector(FlatBufferBuilder builder, int numElems) { builder.startVector(4, numElems, 4); } - public static int endLogKUKAiiwaFusionTrack(FlatBufferBuilder builder) { - int o = builder.endObject(); - return o; - } - public static void finishLogKUKAiiwaFusionTrackBuffer(FlatBufferBuilder builder, int offset) { builder.finish(offset, "flik"); } - public static void finishSizePrefixedLogKUKAiiwaFusionTrackBuffer(FlatBufferBuilder builder, int offset) { builder.finishSizePrefixed(offset, "flik"); } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/LogKUKAiiwaFusionTrack.py b/include/grl/flatbuffer/grl/flatbuffer/LogKUKAiiwaFusionTrack.py deleted file mode 100644 index 5c223a6..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/LogKUKAiiwaFusionTrack.py +++ /dev/null @@ -1,44 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class LogKUKAiiwaFusionTrack(object): - __slots__ = ['_tab'] - - @classmethod - def GetRootAsLogKUKAiiwaFusionTrack(cls, buf, offset): - n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) - x = LogKUKAiiwaFusionTrack() - x.Init(buf, n + offset) - return x - - # LogKUKAiiwaFusionTrack - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - - # LogKUKAiiwaFusionTrack - def States(self, j): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - x = self._tab.Vector(o) - x += flatbuffers.number_types.UOffsetTFlags.py_type(j) * 4 - x = self._tab.Indirect(x) - from .KUKAiiwaFusionTrackMessage import KUKAiiwaFusionTrackMessage - obj = KUKAiiwaFusionTrackMessage() - obj.Init(self._tab.Bytes, x) - return obj - return None - - # LogKUKAiiwaFusionTrack - def StatesLength(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - return self._tab.VectorLen(o) - return 0 - -def LogKUKAiiwaFusionTrackStart(builder): builder.StartObject(1) -def LogKUKAiiwaFusionTrackAddStates(builder, states): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(states), 0) -def LogKUKAiiwaFusionTrackStartStatesVector(builder, numElems): return builder.StartVector(4, numElems, 4) -def LogKUKAiiwaFusionTrackEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/MoveArmCartesianServo.java b/include/grl/flatbuffer/grl/flatbuffer/MoveArmCartesianServo.java deleted file mode 100644 index 3e0a78f..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/MoveArmCartesianServo.java +++ /dev/null @@ -1,31 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class MoveArmCartesianServo extends Table { - public static MoveArmCartesianServo getRootAsMoveArmCartesianServo(ByteBuffer _bb) { return getRootAsMoveArmCartesianServo(_bb, new MoveArmCartesianServo()); } - public static MoveArmCartesianServo getRootAsMoveArmCartesianServo(ByteBuffer _bb, MoveArmCartesianServo obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public MoveArmCartesianServo __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - public String parent() { int o = __offset(4); return o != 0 ? __string(o + bb_pos) : null; } - public ByteBuffer parentAsByteBuffer() { return __vector_as_bytebuffer(4, 1); } - public ByteBuffer parentInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 4, 1); } - public Pose goal() { return goal(new Pose()); } - public Pose goal(Pose obj) { int o = __offset(6); return o != 0 ? obj.__assign(o + bb_pos, bb) : null; } - - public static void startMoveArmCartesianServo(FlatBufferBuilder builder) { builder.startObject(2); } - public static void addParent(FlatBufferBuilder builder, int parentOffset) { builder.addOffset(0, parentOffset, 0); } - public static void addGoal(FlatBufferBuilder builder, int goalOffset) { builder.addStruct(1, goalOffset, 0); } - public static int endMoveArmCartesianServo(FlatBufferBuilder builder) { - int o = builder.endObject(); - return o; - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/MoveArmCartesianServo.py b/include/grl/flatbuffer/grl/flatbuffer/MoveArmCartesianServo.py deleted file mode 100644 index 1f8b622..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/MoveArmCartesianServo.py +++ /dev/null @@ -1,42 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class MoveArmCartesianServo(object): - __slots__ = ['_tab'] - - @classmethod - def GetRootAsMoveArmCartesianServo(cls, buf, offset): - n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) - x = MoveArmCartesianServo() - x.Init(buf, n + offset) - return x - - # MoveArmCartesianServo - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - - # MoveArmCartesianServo - def Parent(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - return self._tab.String(o + self._tab.Pos) - return bytes() - - # MoveArmCartesianServo - def Goal(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) - if o != 0: - x = o + self._tab.Pos - from .Pose import Pose - obj = Pose() - obj.Init(self._tab.Bytes, x) - return obj - return None - -def MoveArmCartesianServoStart(builder): builder.StartObject(2) -def MoveArmCartesianServoAddParent(builder, parent): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(parent), 0) -def MoveArmCartesianServoAddGoal(builder, goal): builder.PrependStructSlot(1, flatbuffers.number_types.UOffsetTFlags.py_type(goal), 0) -def MoveArmCartesianServoEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/MoveArmJointServo.java b/include/grl/flatbuffer/grl/flatbuffer/MoveArmJointServo.java deleted file mode 100644 index 92f91a2..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/MoveArmJointServo.java +++ /dev/null @@ -1,34 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class MoveArmJointServo extends Table { - public static MoveArmJointServo getRootAsMoveArmJointServo(ByteBuffer _bb) { return getRootAsMoveArmJointServo(_bb, new MoveArmJointServo()); } - public static MoveArmJointServo getRootAsMoveArmJointServo(ByteBuffer _bb, MoveArmJointServo obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public MoveArmJointServo __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - public JointState goal() { return goal(new JointState()); } - public JointState goal(JointState obj) { int o = __offset(4); return o != 0 ? obj.__assign(__indirect(o + bb_pos), bb) : null; } - - public static int createMoveArmJointServo(FlatBufferBuilder builder, - int goalOffset) { - builder.startObject(1); - MoveArmJointServo.addGoal(builder, goalOffset); - return MoveArmJointServo.endMoveArmJointServo(builder); - } - - public static void startMoveArmJointServo(FlatBufferBuilder builder) { builder.startObject(1); } - public static void addGoal(FlatBufferBuilder builder, int goalOffset) { builder.addOffset(0, goalOffset, 0); } - public static int endMoveArmJointServo(FlatBufferBuilder builder) { - int o = builder.endObject(); - return o; - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/MoveArmJointServo.py b/include/grl/flatbuffer/grl/flatbuffer/MoveArmJointServo.py deleted file mode 100644 index fb233fc..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/MoveArmJointServo.py +++ /dev/null @@ -1,34 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class MoveArmJointServo(object): - __slots__ = ['_tab'] - - @classmethod - def GetRootAsMoveArmJointServo(cls, buf, offset): - n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) - x = MoveArmJointServo() - x.Init(buf, n + offset) - return x - - # MoveArmJointServo - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - - # MoveArmJointServo - def Goal(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - x = self._tab.Indirect(o + self._tab.Pos) - from .JointState import JointState - obj = JointState() - obj.Init(self._tab.Bytes, x) - return obj - return None - -def MoveArmJointServoStart(builder): builder.StartObject(1) -def MoveArmJointServoAddGoal(builder, goal): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(goal), 0) -def MoveArmJointServoEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/MoveArmTrajectory.java b/include/grl/flatbuffer/grl/flatbuffer/MoveArmTrajectory.java deleted file mode 100644 index 5e4709c..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/MoveArmTrajectory.java +++ /dev/null @@ -1,37 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class MoveArmTrajectory extends Table { - public static MoveArmTrajectory getRootAsMoveArmTrajectory(ByteBuffer _bb) { return getRootAsMoveArmTrajectory(_bb, new MoveArmTrajectory()); } - public static MoveArmTrajectory getRootAsMoveArmTrajectory(ByteBuffer _bb, MoveArmTrajectory obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public MoveArmTrajectory __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - public JointState traj(int j) { return traj(new JointState(), j); } - public JointState traj(JointState obj, int j) { int o = __offset(4); return o != 0 ? obj.__assign(__indirect(__vector(o) + j * 4), bb) : null; } - public int trajLength() { int o = __offset(4); return o != 0 ? __vector_len(o) : 0; } - - public static int createMoveArmTrajectory(FlatBufferBuilder builder, - int trajOffset) { - builder.startObject(1); - MoveArmTrajectory.addTraj(builder, trajOffset); - return MoveArmTrajectory.endMoveArmTrajectory(builder); - } - - public static void startMoveArmTrajectory(FlatBufferBuilder builder) { builder.startObject(1); } - public static void addTraj(FlatBufferBuilder builder, int trajOffset) { builder.addOffset(0, trajOffset, 0); } - public static int createTrajVector(FlatBufferBuilder builder, int[] data) { builder.startVector(4, data.length, 4); for (int i = data.length - 1; i >= 0; i--) builder.addOffset(data[i]); return builder.endVector(); } - public static void startTrajVector(FlatBufferBuilder builder, int numElems) { builder.startVector(4, numElems, 4); } - public static int endMoveArmTrajectory(FlatBufferBuilder builder) { - int o = builder.endObject(); - return o; - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/MoveArmTrajectory.py b/include/grl/flatbuffer/grl/flatbuffer/MoveArmTrajectory.py deleted file mode 100644 index 1464f45..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/MoveArmTrajectory.py +++ /dev/null @@ -1,44 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class MoveArmTrajectory(object): - __slots__ = ['_tab'] - - @classmethod - def GetRootAsMoveArmTrajectory(cls, buf, offset): - n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) - x = MoveArmTrajectory() - x.Init(buf, n + offset) - return x - - # MoveArmTrajectory - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - - # MoveArmTrajectory - def Traj(self, j): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - x = self._tab.Vector(o) - x += flatbuffers.number_types.UOffsetTFlags.py_type(j) * 4 - x = self._tab.Indirect(x) - from .JointState import JointState - obj = JointState() - obj.Init(self._tab.Bytes, x) - return obj - return None - - # MoveArmTrajectory - def TrajLength(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - return self._tab.VectorLen(o) - return 0 - -def MoveArmTrajectoryStart(builder): builder.StartObject(1) -def MoveArmTrajectoryAddTraj(builder, traj): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(traj), 0) -def MoveArmTrajectoryStartTrajVector(builder, numElems): return builder.StartVector(4, numElems, 4) -def MoveArmTrajectoryEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/PauseArm.java b/include/grl/flatbuffer/grl/flatbuffer/PauseArm.java deleted file mode 100644 index ccec622..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/PauseArm.java +++ /dev/null @@ -1,24 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class PauseArm extends Table { - public static PauseArm getRootAsPauseArm(ByteBuffer _bb) { return getRootAsPauseArm(_bb, new PauseArm()); } - public static PauseArm getRootAsPauseArm(ByteBuffer _bb, PauseArm obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public PauseArm __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - - public static void startPauseArm(FlatBufferBuilder builder) { builder.startObject(0); } - public static int endPauseArm(FlatBufferBuilder builder) { - int o = builder.endObject(); - return o; - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/PauseArm.py b/include/grl/flatbuffer/grl/flatbuffer/PauseArm.py deleted file mode 100644 index a3415d1..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/PauseArm.py +++ /dev/null @@ -1,22 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class PauseArm(object): - __slots__ = ['_tab'] - - @classmethod - def GetRootAsPauseArm(cls, buf, offset): - n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) - x = PauseArm() - x.Init(buf, n + offset) - return x - - # PauseArm - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - -def PauseArmStart(builder): builder.StartObject(0) -def PauseArmEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/Pose.java b/include/grl/flatbuffer/grl/flatbuffer/Pose.java deleted file mode 100644 index 90f2736..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/Pose.java +++ /dev/null @@ -1,34 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class Pose extends Struct { - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public Pose __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - public Vector3d position() { return position(new Vector3d()); } - public Vector3d position(Vector3d obj) { return obj.__assign(bb_pos + 0, bb); } - public Quaternion orientation() { return orientation(new Quaternion()); } - public Quaternion orientation(Quaternion obj) { return obj.__assign(bb_pos + 24, bb); } - - public static int createPose(FlatBufferBuilder builder, double position_x, double position_y, double position_z, double orientation_x, double orientation_y, double orientation_z, double orientation_w) { - builder.prep(8, 56); - builder.prep(8, 32); - builder.putDouble(orientation_w); - builder.putDouble(orientation_z); - builder.putDouble(orientation_y); - builder.putDouble(orientation_x); - builder.prep(8, 24); - builder.putDouble(position_z); - builder.putDouble(position_y); - builder.putDouble(position_x); - return builder.offset(); - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/Pose.py b/include/grl/flatbuffer/grl/flatbuffer/Pose.py deleted file mode 100644 index 10e7e23..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/Pose.py +++ /dev/null @@ -1,36 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class Pose(object): - __slots__ = ['_tab'] - - # Pose - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - - # Pose - def Position(self, obj): - obj.Init(self._tab.Bytes, self._tab.Pos + 0) - return obj - - # Pose - def Orientation(self, obj): - obj.Init(self._tab.Bytes, self._tab.Pos + 24) - return obj - - -def CreatePose(builder, position_x, position_y, position_z, orientation_x, orientation_y, orientation_z, orientation_w): - builder.Prep(8, 56) - builder.Prep(8, 32) - builder.PrependFloat64(orientation_w) - builder.PrependFloat64(orientation_z) - builder.PrependFloat64(orientation_y) - builder.PrependFloat64(orientation_x) - builder.Prep(8, 24) - builder.PrependFloat64(position_z) - builder.PrependFloat64(position_y) - builder.PrependFloat64(position_x) - return builder.Offset() diff --git a/include/grl/flatbuffer/grl/flatbuffer/ProcessData.java b/include/grl/flatbuffer/grl/flatbuffer/ProcessData.java deleted file mode 100644 index ae0bb67..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/ProcessData.java +++ /dev/null @@ -1,97 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -/** - * "ProcessData" is a field that appears - * on your physical kuka tablet. - * This message allows you to update these - * fields on the tablet yourself. - */ -public final class ProcessData extends Table { - public static ProcessData getRootAsProcessData(ByteBuffer _bb) { return getRootAsProcessData(_bb, new ProcessData()); } - public static ProcessData getRootAsProcessData(ByteBuffer _bb, ProcessData obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public ProcessData __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - public String dataType() { int o = __offset(4); return o != 0 ? __string(o + bb_pos) : null; } - public ByteBuffer dataTypeAsByteBuffer() { return __vector_as_bytebuffer(4, 1); } - public ByteBuffer dataTypeInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 4, 1); } - public String defaultValue() { int o = __offset(6); return o != 0 ? __string(o + bb_pos) : null; } - public ByteBuffer defaultValueAsByteBuffer() { return __vector_as_bytebuffer(6, 1); } - public ByteBuffer defaultValueInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 6, 1); } - public String displayName() { int o = __offset(8); return o != 0 ? __string(o + bb_pos) : null; } - public ByteBuffer displayNameAsByteBuffer() { return __vector_as_bytebuffer(8, 1); } - public ByteBuffer displayNameInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 8, 1); } - public String id() { int o = __offset(10); return o != 0 ? __string(o + bb_pos) : null; } - public ByteBuffer idAsByteBuffer() { return __vector_as_bytebuffer(10, 1); } - public ByteBuffer idInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 10, 1); } - public String min() { int o = __offset(12); return o != 0 ? __string(o + bb_pos) : null; } - public ByteBuffer minAsByteBuffer() { return __vector_as_bytebuffer(12, 1); } - public ByteBuffer minInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 12, 1); } - public String max() { int o = __offset(14); return o != 0 ? __string(o + bb_pos) : null; } - public ByteBuffer maxAsByteBuffer() { return __vector_as_bytebuffer(14, 1); } - public ByteBuffer maxInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 14, 1); } - public String unit() { int o = __offset(16); return o != 0 ? __string(o + bb_pos) : null; } - public ByteBuffer unitAsByteBuffer() { return __vector_as_bytebuffer(16, 1); } - public ByteBuffer unitInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 16, 1); } - public String value() { int o = __offset(18); return o != 0 ? __string(o + bb_pos) : null; } - public ByteBuffer valueAsByteBuffer() { return __vector_as_bytebuffer(18, 1); } - public ByteBuffer valueInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 18, 1); } - /** - * should the data be removed completely? - */ - public boolean shouldRemove() { int o = __offset(20); return o != 0 ? 0!=bb.get(o + bb_pos) : false; } - /** - * should the data be updated to these values? - */ - public boolean shouldUpdate() { int o = __offset(22); return o != 0 ? 0!=bb.get(o + bb_pos) : false; } - - public static int createProcessData(FlatBufferBuilder builder, - int dataTypeOffset, - int defaultValueOffset, - int displayNameOffset, - int idOffset, - int minOffset, - int maxOffset, - int unitOffset, - int valueOffset, - boolean shouldRemove, - boolean shouldUpdate) { - builder.startObject(10); - ProcessData.addValue(builder, valueOffset); - ProcessData.addUnit(builder, unitOffset); - ProcessData.addMax(builder, maxOffset); - ProcessData.addMin(builder, minOffset); - ProcessData.addId(builder, idOffset); - ProcessData.addDisplayName(builder, displayNameOffset); - ProcessData.addDefaultValue(builder, defaultValueOffset); - ProcessData.addDataType(builder, dataTypeOffset); - ProcessData.addShouldUpdate(builder, shouldUpdate); - ProcessData.addShouldRemove(builder, shouldRemove); - return ProcessData.endProcessData(builder); - } - - public static void startProcessData(FlatBufferBuilder builder) { builder.startObject(10); } - public static void addDataType(FlatBufferBuilder builder, int dataTypeOffset) { builder.addOffset(0, dataTypeOffset, 0); } - public static void addDefaultValue(FlatBufferBuilder builder, int defaultValueOffset) { builder.addOffset(1, defaultValueOffset, 0); } - public static void addDisplayName(FlatBufferBuilder builder, int displayNameOffset) { builder.addOffset(2, displayNameOffset, 0); } - public static void addId(FlatBufferBuilder builder, int idOffset) { builder.addOffset(3, idOffset, 0); } - public static void addMin(FlatBufferBuilder builder, int minOffset) { builder.addOffset(4, minOffset, 0); } - public static void addMax(FlatBufferBuilder builder, int maxOffset) { builder.addOffset(5, maxOffset, 0); } - public static void addUnit(FlatBufferBuilder builder, int unitOffset) { builder.addOffset(6, unitOffset, 0); } - public static void addValue(FlatBufferBuilder builder, int valueOffset) { builder.addOffset(7, valueOffset, 0); } - public static void addShouldRemove(FlatBufferBuilder builder, boolean shouldRemove) { builder.addBoolean(8, shouldRemove, false); } - public static void addShouldUpdate(FlatBufferBuilder builder, boolean shouldUpdate) { builder.addBoolean(9, shouldUpdate, false); } - public static int endProcessData(FlatBufferBuilder builder) { - int o = builder.endObject(); - return o; - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/ProcessData.py b/include/grl/flatbuffer/grl/flatbuffer/ProcessData.py deleted file mode 100644 index 3101a94..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/ProcessData.py +++ /dev/null @@ -1,108 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -# /// "ProcessData" is a field that appears -# /// on your physical kuka tablet. -# /// This message allows you to update these -# /// fields on the tablet yourself. -class ProcessData(object): - __slots__ = ['_tab'] - - @classmethod - def GetRootAsProcessData(cls, buf, offset): - n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) - x = ProcessData() - x.Init(buf, n + offset) - return x - - # ProcessData - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - - # ProcessData - def DataType(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - return self._tab.String(o + self._tab.Pos) - return bytes() - - # ProcessData - def DefaultValue(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) - if o != 0: - return self._tab.String(o + self._tab.Pos) - return bytes() - - # ProcessData - def DisplayName(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) - if o != 0: - return self._tab.String(o + self._tab.Pos) - return bytes() - - # ProcessData - def Id(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) - if o != 0: - return self._tab.String(o + self._tab.Pos) - return bytes() - - # ProcessData - def Min(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(12)) - if o != 0: - return self._tab.String(o + self._tab.Pos) - return bytes() - - # ProcessData - def Max(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(14)) - if o != 0: - return self._tab.String(o + self._tab.Pos) - return bytes() - - # ProcessData - def Unit(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(16)) - if o != 0: - return self._tab.String(o + self._tab.Pos) - return bytes() - - # ProcessData - def Value(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(18)) - if o != 0: - return self._tab.String(o + self._tab.Pos) - return bytes() - -# /// should the data be removed completely? - # ProcessData - def ShouldRemove(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(20)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.BoolFlags, o + self._tab.Pos) - return 0 - -# /// should the data be updated to these values? - # ProcessData - def ShouldUpdate(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(22)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.BoolFlags, o + self._tab.Pos) - return 0 - -def ProcessDataStart(builder): builder.StartObject(10) -def ProcessDataAddDataType(builder, dataType): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(dataType), 0) -def ProcessDataAddDefaultValue(builder, defaultValue): builder.PrependUOffsetTRelativeSlot(1, flatbuffers.number_types.UOffsetTFlags.py_type(defaultValue), 0) -def ProcessDataAddDisplayName(builder, displayName): builder.PrependUOffsetTRelativeSlot(2, flatbuffers.number_types.UOffsetTFlags.py_type(displayName), 0) -def ProcessDataAddId(builder, id): builder.PrependUOffsetTRelativeSlot(3, flatbuffers.number_types.UOffsetTFlags.py_type(id), 0) -def ProcessDataAddMin(builder, min): builder.PrependUOffsetTRelativeSlot(4, flatbuffers.number_types.UOffsetTFlags.py_type(min), 0) -def ProcessDataAddMax(builder, max): builder.PrependUOffsetTRelativeSlot(5, flatbuffers.number_types.UOffsetTFlags.py_type(max), 0) -def ProcessDataAddUnit(builder, unit): builder.PrependUOffsetTRelativeSlot(6, flatbuffers.number_types.UOffsetTFlags.py_type(unit), 0) -def ProcessDataAddValue(builder, value): builder.PrependUOffsetTRelativeSlot(7, flatbuffers.number_types.UOffsetTFlags.py_type(value), 0) -def ProcessDataAddShouldRemove(builder, shouldRemove): builder.PrependBoolSlot(8, shouldRemove, 0) -def ProcessDataAddShouldUpdate(builder, shouldUpdate): builder.PrependBoolSlot(9, shouldUpdate, 0) -def ProcessDataEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/Quaternion.java b/include/grl/flatbuffer/grl/flatbuffer/Quaternion.java deleted file mode 100644 index d6b4be0..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/Quaternion.java +++ /dev/null @@ -1,29 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class Quaternion extends Struct { - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public Quaternion __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - public double x() { return bb.getDouble(bb_pos + 0); } - public double y() { return bb.getDouble(bb_pos + 8); } - public double z() { return bb.getDouble(bb_pos + 16); } - public double w() { return bb.getDouble(bb_pos + 24); } - - public static int createQuaternion(FlatBufferBuilder builder, double x, double y, double z, double w) { - builder.prep(8, 32); - builder.putDouble(w); - builder.putDouble(z); - builder.putDouble(y); - builder.putDouble(x); - return builder.offset(); - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/Quaternion.py b/include/grl/flatbuffer/grl/flatbuffer/Quaternion.py deleted file mode 100644 index 54f6b1f..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/Quaternion.py +++ /dev/null @@ -1,29 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class Quaternion(object): - __slots__ = ['_tab'] - - # Quaternion - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - - # Quaternion - def X(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(0)) - # Quaternion - def Y(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(8)) - # Quaternion - def Z(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(16)) - # Quaternion - def W(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(24)) - -def CreateQuaternion(builder, x, y, z, w): - builder.Prep(8, 32) - builder.PrependFloat64(w) - builder.PrependFloat64(z) - builder.PrependFloat64(y) - builder.PrependFloat64(x) - return builder.Offset() diff --git a/include/grl/flatbuffer/grl/flatbuffer/ShutdownArm.java b/include/grl/flatbuffer/grl/flatbuffer/ShutdownArm.java deleted file mode 100644 index 675fecd..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/ShutdownArm.java +++ /dev/null @@ -1,24 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class ShutdownArm extends Table { - public static ShutdownArm getRootAsShutdownArm(ByteBuffer _bb) { return getRootAsShutdownArm(_bb, new ShutdownArm()); } - public static ShutdownArm getRootAsShutdownArm(ByteBuffer _bb, ShutdownArm obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public ShutdownArm __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - - public static void startShutdownArm(FlatBufferBuilder builder) { builder.startObject(0); } - public static int endShutdownArm(FlatBufferBuilder builder) { - int o = builder.endObject(); - return o; - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/ShutdownArm.py b/include/grl/flatbuffer/grl/flatbuffer/ShutdownArm.py deleted file mode 100644 index c96dbd1..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/ShutdownArm.py +++ /dev/null @@ -1,22 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class ShutdownArm(object): - __slots__ = ['_tab'] - - @classmethod - def GetRootAsShutdownArm(cls, buf, offset): - n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) - x = ShutdownArm() - x.Init(buf, n + offset) - return x - - # ShutdownArm - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - -def ShutdownArmStart(builder): builder.StartObject(0) -def ShutdownArmEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/SmartServo.java b/include/grl/flatbuffer/grl/flatbuffer/SmartServo.java deleted file mode 100644 index e0204c6..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/SmartServo.java +++ /dev/null @@ -1,61 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class SmartServo extends Table { - public static SmartServo getRootAsSmartServo(ByteBuffer _bb) { return getRootAsSmartServo(_bb, new SmartServo()); } - public static SmartServo getRootAsSmartServo(ByteBuffer _bb, SmartServo obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public SmartServo __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - /** - * normalized joint accelerations from 0 to 1 relative to system capabilities - */ - public double jointAccelerationRel(int j) { int o = __offset(4); return o != 0 ? bb.getDouble(__vector(o) + j * 8) : 0; } - public int jointAccelerationRelLength() { int o = __offset(4); return o != 0 ? __vector_len(o) : 0; } - public ByteBuffer jointAccelerationRelAsByteBuffer() { return __vector_as_bytebuffer(4, 8); } - public ByteBuffer jointAccelerationRelInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 4, 8); } - /** - * normalized joint velocity from 0 to 1 relative to system capabilities - */ - public double jointVelocityRel(int j) { int o = __offset(6); return o != 0 ? bb.getDouble(__vector(o) + j * 8) : 0; } - public int jointVelocityRelLength() { int o = __offset(6); return o != 0 ? __vector_len(o) : 0; } - public ByteBuffer jointVelocityRelAsByteBuffer() { return __vector_as_bytebuffer(6, 8); } - public ByteBuffer jointVelocityRelInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 6, 8); } - public boolean updateMinimumTrajectoryExecutionTime() { int o = __offset(8); return o != 0 ? 0!=bb.get(o + bb_pos) : false; } - public double minimumTrajectoryExecutionTime() { int o = __offset(10); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } - - public static int createSmartServo(FlatBufferBuilder builder, - int jointAccelerationRelOffset, - int jointVelocityRelOffset, - boolean updateMinimumTrajectoryExecutionTime, - double minimumTrajectoryExecutionTime) { - builder.startObject(4); - SmartServo.addMinimumTrajectoryExecutionTime(builder, minimumTrajectoryExecutionTime); - SmartServo.addJointVelocityRel(builder, jointVelocityRelOffset); - SmartServo.addJointAccelerationRel(builder, jointAccelerationRelOffset); - SmartServo.addUpdateMinimumTrajectoryExecutionTime(builder, updateMinimumTrajectoryExecutionTime); - return SmartServo.endSmartServo(builder); - } - - public static void startSmartServo(FlatBufferBuilder builder) { builder.startObject(4); } - public static void addJointAccelerationRel(FlatBufferBuilder builder, int jointAccelerationRelOffset) { builder.addOffset(0, jointAccelerationRelOffset, 0); } - public static int createJointAccelerationRelVector(FlatBufferBuilder builder, double[] data) { builder.startVector(8, data.length, 8); for (int i = data.length - 1; i >= 0; i--) builder.addDouble(data[i]); return builder.endVector(); } - public static void startJointAccelerationRelVector(FlatBufferBuilder builder, int numElems) { builder.startVector(8, numElems, 8); } - public static void addJointVelocityRel(FlatBufferBuilder builder, int jointVelocityRelOffset) { builder.addOffset(1, jointVelocityRelOffset, 0); } - public static int createJointVelocityRelVector(FlatBufferBuilder builder, double[] data) { builder.startVector(8, data.length, 8); for (int i = data.length - 1; i >= 0; i--) builder.addDouble(data[i]); return builder.endVector(); } - public static void startJointVelocityRelVector(FlatBufferBuilder builder, int numElems) { builder.startVector(8, numElems, 8); } - public static void addUpdateMinimumTrajectoryExecutionTime(FlatBufferBuilder builder, boolean updateMinimumTrajectoryExecutionTime) { builder.addBoolean(2, updateMinimumTrajectoryExecutionTime, false); } - public static void addMinimumTrajectoryExecutionTime(FlatBufferBuilder builder, double minimumTrajectoryExecutionTime) { builder.addDouble(3, minimumTrajectoryExecutionTime, 0.0); } - public static int endSmartServo(FlatBufferBuilder builder) { - int o = builder.endObject(); - return o; - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/SmartServo.py b/include/grl/flatbuffer/grl/flatbuffer/SmartServo.py deleted file mode 100644 index 6c832d2..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/SmartServo.py +++ /dev/null @@ -1,88 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class SmartServo(object): - __slots__ = ['_tab'] - - @classmethod - def GetRootAsSmartServo(cls, buf, offset): - n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) - x = SmartServo() - x.Init(buf, n + offset) - return x - - # SmartServo - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - -# /// normalized joint accelerations from 0 to 1 relative to system capabilities - # SmartServo - def JointAccelerationRel(self, j): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - a = self._tab.Vector(o) - return self._tab.Get(flatbuffers.number_types.Float64Flags, a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 8)) - return 0 - - # SmartServo - def JointAccelerationRelAsNumpy(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - return self._tab.GetVectorAsNumpy(flatbuffers.number_types.Float64Flags, o) - return 0 - - # SmartServo - def JointAccelerationRelLength(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - return self._tab.VectorLen(o) - return 0 - -# /// normalized joint velocity from 0 to 1 relative to system capabilities - # SmartServo - def JointVelocityRel(self, j): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) - if o != 0: - a = self._tab.Vector(o) - return self._tab.Get(flatbuffers.number_types.Float64Flags, a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 8)) - return 0 - - # SmartServo - def JointVelocityRelAsNumpy(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) - if o != 0: - return self._tab.GetVectorAsNumpy(flatbuffers.number_types.Float64Flags, o) - return 0 - - # SmartServo - def JointVelocityRelLength(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) - if o != 0: - return self._tab.VectorLen(o) - return 0 - - # SmartServo - def UpdateMinimumTrajectoryExecutionTime(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.BoolFlags, o + self._tab.Pos) - return 0 - - # SmartServo - def MinimumTrajectoryExecutionTime(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) - return 0.0 - -def SmartServoStart(builder): builder.StartObject(4) -def SmartServoAddJointAccelerationRel(builder, jointAccelerationRel): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(jointAccelerationRel), 0) -def SmartServoStartJointAccelerationRelVector(builder, numElems): return builder.StartVector(8, numElems, 8) -def SmartServoAddJointVelocityRel(builder, jointVelocityRel): builder.PrependUOffsetTRelativeSlot(1, flatbuffers.number_types.UOffsetTFlags.py_type(jointVelocityRel), 0) -def SmartServoStartJointVelocityRelVector(builder, numElems): return builder.StartVector(8, numElems, 8) -def SmartServoAddUpdateMinimumTrajectoryExecutionTime(builder, updateMinimumTrajectoryExecutionTime): builder.PrependBoolSlot(2, updateMinimumTrajectoryExecutionTime, 0) -def SmartServoAddMinimumTrajectoryExecutionTime(builder, minimumTrajectoryExecutionTime): builder.PrependFloat64Slot(3, minimumTrajectoryExecutionTime, 0.0) -def SmartServoEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/StartArm.java b/include/grl/flatbuffer/grl/flatbuffer/StartArm.java deleted file mode 100644 index 2a0b60b..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/StartArm.java +++ /dev/null @@ -1,24 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class StartArm extends Table { - public static StartArm getRootAsStartArm(ByteBuffer _bb) { return getRootAsStartArm(_bb, new StartArm()); } - public static StartArm getRootAsStartArm(ByteBuffer _bb, StartArm obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public StartArm __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - - public static void startStartArm(FlatBufferBuilder builder) { builder.startObject(0); } - public static int endStartArm(FlatBufferBuilder builder) { - int o = builder.endObject(); - return o; - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/StartArm.py b/include/grl/flatbuffer/grl/flatbuffer/StartArm.py deleted file mode 100644 index 23446e8..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/StartArm.py +++ /dev/null @@ -1,22 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class StartArm(object): - __slots__ = ['_tab'] - - @classmethod - def GetRootAsStartArm(cls, buf, offset): - n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) - x = StartArm() - x.Init(buf, n + offset) - return x - - # StartArm - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - -def StartArmStart(builder): builder.StartObject(0) -def StartArmEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/StopArm.java b/include/grl/flatbuffer/grl/flatbuffer/StopArm.java deleted file mode 100644 index ad15226..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/StopArm.java +++ /dev/null @@ -1,24 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class StopArm extends Table { - public static StopArm getRootAsStopArm(ByteBuffer _bb) { return getRootAsStopArm(_bb, new StopArm()); } - public static StopArm getRootAsStopArm(ByteBuffer _bb, StopArm obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public StopArm __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - - public static void startStopArm(FlatBufferBuilder builder) { builder.startObject(0); } - public static int endStopArm(FlatBufferBuilder builder) { - int o = builder.endObject(); - return o; - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/StopArm.py b/include/grl/flatbuffer/grl/flatbuffer/StopArm.py deleted file mode 100644 index 8f7d1e4..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/StopArm.py +++ /dev/null @@ -1,22 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class StopArm(object): - __slots__ = ['_tab'] - - @classmethod - def GetRootAsStopArm(cls, buf, offset): - n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) - x = StopArm() - x.Init(buf, n + offset) - return x - - # StopArm - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - -def StopArmStart(builder): builder.StartObject(0) -def StopArmEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/TeachArm.java b/include/grl/flatbuffer/grl/flatbuffer/TeachArm.java deleted file mode 100644 index 0c75807..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/TeachArm.java +++ /dev/null @@ -1,24 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class TeachArm extends Table { - public static TeachArm getRootAsTeachArm(ByteBuffer _bb) { return getRootAsTeachArm(_bb, new TeachArm()); } - public static TeachArm getRootAsTeachArm(ByteBuffer _bb, TeachArm obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public TeachArm __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - - public static void startTeachArm(FlatBufferBuilder builder) { builder.startObject(0); } - public static int endTeachArm(FlatBufferBuilder builder) { - int o = builder.endObject(); - return o; - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/TeachArm.py b/include/grl/flatbuffer/grl/flatbuffer/TeachArm.py deleted file mode 100644 index a8818fd..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/TeachArm.py +++ /dev/null @@ -1,22 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class TeachArm(object): - __slots__ = ['_tab'] - - @classmethod - def GetRootAsTeachArm(cls, buf, offset): - n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) - x = TeachArm() - x.Init(buf, n + offset) - return x - - # TeachArm - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - -def TeachArmStart(builder): builder.StartObject(0) -def TeachArmEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/Time.java b/include/grl/flatbuffer/grl/flatbuffer/Time.java deleted file mode 100644 index d92b522..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/Time.java +++ /dev/null @@ -1,23 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class Time extends Struct { - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public Time __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - public double sec() { return bb.getDouble(bb_pos + 0); } - - public static int createTime(FlatBufferBuilder builder, double sec) { - builder.prep(8, 8); - builder.putDouble(sec); - return builder.offset(); - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/Time.py b/include/grl/flatbuffer/grl/flatbuffer/Time.py deleted file mode 100644 index 46b015c..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/Time.py +++ /dev/null @@ -1,20 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class Time(object): - __slots__ = ['_tab'] - - # Time - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - - # Time - def Sec(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(0)) - -def CreateTime(builder, sec): - builder.Prep(8, 8) - builder.PrependFloat64(sec) - return builder.Offset() diff --git a/include/grl/flatbuffer/grl/flatbuffer/TimeEvent.java b/include/grl/flatbuffer/grl/flatbuffer/TimeEvent.java deleted file mode 100644 index b76eedd..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/TimeEvent.java +++ /dev/null @@ -1,112 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -/** - * Note that all of these time entries are - * longs with a minimum step of 100ns, - * see google cartographer's cartographer::common::time - */ -public final class TimeEvent extends Table { - public static TimeEvent getRootAsTimeEvent(ByteBuffer _bb) { return getRootAsTimeEvent(_bb, new TimeEvent()); } - public static TimeEvent getRootAsTimeEvent(ByteBuffer _bb, TimeEvent obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public TimeEvent __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - /** - * Identifying string for this time stamped data topic - * something like "/opticaltracker/00000000/frame" where - * 00000000 is the serial number of the optical tracker. - */ - public String eventName() { int o = __offset(4); return o != 0 ? __string(o + bb_pos) : null; } - public ByteBuffer eventNameAsByteBuffer() { return __vector_as_bytebuffer(4, 1); } - public ByteBuffer eventNameInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 4, 1); } - /** - * The time just before a data update request is made - */ - public long localRequestTime() { int o = __offset(6); return o != 0 ? bb.getLong(o + bb_pos) : 0L; } - /** - * Identifying string for the clock used to drive the device - * something like "/opticaltracker/00000000/clock" - * if it is the clock internal to a sensor like an optical tracker - */ - public String deviceClockId() { int o = __offset(8); return o != 0 ? __string(o + bb_pos) : null; } - public ByteBuffer deviceClockIdAsByteBuffer() { return __vector_as_bytebuffer(8, 1); } - public ByteBuffer deviceClockIdInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 8, 1); } - /** - * The time provided by the device specified by device_clock_id - */ - public long deviceTime() { int o = __offset(10); return o != 0 ? bb.getLong(o + bb_pos) : 0L; } - /** - * Identifying string for the clock used to drive the device - * or "/control_computer/clock/steady" if the device has no clock - * and the time is the desktop computer - * running the steady clock (vs clocks which might change time) - */ - public String localClockId() { int o = __offset(12); return o != 0 ? __string(o + bb_pos) : null; } - public ByteBuffer localClockIdAsByteBuffer() { return __vector_as_bytebuffer(12, 1); } - public ByteBuffer localClockIdInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 12, 1); } - /** - * The time at which the data was received - */ - public long localReceiveTime() { int o = __offset(14); return o != 0 ? bb.getLong(o + bb_pos) : 0L; } - /** - * The corrected local time which represents when the sensor - * data was actually captured. - */ - public long correctedLocalTime() { int o = __offset(16); return o != 0 ? bb.getLong(o + bb_pos) : 0L; } - /** - * Estimated duration of the skew between the device clock - * and the local time clock - */ - public long clockSkew() { int o = __offset(18); return o != 0 ? bb.getLong(o + bb_pos) : 0L; } - /** - * The minimum expected delay in transporting the data request - */ - public long minTransportDelay() { int o = __offset(20); return o != 0 ? bb.getLong(o + bb_pos) : 0L; } - - public static int createTimeEvent(FlatBufferBuilder builder, - int event_nameOffset, - long local_request_time, - int device_clock_idOffset, - long device_time, - int local_clock_idOffset, - long local_receive_time, - long corrected_local_time, - long clock_skew, - long min_transport_delay) { - builder.startObject(9); - TimeEvent.addMinTransportDelay(builder, min_transport_delay); - TimeEvent.addClockSkew(builder, clock_skew); - TimeEvent.addCorrectedLocalTime(builder, corrected_local_time); - TimeEvent.addLocalReceiveTime(builder, local_receive_time); - TimeEvent.addDeviceTime(builder, device_time); - TimeEvent.addLocalRequestTime(builder, local_request_time); - TimeEvent.addLocalClockId(builder, local_clock_idOffset); - TimeEvent.addDeviceClockId(builder, device_clock_idOffset); - TimeEvent.addEventName(builder, event_nameOffset); - return TimeEvent.endTimeEvent(builder); - } - - public static void startTimeEvent(FlatBufferBuilder builder) { builder.startObject(9); } - public static void addEventName(FlatBufferBuilder builder, int eventNameOffset) { builder.addOffset(0, eventNameOffset, 0); } - public static void addLocalRequestTime(FlatBufferBuilder builder, long localRequestTime) { builder.addLong(1, localRequestTime, 0L); } - public static void addDeviceClockId(FlatBufferBuilder builder, int deviceClockIdOffset) { builder.addOffset(2, deviceClockIdOffset, 0); } - public static void addDeviceTime(FlatBufferBuilder builder, long deviceTime) { builder.addLong(3, deviceTime, 0L); } - public static void addLocalClockId(FlatBufferBuilder builder, int localClockIdOffset) { builder.addOffset(4, localClockIdOffset, 0); } - public static void addLocalReceiveTime(FlatBufferBuilder builder, long localReceiveTime) { builder.addLong(5, localReceiveTime, 0L); } - public static void addCorrectedLocalTime(FlatBufferBuilder builder, long correctedLocalTime) { builder.addLong(6, correctedLocalTime, 0L); } - public static void addClockSkew(FlatBufferBuilder builder, long clockSkew) { builder.addLong(7, clockSkew, 0L); } - public static void addMinTransportDelay(FlatBufferBuilder builder, long minTransportDelay) { builder.addLong(8, minTransportDelay, 0L); } - public static int endTimeEvent(FlatBufferBuilder builder) { - int o = builder.endObject(); - return o; - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/TimeEvent.py b/include/grl/flatbuffer/grl/flatbuffer/TimeEvent.py deleted file mode 100644 index 0a764dc..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/TimeEvent.py +++ /dev/null @@ -1,115 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -# /// Note that all of these time entries are -# /// longs with a minimum step of 100ns, -# /// see google cartographer's cartographer::common::time -class TimeEvent(object): - __slots__ = ['_tab'] - - @classmethod - def GetRootAsTimeEvent(cls, buf, offset): - n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) - x = TimeEvent() - x.Init(buf, n + offset) - return x - - # TimeEvent - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - -# /// Identifying string for this time stamped data topic -# /// something like "/opticaltracker/00000000/frame" where -# /// 00000000 is the serial number of the optical tracker. - # TimeEvent - def EventName(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - return self._tab.String(o + self._tab.Pos) - return bytes() - -# /// The time just before a data update request is made - # TimeEvent - def LocalRequestTime(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Int64Flags, o + self._tab.Pos) - return 0 - -# /// Identifying string for the clock used to drive the device -# /// something like "/opticaltracker/00000000/clock" -# /// if it is the clock internal to a sensor like an optical tracker - # TimeEvent - def DeviceClockId(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) - if o != 0: - return self._tab.String(o + self._tab.Pos) - return bytes() - -# /// The time provided by the device specified by device_clock_id - # TimeEvent - def DeviceTime(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Int64Flags, o + self._tab.Pos) - return 0 - -# /// Identifying string for the clock used to drive the device -# /// or "/control_computer/clock/steady" if the device has no clock -# /// and the time is the desktop computer -# /// running the steady clock (vs clocks which might change time) - # TimeEvent - def LocalClockId(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(12)) - if o != 0: - return self._tab.String(o + self._tab.Pos) - return bytes() - -# /// The time at which the data was received - # TimeEvent - def LocalReceiveTime(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(14)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Int64Flags, o + self._tab.Pos) - return 0 - -# /// The corrected local time which represents when the sensor -# /// data was actually captured. - # TimeEvent - def CorrectedLocalTime(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(16)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Int64Flags, o + self._tab.Pos) - return 0 - -# /// Estimated duration of the skew between the device clock -# /// and the local time clock - # TimeEvent - def ClockSkew(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(18)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Int64Flags, o + self._tab.Pos) - return 0 - -# /// The minimum expected delay in transporting the data request - # TimeEvent - def MinTransportDelay(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(20)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Int64Flags, o + self._tab.Pos) - return 0 - -def TimeEventStart(builder): builder.StartObject(9) -def TimeEventAddEventName(builder, eventName): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(eventName), 0) -def TimeEventAddLocalRequestTime(builder, localRequestTime): builder.PrependInt64Slot(1, localRequestTime, 0) -def TimeEventAddDeviceClockId(builder, deviceClockId): builder.PrependUOffsetTRelativeSlot(2, flatbuffers.number_types.UOffsetTFlags.py_type(deviceClockId), 0) -def TimeEventAddDeviceTime(builder, deviceTime): builder.PrependInt64Slot(3, deviceTime, 0) -def TimeEventAddLocalClockId(builder, localClockId): builder.PrependUOffsetTRelativeSlot(4, flatbuffers.number_types.UOffsetTFlags.py_type(localClockId), 0) -def TimeEventAddLocalReceiveTime(builder, localReceiveTime): builder.PrependInt64Slot(5, localReceiveTime, 0) -def TimeEventAddCorrectedLocalTime(builder, correctedLocalTime): builder.PrependInt64Slot(6, correctedLocalTime, 0) -def TimeEventAddClockSkew(builder, clockSkew): builder.PrependInt64Slot(7, clockSkew, 0) -def TimeEventAddMinTransportDelay(builder, minTransportDelay): builder.PrependInt64Slot(8, minTransportDelay, 0) -def TimeEventEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/Vector3d.java b/include/grl/flatbuffer/grl/flatbuffer/Vector3d.java deleted file mode 100644 index 8217e81..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/Vector3d.java +++ /dev/null @@ -1,27 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class Vector3d extends Struct { - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public Vector3d __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - public double x() { return bb.getDouble(bb_pos + 0); } - public double y() { return bb.getDouble(bb_pos + 8); } - public double z() { return bb.getDouble(bb_pos + 16); } - - public static int createVector3d(FlatBufferBuilder builder, double x, double y, double z) { - builder.prep(8, 24); - builder.putDouble(z); - builder.putDouble(y); - builder.putDouble(x); - return builder.offset(); - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/Vector3d.py b/include/grl/flatbuffer/grl/flatbuffer/Vector3d.py deleted file mode 100644 index ffd7bf0..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/Vector3d.py +++ /dev/null @@ -1,26 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class Vector3d(object): - __slots__ = ['_tab'] - - # Vector3d - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - - # Vector3d - def X(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(0)) - # Vector3d - def Y(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(8)) - # Vector3d - def Z(self): return self._tab.Get(flatbuffers.number_types.Float64Flags, self._tab.Pos + flatbuffers.number_types.UOffsetTFlags.py_type(16)) - -def CreateVector3d(builder, x, y, z): - builder.Prep(8, 24) - builder.PrependFloat64(z) - builder.PrependFloat64(y) - builder.PrependFloat64(x) - return builder.Offset() diff --git a/include/grl/flatbuffer/grl/flatbuffer/VrepControlPoint.java b/include/grl/flatbuffer/grl/flatbuffer/VrepControlPoint.java deleted file mode 100644 index 7fbbe7a..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/VrepControlPoint.java +++ /dev/null @@ -1,52 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class VrepControlPoint extends Table { - public static VrepControlPoint getRootAsVrepControlPoint(ByteBuffer _bb) { return getRootAsVrepControlPoint(_bb, new VrepControlPoint()); } - public static VrepControlPoint getRootAsVrepControlPoint(ByteBuffer _bb, VrepControlPoint obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public VrepControlPoint __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - public Vector3d position() { return position(new Vector3d()); } - public Vector3d position(Vector3d obj) { int o = __offset(4); return o != 0 ? obj.__assign(o + bb_pos, bb) : null; } - public EulerXYZd rotation() { return rotation(new EulerXYZd()); } - public EulerXYZd rotation(EulerXYZd obj) { int o = __offset(6); return o != 0 ? obj.__assign(o + bb_pos, bb) : null; } - public double relativeVelocity() { int o = __offset(8); return o != 0 ? bb.getDouble(o + bb_pos) : 1.0; } - public int bezierPointCount() { int o = __offset(10); return o != 0 ? bb.getInt(o + bb_pos) : 1; } - public double interpolationFactor1() { int o = __offset(12); return o != 0 ? bb.getDouble(o + bb_pos) : 0.5; } - public double interpolationFactor2() { int o = __offset(14); return o != 0 ? bb.getDouble(o + bb_pos) : 0.5; } - public double virtualDistance() { int o = __offset(16); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } - public int auxiliaryFlags() { int o = __offset(18); return o != 0 ? bb.getInt(o + bb_pos) : 0; } - public double auxiliaryChannel1() { int o = __offset(20); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } - public double auxiliaryChannel2() { int o = __offset(22); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } - public double auxiliaryChannel3() { int o = __offset(24); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } - public double auxiliaryChannel4() { int o = __offset(26); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } - - public static void startVrepControlPoint(FlatBufferBuilder builder) { builder.startObject(12); } - public static void addPosition(FlatBufferBuilder builder, int positionOffset) { builder.addStruct(0, positionOffset, 0); } - public static void addRotation(FlatBufferBuilder builder, int rotationOffset) { builder.addStruct(1, rotationOffset, 0); } - public static void addRelativeVelocity(FlatBufferBuilder builder, double relativeVelocity) { builder.addDouble(2, relativeVelocity, 1.0); } - public static void addBezierPointCount(FlatBufferBuilder builder, int bezierPointCount) { builder.addInt(3, bezierPointCount, 1); } - public static void addInterpolationFactor1(FlatBufferBuilder builder, double interpolationFactor1) { builder.addDouble(4, interpolationFactor1, 0.5); } - public static void addInterpolationFactor2(FlatBufferBuilder builder, double interpolationFactor2) { builder.addDouble(5, interpolationFactor2, 0.5); } - public static void addVirtualDistance(FlatBufferBuilder builder, double virtualDistance) { builder.addDouble(6, virtualDistance, 0.0); } - public static void addAuxiliaryFlags(FlatBufferBuilder builder, int auxiliaryFlags) { builder.addInt(7, auxiliaryFlags, 0); } - public static void addAuxiliaryChannel1(FlatBufferBuilder builder, double auxiliaryChannel1) { builder.addDouble(8, auxiliaryChannel1, 0.0); } - public static void addAuxiliaryChannel2(FlatBufferBuilder builder, double auxiliaryChannel2) { builder.addDouble(9, auxiliaryChannel2, 0.0); } - public static void addAuxiliaryChannel3(FlatBufferBuilder builder, double auxiliaryChannel3) { builder.addDouble(10, auxiliaryChannel3, 0.0); } - public static void addAuxiliaryChannel4(FlatBufferBuilder builder, double auxiliaryChannel4) { builder.addDouble(11, auxiliaryChannel4, 0.0); } - public static int endVrepControlPoint(FlatBufferBuilder builder) { - int o = builder.endObject(); - return o; - } - public static void finishVrepControlPointBuffer(FlatBufferBuilder builder, int offset) { builder.finish(offset); } - public static void finishSizePrefixedVrepControlPointBuffer(FlatBufferBuilder builder, int offset) { builder.finishSizePrefixed(offset); } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/VrepControlPoint.py b/include/grl/flatbuffer/grl/flatbuffer/VrepControlPoint.py deleted file mode 100644 index 58cfa1b..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/VrepControlPoint.py +++ /dev/null @@ -1,126 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class VrepControlPoint(object): - __slots__ = ['_tab'] - - @classmethod - def GetRootAsVrepControlPoint(cls, buf, offset): - n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) - x = VrepControlPoint() - x.Init(buf, n + offset) - return x - - # VrepControlPoint - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - - # VrepControlPoint - def Position(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - x = o + self._tab.Pos - from .Vector3d import Vector3d - obj = Vector3d() - obj.Init(self._tab.Bytes, x) - return obj - return None - - # VrepControlPoint - def Rotation(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) - if o != 0: - x = o + self._tab.Pos - from .EulerXYZd import EulerXYZd - obj = EulerXYZd() - obj.Init(self._tab.Bytes, x) - return obj - return None - - # VrepControlPoint - def RelativeVelocity(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) - return 1.0 - - # VrepControlPoint - def BezierPointCount(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Int32Flags, o + self._tab.Pos) - return 1 - - # VrepControlPoint - def InterpolationFactor1(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(12)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) - return 0.5 - - # VrepControlPoint - def InterpolationFactor2(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(14)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) - return 0.5 - - # VrepControlPoint - def VirtualDistance(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(16)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) - return 0.0 - - # VrepControlPoint - def AuxiliaryFlags(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(18)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Int32Flags, o + self._tab.Pos) - return 0 - - # VrepControlPoint - def AuxiliaryChannel1(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(20)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) - return 0.0 - - # VrepControlPoint - def AuxiliaryChannel2(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(22)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) - return 0.0 - - # VrepControlPoint - def AuxiliaryChannel3(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(24)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) - return 0.0 - - # VrepControlPoint - def AuxiliaryChannel4(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(26)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) - return 0.0 - -def VrepControlPointStart(builder): builder.StartObject(12) -def VrepControlPointAddPosition(builder, position): builder.PrependStructSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(position), 0) -def VrepControlPointAddRotation(builder, rotation): builder.PrependStructSlot(1, flatbuffers.number_types.UOffsetTFlags.py_type(rotation), 0) -def VrepControlPointAddRelativeVelocity(builder, relativeVelocity): builder.PrependFloat64Slot(2, relativeVelocity, 1.0) -def VrepControlPointAddBezierPointCount(builder, bezierPointCount): builder.PrependInt32Slot(3, bezierPointCount, 1) -def VrepControlPointAddInterpolationFactor1(builder, interpolationFactor1): builder.PrependFloat64Slot(4, interpolationFactor1, 0.5) -def VrepControlPointAddInterpolationFactor2(builder, interpolationFactor2): builder.PrependFloat64Slot(5, interpolationFactor2, 0.5) -def VrepControlPointAddVirtualDistance(builder, virtualDistance): builder.PrependFloat64Slot(6, virtualDistance, 0.0) -def VrepControlPointAddAuxiliaryFlags(builder, auxiliaryFlags): builder.PrependInt32Slot(7, auxiliaryFlags, 0) -def VrepControlPointAddAuxiliaryChannel1(builder, auxiliaryChannel1): builder.PrependFloat64Slot(8, auxiliaryChannel1, 0.0) -def VrepControlPointAddAuxiliaryChannel2(builder, auxiliaryChannel2): builder.PrependFloat64Slot(9, auxiliaryChannel2, 0.0) -def VrepControlPointAddAuxiliaryChannel3(builder, auxiliaryChannel3): builder.PrependFloat64Slot(10, auxiliaryChannel3, 0.0) -def VrepControlPointAddAuxiliaryChannel4(builder, auxiliaryChannel4): builder.PrependFloat64Slot(11, auxiliaryChannel4, 0.0) -def VrepControlPointEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/VrepPath.java b/include/grl/flatbuffer/grl/flatbuffer/VrepPath.java deleted file mode 100644 index ae3a459..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/VrepPath.java +++ /dev/null @@ -1,39 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class VrepPath extends Table { - public static VrepPath getRootAsVrepPath(ByteBuffer _bb) { return getRootAsVrepPath(_bb, new VrepPath()); } - public static VrepPath getRootAsVrepPath(ByteBuffer _bb, VrepPath obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public VrepPath __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - public VrepControlPoint controlPoints(int j) { return controlPoints(new VrepControlPoint(), j); } - public VrepControlPoint controlPoints(VrepControlPoint obj, int j) { int o = __offset(4); return o != 0 ? obj.__assign(__indirect(__vector(o) + j * 4), bb) : null; } - public int controlPointsLength() { int o = __offset(4); return o != 0 ? __vector_len(o) : 0; } - - public static int createVrepPath(FlatBufferBuilder builder, - int controlPointsOffset) { - builder.startObject(1); - VrepPath.addControlPoints(builder, controlPointsOffset); - return VrepPath.endVrepPath(builder); - } - - public static void startVrepPath(FlatBufferBuilder builder) { builder.startObject(1); } - public static void addControlPoints(FlatBufferBuilder builder, int controlPointsOffset) { builder.addOffset(0, controlPointsOffset, 0); } - public static int createControlPointsVector(FlatBufferBuilder builder, int[] data) { builder.startVector(4, data.length, 4); for (int i = data.length - 1; i >= 0; i--) builder.addOffset(data[i]); return builder.endVector(); } - public static void startControlPointsVector(FlatBufferBuilder builder, int numElems) { builder.startVector(4, numElems, 4); } - public static int endVrepPath(FlatBufferBuilder builder) { - int o = builder.endObject(); - return o; - } - public static void finishVrepPathBuffer(FlatBufferBuilder builder, int offset) { builder.finish(offset); } - public static void finishSizePrefixedVrepPathBuffer(FlatBufferBuilder builder, int offset) { builder.finishSizePrefixed(offset); } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/VrepPath.py b/include/grl/flatbuffer/grl/flatbuffer/VrepPath.py deleted file mode 100644 index 4fb6567..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/VrepPath.py +++ /dev/null @@ -1,44 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class VrepPath(object): - __slots__ = ['_tab'] - - @classmethod - def GetRootAsVrepPath(cls, buf, offset): - n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) - x = VrepPath() - x.Init(buf, n + offset) - return x - - # VrepPath - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - - # VrepPath - def ControlPoints(self, j): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - x = self._tab.Vector(o) - x += flatbuffers.number_types.UOffsetTFlags.py_type(j) * 4 - x = self._tab.Indirect(x) - from .VrepControlPoint import VrepControlPoint - obj = VrepControlPoint() - obj.Init(self._tab.Bytes, x) - return obj - return None - - # VrepPath - def ControlPointsLength(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - return self._tab.VectorLen(o) - return 0 - -def VrepPathStart(builder): builder.StartObject(1) -def VrepPathAddControlPoints(builder, controlPoints): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(controlPoints), 0) -def VrepPathStartControlPointsVector(builder, numElems): return builder.StartVector(4, numElems, 4) -def VrepPathEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/Wrench.java b/include/grl/flatbuffer/grl/flatbuffer/Wrench.java deleted file mode 100644 index 1d499d2..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/Wrench.java +++ /dev/null @@ -1,39 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class Wrench extends Struct { - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public Wrench __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - public Vector3d force() { return force(new Vector3d()); } - public Vector3d force(Vector3d obj) { return obj.__assign(bb_pos + 0, bb); } - public Vector3d torque() { return torque(new Vector3d()); } - public Vector3d torque(Vector3d obj) { return obj.__assign(bb_pos + 24, bb); } - public Vector3d forceOffset() { return forceOffset(new Vector3d()); } - public Vector3d forceOffset(Vector3d obj) { return obj.__assign(bb_pos + 48, bb); } - - public static int createWrench(FlatBufferBuilder builder, double force_x, double force_y, double force_z, double torque_x, double torque_y, double torque_z, double force_offset_x, double force_offset_y, double force_offset_z) { - builder.prep(8, 72); - builder.prep(8, 24); - builder.putDouble(force_offset_z); - builder.putDouble(force_offset_y); - builder.putDouble(force_offset_x); - builder.prep(8, 24); - builder.putDouble(torque_z); - builder.putDouble(torque_y); - builder.putDouble(torque_x); - builder.prep(8, 24); - builder.putDouble(force_z); - builder.putDouble(force_y); - builder.putDouble(force_x); - return builder.offset(); - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/Wrench.py b/include/grl/flatbuffer/grl/flatbuffer/Wrench.py deleted file mode 100644 index 9d94d48..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/Wrench.py +++ /dev/null @@ -1,44 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class Wrench(object): - __slots__ = ['_tab'] - - # Wrench - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - - # Wrench - def Force(self, obj): - obj.Init(self._tab.Bytes, self._tab.Pos + 0) - return obj - - # Wrench - def Torque(self, obj): - obj.Init(self._tab.Bytes, self._tab.Pos + 24) - return obj - - # Wrench - def ForceOffset(self, obj): - obj.Init(self._tab.Bytes, self._tab.Pos + 48) - return obj - - -def CreateWrench(builder, force_x, force_y, force_z, torque_x, torque_y, torque_z, force_offset_x, force_offset_y, force_offset_z): - builder.Prep(8, 72) - builder.Prep(8, 24) - builder.PrependFloat64(force_offset_z) - builder.PrependFloat64(force_offset_y) - builder.PrependFloat64(force_offset_x) - builder.Prep(8, 24) - builder.PrependFloat64(torque_z) - builder.PrependFloat64(torque_y) - builder.PrependFloat64(torque_x) - builder.Prep(8, 24) - builder.PrependFloat64(force_z) - builder.PrependFloat64(force_y) - builder.PrependFloat64(force_x) - return builder.Offset() diff --git a/include/grl/flatbuffer/grl/flatbuffer/__init__.py b/include/grl/flatbuffer/grl/flatbuffer/__init__.py deleted file mode 100644 index e69de29..0000000 diff --git a/include/grl/flatbuffer/grl/flatbuffer/ftk3DFiducial.java b/include/grl/flatbuffer/grl/flatbuffer/ftk3DFiducial.java deleted file mode 100644 index a20ab92..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/ftk3DFiducial.java +++ /dev/null @@ -1,39 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class ftk3DFiducial extends Table { - public static ftk3DFiducial getRootAsftk3DFiducial(ByteBuffer _bb) { return getRootAsftk3DFiducial(_bb, new ftk3DFiducial()); } - public static ftk3DFiducial getRootAsftk3DFiducial(ByteBuffer _bb, ftk3DFiducial obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public ftk3DFiducial __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - public long markerID() { int o = __offset(4); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } - public long leftIndex() { int o = __offset(6); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } - public long rightIndex() { int o = __offset(8); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } - public Vector3d position() { return position(new Vector3d()); } - public Vector3d position(Vector3d obj) { int o = __offset(10); return o != 0 ? obj.__assign(o + bb_pos, bb) : null; } - public double epipolarErrorPixels() { int o = __offset(12); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } - public double triangulationError() { int o = __offset(14); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } - public double probability() { int o = __offset(16); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } - - public static void startftk3DFiducial(FlatBufferBuilder builder) { builder.startObject(7); } - public static void addMarkerID(FlatBufferBuilder builder, long markerID) { builder.addInt(0, (int)markerID, (int)0L); } - public static void addLeftIndex(FlatBufferBuilder builder, long leftIndex) { builder.addInt(1, (int)leftIndex, (int)0L); } - public static void addRightIndex(FlatBufferBuilder builder, long rightIndex) { builder.addInt(2, (int)rightIndex, (int)0L); } - public static void addPosition(FlatBufferBuilder builder, int positionOffset) { builder.addStruct(3, positionOffset, 0); } - public static void addEpipolarErrorPixels(FlatBufferBuilder builder, double epipolarErrorPixels) { builder.addDouble(4, epipolarErrorPixels, 0.0); } - public static void addTriangulationError(FlatBufferBuilder builder, double triangulationError) { builder.addDouble(5, triangulationError, 0.0); } - public static void addProbability(FlatBufferBuilder builder, double probability) { builder.addDouble(6, probability, 0.0); } - public static int endftk3DFiducial(FlatBufferBuilder builder) { - int o = builder.endObject(); - return o; - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/ftk3DFiducial.py b/include/grl/flatbuffer/grl/flatbuffer/ftk3DFiducial.py deleted file mode 100644 index 7644659..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/ftk3DFiducial.py +++ /dev/null @@ -1,82 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class ftk3DFiducial(object): - __slots__ = ['_tab'] - - @classmethod - def GetRootAsftk3DFiducial(cls, buf, offset): - n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) - x = ftk3DFiducial() - x.Init(buf, n + offset) - return x - - # ftk3DFiducial - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - - # ftk3DFiducial - def MarkerID(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) - return 0 - - # ftk3DFiducial - def LeftIndex(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) - return 0 - - # ftk3DFiducial - def RightIndex(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) - return 0 - - # ftk3DFiducial - def Position(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) - if o != 0: - x = o + self._tab.Pos - from .Vector3d import Vector3d - obj = Vector3d() - obj.Init(self._tab.Bytes, x) - return obj - return None - - # ftk3DFiducial - def EpipolarErrorPixels(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(12)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) - return 0.0 - - # ftk3DFiducial - def TriangulationError(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(14)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) - return 0.0 - - # ftk3DFiducial - def Probability(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(16)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) - return 0.0 - -def ftk3DFiducialStart(builder): builder.StartObject(7) -def ftk3DFiducialAddMarkerID(builder, markerID): builder.PrependUint32Slot(0, markerID, 0) -def ftk3DFiducialAddLeftIndex(builder, leftIndex): builder.PrependUint32Slot(1, leftIndex, 0) -def ftk3DFiducialAddRightIndex(builder, rightIndex): builder.PrependUint32Slot(2, rightIndex, 0) -def ftk3DFiducialAddPosition(builder, position): builder.PrependStructSlot(3, flatbuffers.number_types.UOffsetTFlags.py_type(position), 0) -def ftk3DFiducialAddEpipolarErrorPixels(builder, epipolarErrorPixels): builder.PrependFloat64Slot(4, epipolarErrorPixels, 0.0) -def ftk3DFiducialAddTriangulationError(builder, triangulationError): builder.PrependFloat64Slot(5, triangulationError, 0.0) -def ftk3DFiducialAddProbability(builder, probability): builder.PrependFloat64Slot(6, probability, 0.0) -def ftk3DFiducialEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/ftkDeviceType.java b/include/grl/flatbuffer/grl/flatbuffer/ftkDeviceType.java deleted file mode 100644 index d4c2a51..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/ftkDeviceType.java +++ /dev/null @@ -1,13 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -public final class ftkDeviceType { - private ftkDeviceType() { } - public static final int DEV_SIMULATOR = 0; - public static final int DEV_INFINITRACK = 1; - public static final int DEV_FUSIONTRACK_500 = 2; - public static final int DEV_FUSIONTRACK_250 = 3; - public static final int DEV_UNKNOWN_DEVICE = 127; -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/ftkDeviceType.py b/include/grl/flatbuffer/grl/flatbuffer/ftkDeviceType.py deleted file mode 100644 index 6c94614..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/ftkDeviceType.py +++ /dev/null @@ -1,11 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -class ftkDeviceType(object): - DEV_SIMULATOR = 0 - DEV_INFINITRACK = 1 - DEV_FUSIONTRACK_500 = 2 - DEV_FUSIONTRACK_250 = 3 - DEV_UNKNOWN_DEVICE = 127 - diff --git a/include/grl/flatbuffer/grl/flatbuffer/ftkGeometry.java b/include/grl/flatbuffer/grl/flatbuffer/ftkGeometry.java deleted file mode 100644 index 780b533..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/ftkGeometry.java +++ /dev/null @@ -1,50 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class ftkGeometry extends Table { - public static ftkGeometry getRootAsftkGeometry(ByteBuffer _bb) { return getRootAsftkGeometry(_bb, new ftkGeometry()); } - public static ftkGeometry getRootAsftkGeometry(ByteBuffer _bb, ftkGeometry obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public ftkGeometry __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - public String name() { int o = __offset(4); return o != 0 ? __string(o + bb_pos) : null; } - public ByteBuffer nameAsByteBuffer() { return __vector_as_bytebuffer(4, 1); } - public ByteBuffer nameInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 4, 1); } - public long geometryID() { int o = __offset(6); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } - public long version() { int o = __offset(8); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } - public Vector3d positions(int j) { return positions(new Vector3d(), j); } - public Vector3d positions(Vector3d obj, int j) { int o = __offset(10); return o != 0 ? obj.__assign(__vector(o) + j * 24, bb) : null; } - public int positionsLength() { int o = __offset(10); return o != 0 ? __vector_len(o) : 0; } - - public static int createftkGeometry(FlatBufferBuilder builder, - int nameOffset, - long geometryID, - long version, - int positionsOffset) { - builder.startObject(4); - ftkGeometry.addPositions(builder, positionsOffset); - ftkGeometry.addVersion(builder, version); - ftkGeometry.addGeometryID(builder, geometryID); - ftkGeometry.addName(builder, nameOffset); - return ftkGeometry.endftkGeometry(builder); - } - - public static void startftkGeometry(FlatBufferBuilder builder) { builder.startObject(4); } - public static void addName(FlatBufferBuilder builder, int nameOffset) { builder.addOffset(0, nameOffset, 0); } - public static void addGeometryID(FlatBufferBuilder builder, long geometryID) { builder.addInt(1, (int)geometryID, (int)0L); } - public static void addVersion(FlatBufferBuilder builder, long version) { builder.addInt(2, (int)version, (int)0L); } - public static void addPositions(FlatBufferBuilder builder, int positionsOffset) { builder.addOffset(3, positionsOffset, 0); } - public static void startPositionsVector(FlatBufferBuilder builder, int numElems) { builder.startVector(24, numElems, 8); } - public static int endftkGeometry(FlatBufferBuilder builder) { - int o = builder.endObject(); - return o; - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/ftkGeometry.py b/include/grl/flatbuffer/grl/flatbuffer/ftkGeometry.py deleted file mode 100644 index 45db60a..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/ftkGeometry.py +++ /dev/null @@ -1,67 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class ftkGeometry(object): - __slots__ = ['_tab'] - - @classmethod - def GetRootAsftkGeometry(cls, buf, offset): - n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) - x = ftkGeometry() - x.Init(buf, n + offset) - return x - - # ftkGeometry - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - - # ftkGeometry - def Name(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - return self._tab.String(o + self._tab.Pos) - return bytes() - - # ftkGeometry - def GeometryID(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) - return 0 - - # ftkGeometry - def Version(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) - return 0 - - # ftkGeometry - def Positions(self, j): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) - if o != 0: - x = self._tab.Vector(o) - x += flatbuffers.number_types.UOffsetTFlags.py_type(j) * 24 - from .Vector3d import Vector3d - obj = Vector3d() - obj.Init(self._tab.Bytes, x) - return obj - return None - - # ftkGeometry - def PositionsLength(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) - if o != 0: - return self._tab.VectorLen(o) - return 0 - -def ftkGeometryStart(builder): builder.StartObject(4) -def ftkGeometryAddName(builder, name): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(name), 0) -def ftkGeometryAddGeometryID(builder, geometryID): builder.PrependUint32Slot(1, geometryID, 0) -def ftkGeometryAddVersion(builder, version): builder.PrependUint32Slot(2, version, 0) -def ftkGeometryAddPositions(builder, positions): builder.PrependUOffsetTRelativeSlot(3, flatbuffers.number_types.UOffsetTFlags.py_type(positions), 0) -def ftkGeometryStartPositionsVector(builder, numElems): return builder.StartVector(24, numElems, 8) -def ftkGeometryEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/ftkMarker.java b/include/grl/flatbuffer/grl/flatbuffer/ftkMarker.java deleted file mode 100644 index 796e077..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/ftkMarker.java +++ /dev/null @@ -1,42 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class ftkMarker extends Table { - public static ftkMarker getRootAsftkMarker(ByteBuffer _bb) { return getRootAsftkMarker(_bb, new ftkMarker()); } - public static ftkMarker getRootAsftkMarker(ByteBuffer _bb, ftkMarker obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public ftkMarker __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - public String name() { int o = __offset(4); return o != 0 ? __string(o + bb_pos) : null; } - public ByteBuffer nameAsByteBuffer() { return __vector_as_bytebuffer(4, 1); } - public ByteBuffer nameInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 4, 1); } - public long ID() { int o = __offset(6); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } - public long geometryID() { int o = __offset(8); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } - public long geometryPresenceMask(int j) { int o = __offset(10); return o != 0 ? (long)bb.getInt(__vector(o) + j * 4) & 0xFFFFFFFFL : 0; } - public int geometryPresenceMaskLength() { int o = __offset(10); return o != 0 ? __vector_len(o) : 0; } - public ByteBuffer geometryPresenceMaskAsByteBuffer() { return __vector_as_bytebuffer(10, 4); } - public ByteBuffer geometryPresenceMaskInByteBuffer(ByteBuffer _bb) { return __vector_in_bytebuffer(_bb, 10, 4); } - public Pose transform() { return transform(new Pose()); } - public Pose transform(Pose obj) { int o = __offset(12); return o != 0 ? obj.__assign(o + bb_pos, bb) : null; } - - public static void startftkMarker(FlatBufferBuilder builder) { builder.startObject(5); } - public static void addName(FlatBufferBuilder builder, int nameOffset) { builder.addOffset(0, nameOffset, 0); } - public static void addID(FlatBufferBuilder builder, long ID) { builder.addInt(1, (int)ID, (int)0L); } - public static void addGeometryID(FlatBufferBuilder builder, long geometryID) { builder.addInt(2, (int)geometryID, (int)0L); } - public static void addGeometryPresenceMask(FlatBufferBuilder builder, int geometryPresenceMaskOffset) { builder.addOffset(3, geometryPresenceMaskOffset, 0); } - public static int createGeometryPresenceMaskVector(FlatBufferBuilder builder, int[] data) { builder.startVector(4, data.length, 4); for (int i = data.length - 1; i >= 0; i--) builder.addInt(data[i]); return builder.endVector(); } - public static void startGeometryPresenceMaskVector(FlatBufferBuilder builder, int numElems) { builder.startVector(4, numElems, 4); } - public static void addTransform(FlatBufferBuilder builder, int transformOffset) { builder.addStruct(4, transformOffset, 0); } - public static int endftkMarker(FlatBufferBuilder builder) { - int o = builder.endObject(); - return o; - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/ftkMarker.py b/include/grl/flatbuffer/grl/flatbuffer/ftkMarker.py deleted file mode 100644 index 81700b2..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/ftkMarker.py +++ /dev/null @@ -1,82 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class ftkMarker(object): - __slots__ = ['_tab'] - - @classmethod - def GetRootAsftkMarker(cls, buf, offset): - n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) - x = ftkMarker() - x.Init(buf, n + offset) - return x - - # ftkMarker - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - - # ftkMarker - def Name(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - return self._tab.String(o + self._tab.Pos) - return bytes() - - # ftkMarker - def ID(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) - return 0 - - # ftkMarker - def GeometryID(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) - return 0 - - # ftkMarker - def GeometryPresenceMask(self, j): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) - if o != 0: - a = self._tab.Vector(o) - return self._tab.Get(flatbuffers.number_types.Uint32Flags, a + flatbuffers.number_types.UOffsetTFlags.py_type(j * 4)) - return 0 - - # ftkMarker - def GeometryPresenceMaskAsNumpy(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) - if o != 0: - return self._tab.GetVectorAsNumpy(flatbuffers.number_types.Uint32Flags, o) - return 0 - - # ftkMarker - def GeometryPresenceMaskLength(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) - if o != 0: - return self._tab.VectorLen(o) - return 0 - - # ftkMarker - def Transform(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(12)) - if o != 0: - x = o + self._tab.Pos - from .Pose import Pose - obj = Pose() - obj.Init(self._tab.Bytes, x) - return obj - return None - -def ftkMarkerStart(builder): builder.StartObject(5) -def ftkMarkerAddName(builder, name): builder.PrependUOffsetTRelativeSlot(0, flatbuffers.number_types.UOffsetTFlags.py_type(name), 0) -def ftkMarkerAddID(builder, ID): builder.PrependUint32Slot(1, ID, 0) -def ftkMarkerAddGeometryID(builder, geometryID): builder.PrependUint32Slot(2, geometryID, 0) -def ftkMarkerAddGeometryPresenceMask(builder, geometryPresenceMask): builder.PrependUOffsetTRelativeSlot(3, flatbuffers.number_types.UOffsetTFlags.py_type(geometryPresenceMask), 0) -def ftkMarkerStartGeometryPresenceMaskVector(builder, numElems): return builder.StartVector(4, numElems, 4) -def ftkMarkerAddTransform(builder, transform): builder.PrependStructSlot(4, flatbuffers.number_types.UOffsetTFlags.py_type(transform), 0) -def ftkMarkerEnd(builder): return builder.EndObject() diff --git a/include/grl/flatbuffer/grl/flatbuffer/ftkQueryStatus.java b/include/grl/flatbuffer/grl/flatbuffer/ftkQueryStatus.java deleted file mode 100644 index c10ee6e..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/ftkQueryStatus.java +++ /dev/null @@ -1,17 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -public final class ftkQueryStatus { - private ftkQueryStatus() { } - public static final int QS_WAR_SKIPPED = -1; - public static final int QS_OK = 0; - public static final int QS_ERR_OVERFLOW = 1; - public static final int QS_ERR_INVALID_RESERVED_SIZE = 2; - public static final int QS_REPROCESS = 10; - - public static final String[] names = { "QS_WAR_SKIPPED", "QS_OK", "QS_ERR_OVERFLOW", "QS_ERR_INVALID_RESERVED_SIZE", "", "", "", "", "", "", "", "QS_REPROCESS", }; - - public static String name(int e) { return names[e - QS_WAR_SKIPPED]; } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/ftkQueryStatus.py b/include/grl/flatbuffer/grl/flatbuffer/ftkQueryStatus.py deleted file mode 100644 index cae8d81..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/ftkQueryStatus.py +++ /dev/null @@ -1,11 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -class ftkQueryStatus(object): - QS_WAR_SKIPPED = -1 - QS_OK = 0 - QS_ERR_OVERFLOW = 1 - QS_ERR_INVALID_RESERVED_SIZE = 2 - QS_REPROCESS = 10 - diff --git a/include/grl/flatbuffer/grl/flatbuffer/ftkRegionOfInterest.java b/include/grl/flatbuffer/grl/flatbuffer/ftkRegionOfInterest.java deleted file mode 100644 index 2b09da6..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/ftkRegionOfInterest.java +++ /dev/null @@ -1,61 +0,0 @@ -// automatically generated by the FlatBuffers compiler, do not modify - -package grl.flatbuffer; - -import java.nio.*; -import java.lang.*; -import java.util.*; -import com.google.flatbuffers.*; - -@SuppressWarnings("unused") -public final class ftkRegionOfInterest extends Table { - public static ftkRegionOfInterest getRootAsftkRegionOfInterest(ByteBuffer _bb) { return getRootAsftkRegionOfInterest(_bb, new ftkRegionOfInterest()); } - public static ftkRegionOfInterest getRootAsftkRegionOfInterest(ByteBuffer _bb, ftkRegionOfInterest obj) { _bb.order(ByteOrder.LITTLE_ENDIAN); return (obj.__assign(_bb.getInt(_bb.position()) + _bb.position(), _bb)); } - public void __init(int _i, ByteBuffer _bb) { bb_pos = _i; bb = _bb; } - public ftkRegionOfInterest __assign(int _i, ByteBuffer _bb) { __init(_i, _bb); return this; } - - public double centerXPixels() { int o = __offset(4); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } - public double centerYPixels() { int o = __offset(6); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } - public long RightEdge() { int o = __offset(8); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } - public long BottomEdge() { int o = __offset(10); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } - public long LeftEdge() { int o = __offset(12); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } - public long TopEdge() { int o = __offset(14); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } - public long pixelsCount() { int o = __offset(16); return o != 0 ? (long)bb.getInt(o + bb_pos) & 0xFFFFFFFFL : 0L; } - public double probability() { int o = __offset(18); return o != 0 ? bb.getDouble(o + bb_pos) : 0.0; } - - public static int createftkRegionOfInterest(FlatBufferBuilder builder, - double centerXPixels, - double centerYPixels, - long RightEdge, - long BottomEdge, - long LeftEdge, - long TopEdge, - long pixelsCount, - double probability) { - builder.startObject(8); - ftkRegionOfInterest.addProbability(builder, probability); - ftkRegionOfInterest.addCenterYPixels(builder, centerYPixels); - ftkRegionOfInterest.addCenterXPixels(builder, centerXPixels); - ftkRegionOfInterest.addPixelsCount(builder, pixelsCount); - ftkRegionOfInterest.addTopEdge(builder, TopEdge); - ftkRegionOfInterest.addLeftEdge(builder, LeftEdge); - ftkRegionOfInterest.addBottomEdge(builder, BottomEdge); - ftkRegionOfInterest.addRightEdge(builder, RightEdge); - return ftkRegionOfInterest.endftkRegionOfInterest(builder); - } - - public static void startftkRegionOfInterest(FlatBufferBuilder builder) { builder.startObject(8); } - public static void addCenterXPixels(FlatBufferBuilder builder, double centerXPixels) { builder.addDouble(0, centerXPixels, 0.0); } - public static void addCenterYPixels(FlatBufferBuilder builder, double centerYPixels) { builder.addDouble(1, centerYPixels, 0.0); } - public static void addRightEdge(FlatBufferBuilder builder, long RightEdge) { builder.addInt(2, (int)RightEdge, (int)0L); } - public static void addBottomEdge(FlatBufferBuilder builder, long BottomEdge) { builder.addInt(3, (int)BottomEdge, (int)0L); } - public static void addLeftEdge(FlatBufferBuilder builder, long LeftEdge) { builder.addInt(4, (int)LeftEdge, (int)0L); } - public static void addTopEdge(FlatBufferBuilder builder, long TopEdge) { builder.addInt(5, (int)TopEdge, (int)0L); } - public static void addPixelsCount(FlatBufferBuilder builder, long pixelsCount) { builder.addInt(6, (int)pixelsCount, (int)0L); } - public static void addProbability(FlatBufferBuilder builder, double probability) { builder.addDouble(7, probability, 0.0); } - public static int endftkRegionOfInterest(FlatBufferBuilder builder) { - int o = builder.endObject(); - return o; - } -} - diff --git a/include/grl/flatbuffer/grl/flatbuffer/ftkRegionOfInterest.py b/include/grl/flatbuffer/grl/flatbuffer/ftkRegionOfInterest.py deleted file mode 100644 index 88dcf74..0000000 --- a/include/grl/flatbuffer/grl/flatbuffer/ftkRegionOfInterest.py +++ /dev/null @@ -1,86 +0,0 @@ -# automatically generated by the FlatBuffers compiler, do not modify - -# namespace: flatbuffer - -import flatbuffers - -class ftkRegionOfInterest(object): - __slots__ = ['_tab'] - - @classmethod - def GetRootAsftkRegionOfInterest(cls, buf, offset): - n = flatbuffers.encode.Get(flatbuffers.packer.uoffset, buf, offset) - x = ftkRegionOfInterest() - x.Init(buf, n + offset) - return x - - # ftkRegionOfInterest - def Init(self, buf, pos): - self._tab = flatbuffers.table.Table(buf, pos) - - # ftkRegionOfInterest - def CenterXPixels(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(4)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) - return 0.0 - - # ftkRegionOfInterest - def CenterYPixels(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(6)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) - return 0.0 - - # ftkRegionOfInterest - def RightEdge(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(8)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) - return 0 - - # ftkRegionOfInterest - def BottomEdge(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(10)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) - return 0 - - # ftkRegionOfInterest - def LeftEdge(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(12)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) - return 0 - - # ftkRegionOfInterest - def TopEdge(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(14)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) - return 0 - - # ftkRegionOfInterest - def PixelsCount(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(16)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Uint32Flags, o + self._tab.Pos) - return 0 - - # ftkRegionOfInterest - def Probability(self): - o = flatbuffers.number_types.UOffsetTFlags.py_type(self._tab.Offset(18)) - if o != 0: - return self._tab.Get(flatbuffers.number_types.Float64Flags, o + self._tab.Pos) - return 0.0 - -def ftkRegionOfInterestStart(builder): builder.StartObject(8) -def ftkRegionOfInterestAddCenterXPixels(builder, centerXPixels): builder.PrependFloat64Slot(0, centerXPixels, 0.0) -def ftkRegionOfInterestAddCenterYPixels(builder, centerYPixels): builder.PrependFloat64Slot(1, centerYPixels, 0.0) -def ftkRegionOfInterestAddRightEdge(builder, RightEdge): builder.PrependUint32Slot(2, RightEdge, 0) -def ftkRegionOfInterestAddBottomEdge(builder, BottomEdge): builder.PrependUint32Slot(3, BottomEdge, 0) -def ftkRegionOfInterestAddLeftEdge(builder, LeftEdge): builder.PrependUint32Slot(4, LeftEdge, 0) -def ftkRegionOfInterestAddTopEdge(builder, TopEdge): builder.PrependUint32Slot(5, TopEdge, 0) -def ftkRegionOfInterestAddPixelsCount(builder, pixelsCount): builder.PrependUint32Slot(6, pixelsCount, 0) -def ftkRegionOfInterestAddProbability(builder, probability): builder.PrependFloat64Slot(7, probability, 0.0) -def ftkRegionOfInterestEnd(builder): return builder.EndObject() From 936f03e8ca9ba02d317474649599fe7d8ee01df5 Mon Sep 17 00:00:00 2001 From: Chunting Date: Thu, 12 Apr 2018 12:05:05 -0400 Subject: [PATCH 092/102] Add new feature for replay process, now user can choose the run mode in lua --- .../grl/flatbuffer/ParseflatbuffertoCSV.hpp | 6 +- .../grl/vrep/InverseKinematicsVrepPlugin.hpp | 108 +++++++++++++----- src/lua/robone.lua | 4 +- .../v_repExtGrlInverseKinematics.cpp | 42 +++++-- 4 files changed, 118 insertions(+), 42 deletions(-) diff --git a/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp b/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp index 3bd01c6..5423c8c 100644 --- a/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp +++ b/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp @@ -799,7 +799,7 @@ int getRowsNumber(std::string filename){ /// Get the joint angles at specific time point (index) /// @return jointPosition, Eigen vector which contains joint position of the seven joints. - int64_t getJointAnglesFromCSV(std::string filename, Eigen::VectorXf &jointPosition, int rowIdx){ + int64_t getJointAnglesFromCSV(std::string filename, Eigen::VectorXf &jointPosition, int rowIdx, bool commanddata){ // int rowNum = getRowsNumber(filename); @@ -821,7 +821,11 @@ int getRowsNumber(std::string filename){ jointPosition(joint_index) = boost::lexical_cast((*loop)[joint_index+5]); } + if(commanddata){ + return boost::lexical_cast((*loop)[1]); + } return boost::lexical_cast((*loop)[0]); + } row++; diff --git a/include/grl/vrep/InverseKinematicsVrepPlugin.hpp b/include/grl/vrep/InverseKinematicsVrepPlugin.hpp index 08ca74c..24b4526 100644 --- a/include/grl/vrep/InverseKinematicsVrepPlugin.hpp +++ b/include/grl/vrep/InverseKinematicsVrepPlugin.hpp @@ -510,14 +510,27 @@ class InverseKinematicsVrepPlugin setObjectTransform(simGetObjectHandle("Dummy"),-1,tipTf); } -void getPoseFromCSV(std::string filename, int time_index){ +void ForwardKinematicsFromCSV(int time_index, bool commanddata=false){ // we only have one robot for the moment so the index of it is 0 const std::size_t simulatedRobotIndex = 0; auto& simArmMultiBody = rbd_mbs_[simulatedRobotIndex]; auto& simArmConfig = rbd_mbcs_[simulatedRobotIndex]; Eigen::VectorXf joint_angles = Eigen::VectorXf::Zero(7); // std::cout << "\nBefore: " << joint_angles << std::endl; - int64_t timeStamp = grl::getJointAnglesFromCSV(filename, joint_angles, time_index); + std::string homePath = std::getenv("HOME"); + std::string vrepDataPath = homePath + "/src/V-REP_PRO_EDU_V3_4_0_Linux/data/"; + std::string data_inPath = vrepDataPath + "data_in"; + // std::string markerPoseCSV = data_inPath+"/FT_Pose_Marker22.csv"; + std::string FK_CSV = "ForwardKinematics_Pose_M.csv"; + + std::string kukaJoint = data_inPath+"/KUKA_Measured_Joint.csv"; // KUKA_Joint.csv FTKUKA_TimeEvent.csv + if(commanddata) { + kukaJoint = data_inPath+"/KUKA_Command_Joint.csv"; + FK_CSV = "ForwardKinematics_Pose_C.csv"; + } + + + int64_t timeStamp = grl::getJointAnglesFromCSV(kukaJoint, joint_angles, time_index, commanddata); assert(timeStamp != -1 ); std::vector vrepJointAngles; @@ -550,14 +563,18 @@ void getPoseFromCSV(std::string filename, int time_index){ Eigen::VectorXd markerPose = grl::Affine3fToMarkerPose(transformEstimate.cast()); Eigen::RowVectorXd pose = grl::getPluckerPose(markerPose); - - // // Write the hand eye calibration results into a file - std::string suffix = "ForwardKinematics_Pose.csv"; - auto myFile = boost::filesystem::current_path() /suffix; - - boost::filesystem::ofstream ofs(myFile, std::ios_base::trunc | std::ios_base::out); + std::string myFile = vrepDataPath + FK_CSV; + boost::filesystem::ofstream ofs(myFile, std::ios_base::app | std::ios_base::out); + // if(time_index == 0){ + // ofs.open(myFile, std::ios_base::trunc | std::ios_base::out); + // } + if(time_index == 0) { - ofs << "local_receive_time_offset_X, K_X,K_Y,K_Z,K_A,K_B,K_C\n"; + if(commanddata){ + ofs << "local_receive_time_offset_X, C_K_X, C_K_Y, C_K_Z, C_K_A, C_K_B, C_K_C\n"; + }else{ + ofs << "local_receive_time_offset_X, M_K_X, M_K_Y, M_K_Z, M_K_A, M_K_B, M_K_C\n"; + } } ofs << timeStamp << ", " << pose[0] << ", " << pose[1] << ", "<< pose[2] << ", "<< pose[3] << ", "<< pose[4] << ", "<< pose[5] << "\n"; ofs.close(); @@ -662,6 +679,11 @@ void getPoseFromCSV(std::string filename, int time_index){ /// simulatedArm is the current simulation, measuredArm /// is based off of data measured from the real physical arm enum class ArmStateSource { simulatedArm, measuredArm }; + /// Determines which mode to call in run_one function. + /// ik_mode, run the updateKinematics(); + /// replay_mode, replay the collected data from robot, including command and measured joint angles; + /// test_mode, for the calibration purpose in vrep model; + enum class InvRunMode {ik_mode=1, replay_mode=2, test_mode=3 }; /// Runs inverse kinematics or constrained optimization at every simulation time step @@ -719,7 +741,7 @@ void getPoseFromCSV(std::string filename, int time_index){ const auto& handleParams = VrepRobotArmDriverSimulatedP_->getVrepHandleParams(); Eigen::Affine3d currentEndEffectorPose = getObjectTransform( - std::get(handleParams) + std::get(handleParams) ,std::get(handleParams) ); auto currentEigenT = currentEndEffectorPose.translation(); @@ -727,7 +749,7 @@ void getPoseFromCSV(std::string filename, int time_index){ Eigen::Affine3d desiredEndEffectorPose = getObjectTransform( - std::get(handleParams) + std::get(handleParams) ,std::get(handleParams) ); auto desiredEigenT = desiredEndEffectorPose.translation(); @@ -923,30 +945,50 @@ void getPoseFromCSV(std::string filename, int time_index){ /// may not need this it is in the base class /// blocking call, call in separate thread, just allocates memory void run_one(){ - // If true, run real inverse kinematics algorith, if false go to a test pose. - const bool ik = false; - if(ik) { - updateKinematics(); - } else { - std::string homePath = std::getenv("HOME"); - std::string vrepDataPath = homePath + "/src/V-REP_PRO_EDU_V3_4_0_Linux/data/"; - std::string data_inPath = vrepDataPath + "data_in"; - - std::string kukaJoint = data_inPath+"/KUKA_Measured_Joint.csv"; // KUKA_Joint.csv FTKUKA_TimeEvent.csv - std::string markerPose = data_inPath+"/FT_Pose_Marker22.csv"; - - testPose(); - // getPoseFromCSV(kukaJoint, time_index); - // replayMarker(markerPose, time_index); - // time_index++; - } + // ik_mode, run real inverse kinematics algorith; + // replay_mode, run the replay process; + // otherwise, go to a test pose. + switch(run_mode){ + case InvRunMode::ik_mode: + updateKinematics(); + logger_->info("run ik_mode...."); + break; + case InvRunMode::replay_mode: + ForwardKinematicsFromCSV(time_index, commanddata); + // replayMarker(markerPose, time_index); + time_index++; + logger_->info("run replay_mode.... and commanddata {}", commanddata); + break; + case InvRunMode::test_mode: + testPose(); + logger_->info("run test_mode...."); + break; + default: + logger_->info("Wrong parameter for run_mode, the only options are ik_mode=0, replay_mode=1, and test_mode=2 "); + } + } + /// Call this function in construct function to determine the running mode and running data. + void setInvRunMode(int _run_mode = 1, bool _commanddata = false) { + switch(_run_mode){ + case 1: + run_mode = InvRunMode::ik_mode; + break; + case 2: + run_mode = InvRunMode::replay_mode; + break; + case 3: + run_mode = InvRunMode::test_mode; + break; + default: + run_mode = InvRunMode::ik_mode; + std::cout << "_run_mode: " << _run_mode << "is beyond index..." << std::endl; + } + commanddata = _commanddata; } - - /// may not need this it is in the base class /// this will have output /// blocking call, call in separate thread, just allocates memory - /// this runs the actual optimization algorithm + /// this runs the actual optimization algorithmn void solve(){ } @@ -1008,6 +1050,10 @@ void getPoseFromCSV(std::string filename, int time_index){ std::shared_ptr VrepRobotArmDriverSimulatedP_; std::shared_ptr VrepRobotArmDriverMeasuredP_; vrep::VrepRobotArmDriver::State currentArmState_; + + /// These two parameters are for the replay process. + InvRunMode run_mode = InvRunMode::ik_mode; + bool commanddata = false; // Determines what kind of joint angles to run, command or measured? bool ranOnce_ = false; EIGEN_MAKE_ALIGNED_OPERATOR_NEW diff --git a/src/lua/robone.lua b/src/lua/robone.lua index b14b1c4..b31e3f8 100644 --- a/src/lua/robone.lua +++ b/src/lua/robone.lua @@ -312,7 +312,9 @@ robone.cutBoneScript=function() useRMLSmoothing = false if (grl.isModuleLoaded('GrlInverseKinematics') and useGrlInverseKinematics) then - simExtGrlInverseKinematicsStart() + -- Didn't find any example about lua enum, so here use different int to determine run_mode. + run_mode = { ik_mode = 1, replay_mode = 2, test_mode = 3} + simExtGrlInverseKinematicsStart(run_mode.test_mode, false) end print("Moving Robotiiwa arm along cut path...") diff --git a/src/v_repExtGrlInverseKinematics/v_repExtGrlInverseKinematics.cpp b/src/v_repExtGrlInverseKinematics/v_repExtGrlInverseKinematics.cpp index 4e8fc56..749811e 100755 --- a/src/v_repExtGrlInverseKinematics/v_repExtGrlInverseKinematics.cpp +++ b/src/v_repExtGrlInverseKinematics/v_repExtGrlInverseKinematics.cpp @@ -18,7 +18,7 @@ #include #include "v_repExtGrlInverseKinematics.h" #include "grl/vrep/InverseKinematicsVrepPlugin.hpp" - +#include "luaFunctionData.h" #include #include @@ -43,6 +43,9 @@ LIBRARY vrepLib; // the V-REP library that we will dynamically load and bind #define CONCAT(x,y,z) x y z #define strConCat(x,y,z) CONCAT(x,y,z) #define LUA_GET_SENSOR_DATA_COMMAND "simExtSkeleton_getSensorData" +#define LUA_SIM_EXT_GRL_IK_START_COMMAND "simExtGrlInverseKinematicsStart" + + @@ -60,15 +63,27 @@ std::shared_ptr loggerPG; return (p,o); } */ - +const int inArgs_SIM_EXT_GRL_IK_START[]={ + 2, + sim_lua_arg_int,0, + sim_lua_arg_bool, 0 +}; +std::string LUA_SIM_EXT_GRL_IK_START_CALL_TIP("number result=simExtGrlInverseKinematicsStart(int run_mode, bool commanddataing)"); void LUA_SIM_EXT_GRL_IK_START(SLuaCallBack* p) { - if (!InverseKinematicsControllerPG) { - - loggerPG->error("v_repExtInverseKinematicsController Starting Inverse Kinematics Plugin\n"); - InverseKinematicsControllerPG=std::make_shared(); - InverseKinematicsControllerPG->construct(); - } + if (!InverseKinematicsControllerPG) { + loggerPG->error("v_repExtInverseKinematicsController Starting Inverse Kinematics Plugin\n"); + CLuaFunctionData data; + if (data.readDataFromLua(p, inArgs_SIM_EXT_GRL_IK_START, inArgs_SIM_EXT_GRL_IK_START[0], LUA_SIM_EXT_GRL_IK_START_COMMAND)) + { + std::vector* inData = data.getInDataPtr(); + int run_mode = inData->at(0).intData[0]; + bool commanddata = inData->at(1).boolData[0]; + InverseKinematicsControllerPG = std::make_shared(); + InverseKinematicsControllerPG->construct(); + InverseKinematicsControllerPG->setInvRunMode(run_mode, commanddata); + } + } } void LUA_SIM_EXT_GRL_IK_RESET(SLuaCallBack* p) @@ -179,7 +194,7 @@ VREP_DLLEXPORT unsigned char v_repStart(void* reservedPointer,int reservedInt) int noArgs[]={0}; // no input arguments - simRegisterCustomLuaFunction("simExtGrlInverseKinematicsStart","number result=simExtGrlInverseKinematicsStart()",noArgs,LUA_SIM_EXT_GRL_IK_START); + // simRegisterCustomLuaFunction("simExtGrlInverseKinematicsStart","number result=simExtGrlInverseKinematicsStart()",noArgs,LUA_SIM_EXT_GRL_IK_START); simRegisterCustomLuaFunction("simExtGrlInverseKinematicsStop","number result=simExtGrlInverseKinematicsStop()",noArgs,LUA_SIM_EXT_GRL_IK_STOP); simRegisterCustomLuaFunction("simExtGrlInverseKinematicsReset","number result=simExtGrlInverseKinematicsReset()",noArgs,LUA_SIM_EXT_GRL_IK_RESET); simRegisterCustomLuaFunction("simExtGrlInverseKinematicsAddFrame","number result=simExtGrlInverseKinematicsAddFrame()",noArgs,LUA_SIM_EXT_GRL_IK_ADD_FRAME); @@ -187,7 +202,16 @@ VREP_DLLEXPORT unsigned char v_repStart(void* reservedPointer,int reservedInt) simRegisterCustomLuaFunction("simExtGrlInverseKinematicsApplyTransform","number result=simExtGrlInverseKinematicsApplyTransform()",noArgs,LUA_SIM_EXT_GRL_IK_APPLY_TRANSFORM); simRegisterCustomLuaFunction("simExtGrlInverseKinematicsRestoreSensorPosition","number result=simExtGrlInverseKinematicsRestoreSensorPosition()",noArgs,LUA_SIM_EXT_GRL_IK_RESTORE_SENSOR_POSITION); + std::vector inArgs; + CLuaFunctionData::getInputDataForFunctionRegistration(inArgs_SIM_EXT_GRL_IK_START, inArgs); + simRegisterCustomLuaFunction + ( + LUA_SIM_EXT_GRL_IK_START_COMMAND, + LUA_SIM_EXT_GRL_IK_START_CALL_TIP.c_str(), + &inArgs[0], + LUA_SIM_EXT_GRL_IK_START + ); // ****************************************** loggerPG->info("Inverse Kinematics plugin initialized. Build date/time: {} {}", __DATE__, __TIME__); From ed8a7122c5fbf6b74bc32e4f7973084e3f4cdcaa Mon Sep 17 00:00:00 2001 From: Chunting Date: Fri, 13 Apr 2018 18:08:55 -0400 Subject: [PATCH 093/102] Plot the time latency curves --- .../grl/flatbuffer/ParseflatbuffertoCSV.hpp | 18 +++++++-------- .../grl/vrep/InverseKinematicsVrepPlugin.hpp | 22 ++++++++++--------- src/lua/robone.lua | 15 ++++++++----- .../v_repExtAtracsysFusionTrack.cpp | 2 +- .../v_repExtGrlInverseKinematics.cpp | 4 ++-- .../v_repExtKukaLBRiiwa.cpp | 3 +-- test/readFlatbufferTest.cpp | 13 +++++++---- 7 files changed, 43 insertions(+), 34 deletions(-) diff --git a/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp b/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp index 5423c8c..7bad50e 100644 --- a/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp +++ b/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp @@ -158,12 +158,12 @@ namespace grl { int64_t initial_device_time, int rowIndex){ std::cout<< "initial_local_time: " << initial_local_time << std::endl; - std::size_t time_size = timeEventM.rows(); - assert(time_size>0); - timeEventM.col(grl::TimeType::local_request_time) = timeEventM.col(grl::TimeType::local_request_time) - initial_local_time * grl::VectorXd::Ones(time_size); - timeEventM.col(grl::TimeType::local_receive_time) = timeEventM.col(grl::TimeType::local_receive_time) - initial_local_time * grl::VectorXd::Ones(time_size); - timeEventM.col(grl::TimeType::device_time) = timeEventM.col(grl::TimeType::device_time) - initial_device_time * grl::VectorXd::Ones(time_size); - timeEventM.col(grl::TimeType::time_Y) = timeEventM.col(grl::TimeType::time_Y) - timeEventM(rowIndex, grl::TimeType::time_Y) * grl::VectorXd::Ones(time_size); + std::size_t time_size = timeEventM.rows(); + assert(time_size>0); + timeEventM.col(grl::TimeType::local_request_time) = timeEventM.col(grl::TimeType::local_request_time) - initial_local_time * grl::VectorXd::Ones(time_size); + timeEventM.col(grl::TimeType::local_receive_time) = timeEventM.col(grl::TimeType::local_receive_time) - initial_local_time * grl::VectorXd::Ones(time_size); + timeEventM.col(grl::TimeType::device_time) = timeEventM.col(grl::TimeType::device_time) - initial_device_time * grl::VectorXd::Ones(time_size); + timeEventM.col(grl::TimeType::time_Y) = timeEventM.col(grl::TimeType::time_Y) - timeEventM(rowIndex, grl::TimeType::time_Y) * grl::VectorXd::Ones(time_size); } @@ -384,7 +384,7 @@ namespace grl { assert(kuka_time_size == externalTorque.rows()); assert(kuka_time_size == jointStateInterpolated.rows()); auto receive_time = timeEventM_Kuka.col(grl::TimeType::local_receive_time); - assert(checkmonotonic(receive_time)); + // assert(checkmonotonic(receive_time)); std::ofstream fs; // create a name for the file output @@ -822,9 +822,9 @@ int getRowsNumber(std::string filename){ jointPosition(joint_index) = boost::lexical_cast((*loop)[joint_index+5]); } if(commanddata){ - return boost::lexical_cast((*loop)[1]); + return boost::lexical_cast((*loop)[0]); } - return boost::lexical_cast((*loop)[0]); + return boost::lexical_cast((*loop)[1]); } row++; diff --git a/include/grl/vrep/InverseKinematicsVrepPlugin.hpp b/include/grl/vrep/InverseKinematicsVrepPlugin.hpp index 24b4526..6f3d1a9 100644 --- a/include/grl/vrep/InverseKinematicsVrepPlugin.hpp +++ b/include/grl/vrep/InverseKinematicsVrepPlugin.hpp @@ -563,17 +563,19 @@ void ForwardKinematicsFromCSV(int time_index, bool commanddata=false){ Eigen::VectorXd markerPose = grl::Affine3fToMarkerPose(transformEstimate.cast()); Eigen::RowVectorXd pose = grl::getPluckerPose(markerPose); - std::string myFile = vrepDataPath + FK_CSV; + auto myFile = boost::filesystem::path(vrepDataPath + FK_CSV); + if(time_index == 0 && boost::filesystem::exists(myFile)){ + boost::filesystem::remove(myFile); + std::cout << myFile << " has been removed..." << std::endl; + } boost::filesystem::ofstream ofs(myFile, std::ios_base::app | std::ios_base::out); - // if(time_index == 0){ - // ofs.open(myFile, std::ios_base::trunc | std::ios_base::out); - // } + if(time_index == 0) { if(commanddata){ - ofs << "local_receive_time_offset_X, C_K_X, C_K_Y, C_K_Z, C_K_A, C_K_B, C_K_C\n"; + ofs << "local_request_time_offset_X, C_K_X, C_K_Y, C_K_Z, C_K_A, C_K_B, C_K_C\n"; }else{ - ofs << "local_receive_time_offset_X, M_K_X, M_K_Y, M_K_Z, M_K_A, M_K_B, M_K_C\n"; + ofs << "local_receive_time_offset_X, M_K_X, M_K_Y, M_K_Z, M_K_A, M_K_B, M_K_C\n"; } } ofs << timeStamp << ", " << pose[0] << ", " << pose[1] << ", "<< pose[2] << ", "<< pose[3] << ", "<< pose[4] << ", "<< pose[5] << "\n"; @@ -708,7 +710,6 @@ void ForwardKinematicsFromCSV(int time_index, bool commanddata=false){ jointHandles_ = VrepRobotArmDriverSimulatedP_->getJointHandles(); - /////////////////////////////////////////////////////////// // Copy Joint Interval, the range of motion for each joint @@ -951,17 +952,17 @@ void ForwardKinematicsFromCSV(int time_index, bool commanddata=false){ switch(run_mode){ case InvRunMode::ik_mode: updateKinematics(); - logger_->info("run ik_mode...."); + // logger_->info("run ik_mode...."); break; case InvRunMode::replay_mode: ForwardKinematicsFromCSV(time_index, commanddata); // replayMarker(markerPose, time_index); time_index++; - logger_->info("run replay_mode.... and commanddata {}", commanddata); + // logger_->info("run replay_mode.... and commanddata {}", commanddata); break; case InvRunMode::test_mode: testPose(); - logger_->info("run test_mode...."); + // logger_->info("run test_mode...."); break; default: logger_->info("Wrong parameter for run_mode, the only options are ik_mode=0, replay_mode=1, and test_mode=2 "); @@ -984,6 +985,7 @@ void ForwardKinematicsFromCSV(int time_index, bool commanddata=false){ std::cout << "_run_mode: " << _run_mode << "is beyond index..." << std::endl; } commanddata = _commanddata; + std::cout << "_run_mode: " << _run_mode << " commanddata" << commanddata << std::endl; } /// may not need this it is in the base class /// this will have output diff --git a/src/lua/robone.lua b/src/lua/robone.lua index b31e3f8..cb4fc33 100644 --- a/src/lua/robone.lua +++ b/src/lua/robone.lua @@ -12,8 +12,8 @@ robone = {} require "grl" -KUKA_single_buffer_limit_bytes = 15 -- MB -FB_single_buffer_limit_bytes = 160 -- MB +KUKA_single_buffer_limit_bytes = 256 -- MB +FB_single_buffer_limit_bytes = 1024 -- MB ------------------------------------------ -- Move the arm along the cut file path -- ------------------------------------------ @@ -80,8 +80,6 @@ robone.cutBoneScript=function() simMoveToObject(target,CreatedPathHandle,3,0,0.2,0.02) newPosition = 0 - - -- Create empty vector for tool tip forces (x,y,z,alpha,beta,gamma,joint dependence(?)) -- toolTipForces = matrix(7,1,0) toolTipForces = {0,0,0,0,0,0,0} @@ -312,9 +310,14 @@ robone.cutBoneScript=function() useRMLSmoothing = false if (grl.isModuleLoaded('GrlInverseKinematics') and useGrlInverseKinematics) then - -- Didn't find any example about lua enum, so here use different int to determine run_mode. + -- ik_mode, run real inverse kinematics algorith; + -- replay_mode, run the replay process; + -- otherwise, go to a test pose. + -- commanddata, only in replay_mode we need to set it to determine the joint data set. + commanddata = false run_mode = { ik_mode = 1, replay_mode = 2, test_mode = 3} - simExtGrlInverseKinematicsStart(run_mode.test_mode, false) + print("Moving Robotiiwa arm along inversekinematics") + simExtGrlInverseKinematicsStart(run_mode.replay_mode, commanddata) end print("Moving Robotiiwa arm along cut path...") diff --git a/src/v_repExtAtracsysFusionTrack/v_repExtAtracsysFusionTrack.cpp b/src/v_repExtAtracsysFusionTrack/v_repExtAtracsysFusionTrack.cpp index e5fc60c..4198367 100755 --- a/src/v_repExtAtracsysFusionTrack/v_repExtAtracsysFusionTrack.cpp +++ b/src/v_repExtAtracsysFusionTrack/v_repExtAtracsysFusionTrack.cpp @@ -751,7 +751,7 @@ VREP_DLLEXPORT void *v_repMessage(int message, int *auxiliaryData, void *customD //////////////////// loggerPG->error("Ending Fusion Tracker plugin connection to Kuka iiwa\n" ); loggerPG->info(" Atracsys recordWhileSimulationIsRunningG: {}", recordWhileSimulationIsRunningG); - loggerPG->info("fusionTrackPG->is_recording(): {}",fusionTrackPG->is_recording()); + // loggerPG->info("fusionTrackPG->is_recording(): {}",fusionTrackPG->is_recording()); if(fusionTrackPG && recordWhileSimulationIsRunningG && fusionTrackPG->is_recording()) { bool success = fusionTrackPG->save_recording(); diff --git a/src/v_repExtGrlInverseKinematics/v_repExtGrlInverseKinematics.cpp b/src/v_repExtGrlInverseKinematics/v_repExtGrlInverseKinematics.cpp index 749811e..b2dc8d0 100755 --- a/src/v_repExtGrlInverseKinematics/v_repExtGrlInverseKinematics.cpp +++ b/src/v_repExtGrlInverseKinematics/v_repExtGrlInverseKinematics.cpp @@ -65,14 +65,14 @@ std::shared_ptr loggerPG; */ const int inArgs_SIM_EXT_GRL_IK_START[]={ 2, - sim_lua_arg_int,0, + sim_lua_arg_int, 0, sim_lua_arg_bool, 0 }; std::string LUA_SIM_EXT_GRL_IK_START_CALL_TIP("number result=simExtGrlInverseKinematicsStart(int run_mode, bool commanddataing)"); void LUA_SIM_EXT_GRL_IK_START(SLuaCallBack* p) { if (!InverseKinematicsControllerPG) { - loggerPG->error("v_repExtInverseKinematicsController Starting Inverse Kinematics Plugin\n"); + loggerPG->info("v_repExtInverseKinematicsController Starting Inverse Kinematics Plugin\n"); CLuaFunctionData data; if (data.readDataFromLua(p, inArgs_SIM_EXT_GRL_IK_START, inArgs_SIM_EXT_GRL_IK_START[0], LUA_SIM_EXT_GRL_IK_START_COMMAND)) { diff --git a/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp b/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp index 86797ca..5027efa 100755 --- a/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp +++ b/src/v_repExtKukaLBRiiwaPlugin/v_repExtKukaLBRiiwa.cpp @@ -81,6 +81,7 @@ void LUA_SIM_EXT_KUKA_LBR_IIWA_START(SLuaCallBack* p) /// if (!kukaVrepPluginPG) { loggerPG->error("simExtKukaLBRiiwaStart: Starting KUKA LBR iiwa plugin connection to Kuka iiwa...\n kukaVrepPluginPG hasn't been initialized...\n" ); + } CLuaFunctionData data; @@ -137,8 +138,6 @@ void LUA_SIM_EXT_KUKA_LBR_IIWA_START(SLuaCallBack* p) kukaVrepPluginPG->construct(); } - } - } catch (const boost::exception& e){ // log the error and print it to the screen, don't release the exception std::string initerr("v_repExtKukaLBRiiwa plugin encountered the following error and will disable itself:\n" + boost::diagnostic_information(e)); diff --git a/test/readFlatbufferTest.cpp b/test/readFlatbufferTest.cpp index 06bf7fb..4adb299 100644 --- a/test/readFlatbufferTest.cpp +++ b/test/readFlatbufferTest.cpp @@ -31,13 +31,14 @@ int main(int argc, char* argv[]) { - std::string kukaTimeStamp("2018_03_26_19_06_21_Kukaiiwa.iiwa"); - std::string FTTimeStamp("2018_03_26_19_06_21_FusionTrack.flik"); + std::string kukaTimeStamp("2018_04_13_16_20_28_Kukaiiwa.iiwa"); + std::string FTTimeStamp("2018_04_13_16_20_28_FusionTrack.flik"); std::string URDFModrl("Robone_KukaLBRiiwa.urdf"); /// Define the csv file names if(argc == 2){ - kukaTimeStamp = std::string(argv[1]); - FTTimeStamp = std::string(argv[2]); + kukaTimeStamp = std::string(argv[0]); + FTTimeStamp = std::string(argv[1]); + std::cout << "Arguments: " << kukaTimeStamp << " " << FTTimeStamp << std::endl; } /// Create a new folder in the name of time stamp for the generated csv files. std::string homePath = std::getenv("HOME"); @@ -211,6 +212,8 @@ int main(int argc, char* argv[]) // Change it back to the original size timeEventM_FT.resize(FT_size, grl::col_timeEvent); markerPose_FT.resize(FT_size, grl::col_Pose); + timeEventM_FT = grl::MatrixXd::Zero(FT_size, grl::col_timeEvent); + markerPose_FT = Eigen::MatrixXd::Zero(FT_size, grl::col_Pose); // Get the pose of the bone marker int validsize_55 = grl::getMarkerPose(logKUKAiiwaFusionTrackP, markerID_55, timeEventM_FT, markerPose_FT); @@ -221,6 +224,8 @@ int main(int argc, char* argv[]) // Change it back to the original size timeEventM_FT.resize(FT_size, grl::col_timeEvent); markerPose_FT.resize(FT_size, grl::col_Pose); + timeEventM_FT = grl::MatrixXd::Zero(FT_size, grl::col_timeEvent); + markerPose_FT = Eigen::MatrixXd::Zero(FT_size, grl::col_Pose); // Get the pose of the bone marker int validsize_50000 = grl::getMarkerPose(logKUKAiiwaFusionTrackP, markerID_50000, timeEventM_FT, markerPose_FT); From 9c83f12ffd06765c40f89e7bce249fd8e5ea4b4f Mon Sep 17 00:00:00 2001 From: Chunting Date: Sun, 15 Apr 2018 12:17:27 -0400 Subject: [PATCH 094/102] Using quaternion replaces Euler-Angles to present marker orientiation --- .../grl/flatbuffer/ParseflatbuffertoCSV.hpp | 17 ++++++------ .../grl/vrep/InverseKinematicsVrepPlugin.hpp | 9 +------ src/lua/robone.lua | 2 +- test/readFlatbufferTest.cpp | 27 ++++++++++--------- 4 files changed, 25 insertions(+), 30 deletions(-) diff --git a/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp b/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp index 7bad50e..960cdfa 100644 --- a/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp +++ b/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp @@ -108,6 +108,7 @@ namespace grl { int col_Kuka_Joint = Joint_Labels.size(); const static int jointNum = 7; int col_Pose = PK_Pose_Labels.size(); + int col_trans = Transform_Labels.size(); /// Get CSV labels @@ -178,8 +179,7 @@ namespace grl { int getMarkerPose(const fbs_tk::Root &logKUKAiiwaFusionTrackP, uint32_t &markerID, grl::MatrixXd& timeEventM, - Eigen::MatrixXd& markerPose, - bool markerFrame = false){ + Eigen::MatrixXd& markerPose){ auto states = logKUKAiiwaFusionTrackP->states(); std::size_t state_size = states->size(); assert(state_size>0); @@ -265,7 +265,6 @@ namespace grl { assert(state_size>0); // The first columne is counter int row = 0; - // Eigen::MatrixXd markerPose(state_size, grl::col_Pose); // std::cout <<"State size: " << state_size << " markerPose rows: " << markerPose.rows() << std::endl; int BadCount = 0; @@ -640,15 +639,16 @@ namespace grl { fs << "local_request_time," << "local_receive_time," << "FT_device_time_offset," - << "device_time_offset_kuka," + << "KUKA_device_time_offset," << "Y_FT," << "Y_kuka," << "FT_X," << "FT_Y," << "FT_Z," - << "FT_A," - << "FT_B," - << "FT_C," + << "FT_QW" + << "FT_QX," + << "FT_QY," + << "FT_QZ," << "K_Joint1," << "K_Joint2," << "K_Joint3," @@ -697,7 +697,8 @@ namespace grl { << matrixrow[2] << "," << matrixrow[3] << "," << matrixrow[4] << "," - << matrixrow[5] << std::endl; + << matrixrow[5] << "," + << matrixrow[6] << std::endl; FT_index++; } else { // In case the time is extactly equivent with each other. diff --git a/include/grl/vrep/InverseKinematicsVrepPlugin.hpp b/include/grl/vrep/InverseKinematicsVrepPlugin.hpp index 6f3d1a9..bf84dfc 100644 --- a/include/grl/vrep/InverseKinematicsVrepPlugin.hpp +++ b/include/grl/vrep/InverseKinematicsVrepPlugin.hpp @@ -433,18 +433,11 @@ class InverseKinematicsVrepPlugin } /// @todo verify object lifetimes - - // add virtual fixtures? need names and number of rows - - + /// add virtual fixtures? need names and number of rows /// @todo read objective rows from vrep - /// @todo set vrep explicit handling of IK here, plus unset in destructor of this object - } - - /// Set dummy frames named Dummy0 to Dummy19 to the current state of the /// RBDyn and V-REP representations of joints for debugging. void debugFrames(bool print = false) diff --git a/src/lua/robone.lua b/src/lua/robone.lua index cb4fc33..76c16d2 100644 --- a/src/lua/robone.lua +++ b/src/lua/robone.lua @@ -317,7 +317,7 @@ robone.cutBoneScript=function() commanddata = false run_mode = { ik_mode = 1, replay_mode = 2, test_mode = 3} print("Moving Robotiiwa arm along inversekinematics") - simExtGrlInverseKinematicsStart(run_mode.replay_mode, commanddata) + simExtGrlInverseKinematicsStart(run_mode.test_mode, commanddata) end print("Moving Robotiiwa arm along cut path...") diff --git a/test/readFlatbufferTest.cpp b/test/readFlatbufferTest.cpp index 4adb299..a717b6c 100644 --- a/test/readFlatbufferTest.cpp +++ b/test/readFlatbufferTest.cpp @@ -89,7 +89,7 @@ int main(int argc, char* argv[]) std::size_t kuka_size = grl::getStateSize(kukaStatesP); /// Create matrix to contain data - Eigen::MatrixXd markerPose_FT(FT_size, grl::col_Pose); + Eigen::MatrixXd markerPose_FT(FT_size, grl::col_trans); Eigen::MatrixXd markerTransform_FT = Eigen::MatrixXd::Zero(FT_size, grl::Transform_Labels.size()); grl::MatrixXd timeEventM_FT = grl::MatrixXd::Zero(FT_size, grl::Time_Labels.size()); grl::MatrixXd timeEventM_Kuka = grl::MatrixXd::Zero(kuka_size, grl::Time_Labels.size()); @@ -114,7 +114,8 @@ int main(int argc, char* argv[]) commandedTorque, externalTorque, jointStateInterpolated); - int validsize_22 = grl::getMarkerPose(logKUKAiiwaFusionTrackP, markerID_22, timeEventM_FT, markerPose_FT); + // int validsize_22 = grl::getMarkerPose(logKUKAiiwaFusionTrackP, markerID_22, timeEventM_FT, markerPose_FT); + int validsize_22 = grl::getMarkerMessage(logKUKAiiwaFusionTrackP, markerID_22, timeEventM_FT, markerPose_FT); std::cout<< "------Kuka_size: "<< kuka_size << " FT_size: "<< FT_size << std::endl; std::cout<<"validsize_22: " << validsize_22 << std::endl; @@ -204,34 +205,34 @@ int main(int argc, char* argv[]) //////////////////////////////////////////////////////////////////////////////////////////////////// // Get the pose of the marker 22. // Since the bad data is skipped, the timeEventM_FT also needs to be filled out with the orinial time stamp. - std::vector FT_Labels_Pose = grl::getLabels(grl::LabelsType::FT_Pose); grl::writeFTKUKAMessageToCSV(FTKUKA_TimeEvent_CSV, timeEventM_FT, timeEventM_Kuka, measuredJointPosition, markerPose_FT, FT_index, kuka_index); grl::writeTimeEventToCSV(FT_TimeEvent_CSV, timeEventM_FT, FT_index); - grl::writeMatrixToCSV(FT_Marker22_CSV, FT_Labels_Pose, timeEventM_FT, markerPose_FT, FT_index); + grl::writeMatrixToCSV(FT_Marker22_CSV, grl::Transform_Labels, timeEventM_FT, markerPose_FT, FT_index); // Change it back to the original size timeEventM_FT.resize(FT_size, grl::col_timeEvent); - markerPose_FT.resize(FT_size, grl::col_Pose); + markerPose_FT.resize(FT_size, grl::col_trans); timeEventM_FT = grl::MatrixXd::Zero(FT_size, grl::col_timeEvent); - markerPose_FT = Eigen::MatrixXd::Zero(FT_size, grl::col_Pose); + markerPose_FT = Eigen::MatrixXd::Zero(FT_size, grl::col_trans); // Get the pose of the bone marker - int validsize_55 = grl::getMarkerPose(logKUKAiiwaFusionTrackP, markerID_55, timeEventM_FT, markerPose_FT); - + // int validsize_55 = grl::getMarkerPose(logKUKAiiwaFusionTrackP, markerID_55, timeEventM_FT, markerPose_FT); + int validsize_55 = grl::getMarkerMessage(logKUKAiiwaFusionTrackP, markerID_55, timeEventM_FT, markerPose_FT); std::cout<<"validsize_55: " << validsize_55 << std::endl; grl::regularizeTimeEvent(timeEventM_FT, initial_local_time, initial_device_time_FT, FT_index); - grl::writeMatrixToCSV(FT_Marker55_CSV, FT_Labels_Pose, timeEventM_FT, markerPose_FT, FT_index); + grl::writeMatrixToCSV(FT_Marker55_CSV, grl::Transform_Labels, timeEventM_FT, markerPose_FT, FT_index); // Change it back to the original size timeEventM_FT.resize(FT_size, grl::col_timeEvent); - markerPose_FT.resize(FT_size, grl::col_Pose); + markerPose_FT.resize(FT_size, grl::col_trans); timeEventM_FT = grl::MatrixXd::Zero(FT_size, grl::col_timeEvent); - markerPose_FT = Eigen::MatrixXd::Zero(FT_size, grl::col_Pose); + markerPose_FT = Eigen::MatrixXd::Zero(FT_size, grl::col_trans); // Get the pose of the bone marker - int validsize_50000 = grl::getMarkerPose(logKUKAiiwaFusionTrackP, markerID_50000, timeEventM_FT, markerPose_FT); + // int validsize_50000 = grl::getMarkerPose(logKUKAiiwaFusionTrackP, markerID_50000, timeEventM_FT, markerPose_FT); + int validsize_50000 = grl::getMarkerMessage(logKUKAiiwaFusionTrackP, markerID_50000, timeEventM_FT, markerPose_FT); std::cout<<"validsize_50000: " << validsize_50000 << std::endl; grl::regularizeTimeEvent(timeEventM_FT, initial_local_time, initial_device_time_FT, FT_index); - grl::writeMatrixToCSV(FT_Marker50000_CSV, FT_Labels_Pose, timeEventM_FT, markerPose_FT, FT_index); + grl::writeMatrixToCSV(FT_Marker50000_CSV, grl::Transform_Labels, timeEventM_FT, markerPose_FT, FT_index); return 0; } From 1ca40383d8d2dad92f68d13fdf6fa8b6b5ee46d9 Mon Sep 17 00:00:00 2001 From: Chunting Date: Mon, 16 Apr 2018 17:08:11 -0400 Subject: [PATCH 095/102] Write a data processing log for csv files. --- .../grl/flatbuffer/ParseflatbuffertoCSV.hpp | 14 ++-- test/readFlatbufferTest.cpp | 79 ++++++++++++------- 2 files changed, 54 insertions(+), 39 deletions(-) diff --git a/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp b/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp index 960cdfa..ff1f313 100644 --- a/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp +++ b/include/grl/flatbuffer/ParseflatbuffertoCSV.hpp @@ -108,7 +108,6 @@ namespace grl { int col_Kuka_Joint = Joint_Labels.size(); const static int jointNum = 7; int col_Pose = PK_Pose_Labels.size(); - int col_trans = Transform_Labels.size(); /// Get CSV labels @@ -645,10 +644,9 @@ namespace grl { << "FT_X," << "FT_Y," << "FT_Z," - << "FT_QW" - << "FT_QX," - << "FT_QY," - << "FT_QZ," + << "FT_A" + << "FT_B," + << "FT_C," << "K_Joint1," << "K_Joint2," << "K_Joint3," @@ -680,8 +678,7 @@ namespace grl { << jointrow[3] << "," << jointrow[4] << "," << jointrow[5] << "," - << jointrow[6] - << std::endl; + << jointrow[6] << std::endl; kuka_index++; } else if( kuka_local_request_time(kuka_index) > FT_local_request_time(FT_index)) { auto matrixrow = markerPose.row(FT_index); @@ -697,8 +694,7 @@ namespace grl { << matrixrow[2] << "," << matrixrow[3] << "," << matrixrow[4] << "," - << matrixrow[5] << "," - << matrixrow[6] << std::endl; + << matrixrow[5] << std::endl; FT_index++; } else { // In case the time is extactly equivent with each other. diff --git a/test/readFlatbufferTest.cpp b/test/readFlatbufferTest.cpp index a717b6c..06895ce 100644 --- a/test/readFlatbufferTest.cpp +++ b/test/readFlatbufferTest.cpp @@ -27,7 +27,8 @@ /// Command of spliting a large file into some smaller ones. /// split -C 200m --numeric-suffixes 2018_02_28_16_39_13_FusionTrack.json 2018_02_28_16_39_13_FusionTrack /// The command to get the json file from flatbuffer binary file, these two files should be located in the same folder. -/// flatc -I . --json LogKUKAiiwaFusionTrack.fbs -- 2018_03_14_22_06_29_FusionTrack.flik +/// flatc -I . --json LogKUKAiiwaFusionTrack.fbs -- 2018_04_13_16_20_28_FusionTrack.flik +/// flatc -I . --json KUKAiiwa.fbs -- 2018_04_13_16_20_28_Kukaiiwa.iiwa int main(int argc, char* argv[]) { @@ -44,7 +45,7 @@ int main(int argc, char* argv[]) std::string homePath = std::getenv("HOME"); std::string vrepPath = homePath + "/src/V-REP_PRO_EDU_V3_4_0_Linux/data/"; std::string kukaBinaryfile = vrepPath + kukaTimeStamp; - std::cout << kukaBinaryfile << std::endl; + std::string fusiontrackBinaryfile = vrepPath + FTTimeStamp; std::string urdfFile = vrepPath + URDFModrl; std::string foldtimestamp = current_date_and_time_string(); // Write the generated files into a new fold @@ -79,6 +80,18 @@ int main(int argc, char* argv[]) std::string FTKUKA_TimeEvent_CSV = vrepPath + foldtimestamp + "/FTKUKA_TimeEvent.csv"; + std::string README_LOG = vrepPath + foldtimestamp + "/readme.txt"; + + auto readme = boost::filesystem::path(README_LOG); + if(boost::filesystem::exists(readme)){ + boost::filesystem::remove(readme); + std::cout << readme << " has been removed..." << std::endl; + } + boost::filesystem::ofstream ofs(readme, std::ios_base::app | std::ios_base::out); + ofs << "Read the binary files: \n"; + ofs << kukaBinaryfile << "\n" << fusiontrackBinaryfile<< "\n"; + + uint32_t markerID_22 = 22; uint32_t markerID_55 = 55; uint32_t markerID_50000 = 50000; @@ -89,8 +102,8 @@ int main(int argc, char* argv[]) std::size_t kuka_size = grl::getStateSize(kukaStatesP); /// Create matrix to contain data - Eigen::MatrixXd markerPose_FT(FT_size, grl::col_trans); - Eigen::MatrixXd markerTransform_FT = Eigen::MatrixXd::Zero(FT_size, grl::Transform_Labels.size()); + Eigen::MatrixXd markerPose_FT(FT_size, grl::col_Pose); + Eigen::MatrixXd markerTransform_FT = Eigen::MatrixXd::Zero(FT_size, grl::PK_Pose_Labels.size()); grl::MatrixXd timeEventM_FT = grl::MatrixXd::Zero(FT_size, grl::Time_Labels.size()); grl::MatrixXd timeEventM_Kuka = grl::MatrixXd::Zero(kuka_size, grl::Time_Labels.size()); Eigen::VectorXd sequenceCounterVec = Eigen::VectorXd::Zero(kuka_size); @@ -114,11 +127,11 @@ int main(int argc, char* argv[]) commandedTorque, externalTorque, jointStateInterpolated); - // int validsize_22 = grl::getMarkerPose(logKUKAiiwaFusionTrackP, markerID_22, timeEventM_FT, markerPose_FT); - int validsize_22 = grl::getMarkerMessage(logKUKAiiwaFusionTrackP, markerID_22, timeEventM_FT, markerPose_FT); + int validsize_22 = grl::getMarkerPose(logKUKAiiwaFusionTrackP, markerID_22, timeEventM_FT, markerPose_FT); + // int validsize_22 = grl::getMarkerMessage(logKUKAiiwaFusionTrackP, markerID_22, timeEventM_FT, markerPose_FT); - std::cout<< "------Kuka_size: "<< kuka_size << " FT_size: "<< FT_size << std::endl; - std::cout<<"validsize_22: " << validsize_22 << std::endl; + ofs << "------Kuka_size: " << kuka_size << " FT_size: "<< FT_size << std::endl; + ofs <<"validsize_22: " << validsize_22 << std::endl; std::size_t FT_time_size = timeEventM_FT.rows(); std::size_t kuka_time_size = timeEventM_Kuka.rows(); grl::MatrixXd timeEventM = grl::MatrixXd::Zero(FT_size, timeEventM_FT.cols()); @@ -129,17 +142,17 @@ int main(int argc, char* argv[]) // Skip the very beginning data,since the tracker starts to work once the scene is loaded in vrep. // But kuka starts to work only after clicking on the start button. // To combine the time from two devices, they should have the same starting time point. - while(kuka_index timeEventM_FT(FT_index,grl::timeBaseline_index) ){ - FT_index++; - } + // while(kuka_index timeEventM_FT(FT_index,grl::timeBaseline_index) ){ + // FT_index++; + // } auto initial_local_time = std::min(timeEventM_FT(FT_index,grl::timeBaseline_index), timeEventM_Kuka(kuka_index, grl::timeBaseline_index)); auto initial_device_time_kuka = timeEventM_Kuka(kuka_index,grl::TimeType::device_time); auto initial_device_time_FT = timeEventM_FT(FT_index,grl::TimeType::device_time); - std::cout<< "Main initial_local_time: " << initial_local_time << std::endl; + ofs<< "Main initial_local_time: " << initial_local_time << std::endl; timeEventM_FT.col(grl::TimeType::local_request_time) = timeEventM_FT.col(grl::TimeType::local_request_time) - initial_local_time * grl::VectorXd::Ones(FT_time_size); timeEventM_FT.col(grl::TimeType::local_receive_time) = timeEventM_FT.col(grl::TimeType::local_receive_time) - initial_local_time * grl::VectorXd::Ones(FT_time_size); timeEventM_FT.col(grl::TimeType::device_time) = timeEventM_FT.col(grl::TimeType::device_time) - initial_device_time_FT * grl::VectorXd::Ones(FT_time_size); @@ -182,7 +195,7 @@ int main(int argc, char* argv[]) std::size_t nrJoints = mb.nrJoints(); std::size_t nrBodies = strRobot.mb.nrBodies(); std::vector jointNames; - std::cout<<"Joint Size: "<< nrJoints << std::endl; + ofs<<"Joint Size: "<< nrJoints << std::endl; std::vector PK_Pose_Labels = grl::getLabels(grl::LabelsType::Kuka_Pose); @@ -205,34 +218,40 @@ int main(int argc, char* argv[]) //////////////////////////////////////////////////////////////////////////////////////////////////// // Get the pose of the marker 22. // Since the bad data is skipped, the timeEventM_FT also needs to be filled out with the orinial time stamp. - + std::vector FT_Labels_Pose = grl::getLabels(grl::LabelsType::FT_Pose); grl::writeFTKUKAMessageToCSV(FTKUKA_TimeEvent_CSV, timeEventM_FT, timeEventM_Kuka, measuredJointPosition, markerPose_FT, FT_index, kuka_index); grl::writeTimeEventToCSV(FT_TimeEvent_CSV, timeEventM_FT, FT_index); - grl::writeMatrixToCSV(FT_Marker22_CSV, grl::Transform_Labels, timeEventM_FT, markerPose_FT, FT_index); + + grl::writeMatrixToCSV(FT_Marker22_CSV, FT_Labels_Pose, timeEventM_FT, markerPose_FT, FT_index); // Change it back to the original size timeEventM_FT.resize(FT_size, grl::col_timeEvent); - markerPose_FT.resize(FT_size, grl::col_trans); + markerPose_FT.resize(FT_size, grl::col_Pose); timeEventM_FT = grl::MatrixXd::Zero(FT_size, grl::col_timeEvent); - markerPose_FT = Eigen::MatrixXd::Zero(FT_size, grl::col_trans); + markerPose_FT = Eigen::MatrixXd::Zero(FT_size, grl::col_Pose); // Get the pose of the bone marker - // int validsize_55 = grl::getMarkerPose(logKUKAiiwaFusionTrackP, markerID_55, timeEventM_FT, markerPose_FT); - int validsize_55 = grl::getMarkerMessage(logKUKAiiwaFusionTrackP, markerID_55, timeEventM_FT, markerPose_FT); - std::cout<<"validsize_55: " << validsize_55 << std::endl; + int validsize_55 = grl::getMarkerPose(logKUKAiiwaFusionTrackP, markerID_55, timeEventM_FT, markerPose_FT); + // int validsize_55 = grl::getMarkerMessage(logKUKAiiwaFusionTrackP, markerID_55, timeEventM_FT, markerPose_FT); + ofs<<"validsize_55: " << validsize_55 << std::endl; grl::regularizeTimeEvent(timeEventM_FT, initial_local_time, initial_device_time_FT, FT_index); - grl::writeMatrixToCSV(FT_Marker55_CSV, grl::Transform_Labels, timeEventM_FT, markerPose_FT, FT_index); + grl::writeMatrixToCSV(FT_Marker55_CSV, FT_Labels_Pose, timeEventM_FT, markerPose_FT, FT_index); // Change it back to the original size timeEventM_FT.resize(FT_size, grl::col_timeEvent); - markerPose_FT.resize(FT_size, grl::col_trans); + markerPose_FT.resize(FT_size, grl::col_Pose); timeEventM_FT = grl::MatrixXd::Zero(FT_size, grl::col_timeEvent); - markerPose_FT = Eigen::MatrixXd::Zero(FT_size, grl::col_trans); + markerPose_FT = Eigen::MatrixXd::Zero(FT_size, grl::col_Pose); // Get the pose of the bone marker - // int validsize_50000 = grl::getMarkerPose(logKUKAiiwaFusionTrackP, markerID_50000, timeEventM_FT, markerPose_FT); - int validsize_50000 = grl::getMarkerMessage(logKUKAiiwaFusionTrackP, markerID_50000, timeEventM_FT, markerPose_FT); + int validsize_50000 = grl::getMarkerPose(logKUKAiiwaFusionTrackP, markerID_50000, timeEventM_FT, markerPose_FT); + // int validsize_50000 = grl::getMarkerMessage(logKUKAiiwaFusionTrackP, markerID_50000, timeEventM_FT, markerPose_FT); - std::cout<<"validsize_50000: " << validsize_50000 << std::endl; + ofs<<"validsize_50000: " << validsize_50000 << std::endl; grl::regularizeTimeEvent(timeEventM_FT, initial_local_time, initial_device_time_FT, FT_index); - grl::writeMatrixToCSV(FT_Marker50000_CSV, grl::Transform_Labels, timeEventM_FT, markerPose_FT, FT_index); + grl::writeMatrixToCSV(FT_Marker50000_CSV, FT_Labels_Pose, timeEventM_FT, markerPose_FT, FT_index); + + ofs.close(); + + + return 0; } From 56009f36808a644ab0f25c8689efd891b0428edc Mon Sep 17 00:00:00 2001 From: Chunting Date: Wed, 18 Apr 2018 15:39:01 -0400 Subject: [PATCH 096/102] Change the csv labels. --- include/grl/flatbuffer/FlatbufferToEigen.hpp | 1 - .../grl/vrep/InverseKinematicsVrepPlugin.hpp | 45 ++++++++++++------- src/lua/robone.lua | 4 +- test/readFlatbufferTest.cpp | 13 +++++- 4 files changed, 41 insertions(+), 22 deletions(-) diff --git a/include/grl/flatbuffer/FlatbufferToEigen.hpp b/include/grl/flatbuffer/FlatbufferToEigen.hpp index 9dee7bb..ec49413 100644 --- a/include/grl/flatbuffer/FlatbufferToEigen.hpp +++ b/include/grl/flatbuffer/FlatbufferToEigen.hpp @@ -37,7 +37,6 @@ namespace grl { markerPose[4] = q.x(); markerPose[5] = q.y(); markerPose[6] = q.z(); - return markerPose; } diff --git a/include/grl/vrep/InverseKinematicsVrepPlugin.hpp b/include/grl/vrep/InverseKinematicsVrepPlugin.hpp index bf84dfc..0f44b00 100644 --- a/include/grl/vrep/InverseKinematicsVrepPlugin.hpp +++ b/include/grl/vrep/InverseKinematicsVrepPlugin.hpp @@ -404,6 +404,7 @@ class InverseKinematicsVrepPlugin rbd_mbs_.push_back(rbd_mbg_.makeMultiBody(ikGroupBaseName_,isFixed,X_base)); rbd_mbcs_.push_back(rbd::MultiBodyConfig(rbd_mbs_[0])); + // Set the multibody at zero configuration. rbd_mbcs_[0].zero(rbd_mbs_[0]); // we only have one robot for the moment so the index of it is 0 @@ -427,6 +428,23 @@ class InverseKinematicsVrepPlugin // set the preferred position to the initial position // https://github.com/jrl-umi3218/Tasks/blob/15aff94e3e03f6a161a87799ca2cf262b756bd0c/src/QPTasks.h#L426 rbd_preferred_mbcs_.push_back(simArmConfig); + + + // std::string homePath = std::getenv("HOME"); + // std::string vrepDataPath = homePath + "/src/V-REP_PRO_EDU_V3_4_0_Linux/data/"; + // std::string urdfPath = vrepDataPath + "Robone_KukaLBRiiwa.urdf"; + + + // auto strRobot = grl::getURDFModel(urdfPath); + // rbd::MultiBody mb = strRobot.mb; + // rbd::MultiBodyConfig mbc(mb); + // rbd::MultiBodyGraph mbg(strRobot.mbg); + // rbd_mbs_.push_back(mb); + // rbd_mbcs_.push_back(mbc); + // // Set the multibody at zero configuration. + // rbd_mbcs_[1].zero(rbd_mbs_[1]); + // std::size_t nrJoints = mb.nrJoints(); + // std::size_t nrBodies = strRobot.mb.nrBodies(); debugFrames(); @@ -470,6 +488,7 @@ class InverseKinematicsVrepPlugin if( dummy_world_frame ) { // visualize each joint position in world frame + std::cout << linkNames_[i] << std::endl; sva::PTransform plinkWorld = simArmConfig.bodyPosW[simArmMultiBody.bodyIndexByName(linkNames_[i])]; Eigen::Affine3d linkWorld = PTranformToEigenAffine(plinkWorld); std::string dummyName(("Dummy0"+ boost::lexical_cast(i+1))); @@ -478,6 +497,8 @@ class InverseKinematicsVrepPlugin setObjectTransform(currentDummy,-1,linkWorld); prevDummy=currentDummy; // Transformation from parent(i) to i in body coordinate (Xj*Xt). + std::cout << linkNames_[i] << std::endl; + std::cout << jointNames_[i] << std::endl; sva::PTransform plinkToSon = simArmConfig.parentToSon[simArmMultiBody.bodyIndexByName(linkNames_[i])]; Eigen::Affine3d linkToSon = PTranformToEigenAffine(plinkToSon); if(print) logger_->info("{} RBDyn ParentLinkToSon\n{}",dummyName, linkToSon.matrix()); @@ -566,9 +587,9 @@ void ForwardKinematicsFromCSV(int time_index, bool commanddata=false){ if(time_index == 0) { if(commanddata){ - ofs << "local_request_time_offset_X, C_K_X, C_K_Y, C_K_Z, C_K_A, C_K_B, C_K_C\n"; + ofs << "local_request_time_offset_X, Command_KUKA_X, Command_KUKA_Y, Command_KUKA_Z, Command_KUKA_A, Command_KUKA_B, Command_KUKA_C\n"; }else{ - ofs << "local_receive_time_offset_X, M_K_X, M_K_Y, M_K_Z, M_K_A, M_K_B, M_K_C\n"; + ofs << "local_receive_time_offset_X, Measured_KUKA_X, Measured_KUKA_Y, Measured_KUKA_Z, Measured_KUKA_A, Measured_KUKA_B, Measured_KUKA_C\n"; } } ofs << timeStamp << ", " << pose[0] << ", " << pose[1] << ", "<< pose[2] << ", "<< pose[3] << ", "<< pose[4] << ", "<< pose[5] << "\n"; @@ -620,21 +641,11 @@ void ForwardKinematicsFromCSV(int time_index, bool commanddata=false){ auto& simArmMultiBody = rbd_mbs_[simulatedRobotIndex]; auto& simArmConfig = rbd_mbcs_[simulatedRobotIndex]; - std::string homePath = std::getenv("HOME"); - std::string vrepDataPath = homePath + "/src/V-REP_PRO_EDU_V3_4_0_Linux/data/"; - std::string urdfPath = vrepDataPath + "Robone_KukaLBRiiwa.urdf"; - - - auto strRobot = grl::getURDFModel(urdfPath); - rbd::MultiBody mb = strRobot.mb; - rbd::MultiBodyConfig mbc(mb); - rbd::MultiBodyGraph mbg(strRobot.mbg); - std::size_t nrJoints = mb.nrJoints(); - std::size_t nrBodies = strRobot.mb.nrBodies(); + std::vector jointNames; std::vector vrepJointAngles; - double angle = 0.7; + double angle = 0; //0.7 for(auto i : jointHandles_) { angle *= -1; @@ -652,9 +663,9 @@ void ForwardKinematicsFromCSV(int time_index, bool commanddata=false){ rbd::forwardKinematics(simArmMultiBody, simArmConfig); rbd::forwardVelocity(simArmMultiBody, simArmConfig); - // SetRBDynArmFromVrep(jointNames_,jointHandles_,mb,mbc); - // rbd::forwardKinematics(mb, mbc); - // rbd::forwardVelocity(mb, mbc); + // SetRBDynArmFromVrep(jointNames_,jointHandles_,rbd_mbs_[1],rbd_mbcs_[1]); + // rbd::forwardKinematics(rbd_mbs_[1],rbd_mbcs_[1]); + // rbd::forwardVelocity(rbd_mbs_[1],rbd_mbcs_[1]); debugFrames(); } diff --git a/src/lua/robone.lua b/src/lua/robone.lua index 76c16d2..f2113df 100644 --- a/src/lua/robone.lua +++ b/src/lua/robone.lua @@ -269,7 +269,7 @@ robone.cutBoneScript=function() bone=simGetObjectHandle('FemurBone') table=simGetObjectHandle('highTable') - testStraightLine = true + testStraightLine = false CreatedPathHandle=simGetObjectHandle('MillHipCutPath') @@ -449,7 +449,7 @@ robone.startRealArmDriverScript=function() '30200' , -- LocalHostKukaKoniUDPPort, '192.170.10.2' , -- RemoteHostKukaKoniUDPAddress, '30200' , -- RemoteHostKukaKoniUDPPort - 'FRI' , -- KukaCommandMode (options are FRI, JAVA) + 'FRI' , -- KukaCommandMode (options are FRI, JAVA) 'FRI' , -- KukaMonitorMode (options are FRI, JAVA) "IK_Group1_iiwa" -- IKGroupName ) diff --git a/test/readFlatbufferTest.cpp b/test/readFlatbufferTest.cpp index 06895ce..91098ea 100644 --- a/test/readFlatbufferTest.cpp +++ b/test/readFlatbufferTest.cpp @@ -195,10 +195,19 @@ int main(int argc, char* argv[]) std::size_t nrJoints = mb.nrJoints(); std::size_t nrBodies = strRobot.mb.nrBodies(); std::vector jointNames; - ofs<<"Joint Size: "<< nrJoints << std::endl; - std::vector PK_Pose_Labels = grl::getLabels(grl::LabelsType::Kuka_Pose); + std::cout <<"Joint Size: "<< nrJoints << std::endl; + for(int jointIdx=0; jointIdx0){ + mbc.q[jointIdx][0] = 0; + } + } + rbd::forwardKinematics(mb, mbc); + + std::vector PK_Pose_Labels = grl::getLabels(grl::LabelsType::Kuka_Pose); // forward kinematic to get the of the end effector std::vector PoseEE = grl::getPoseEE(measuredJointPosition); // convert the sva::PTransformd to Plucker coordinate. From 6c8d27c980eda8547af7b79c60eda66c69b6a132 Mon Sep 17 00:00:00 2001 From: Chunting Date: Wed, 18 Apr 2018 17:30:16 -0400 Subject: [PATCH 097/102] Edit the instructions --- doc/howto/DataRecordingAndPlayback.rst | 42 ++++++++++++++++++++------ 1 file changed, 32 insertions(+), 10 deletions(-) diff --git a/doc/howto/DataRecordingAndPlayback.rst b/doc/howto/DataRecordingAndPlayback.rst index c2eb9de..b73b4b3 100644 --- a/doc/howto/DataRecordingAndPlayback.rst +++ b/doc/howto/DataRecordingAndPlayback.rst @@ -1,21 +1,43 @@ -========== +==================== Data Analysis -========== +==================== .. note:: I will interpret the data files, including the relative location and the main content. -Flatbuffer fbs file +How to collect data ================================== -'FlatBuffers ' __ is an efficient cross platform serialization library for C++, C#, C, Go, Java, JavaScript, TypeScript, PHP, and Python. -It was originally created at Google for game development and other performance-critical applications. +In this project, the messages are communicated by `FlatBuffers `__ between PC and Kuka workstation, which makes reading and writing data efficient. -In this project, the messages are communicated by flatbuffer between PC and Kuka workstation, which makes reading and writing data efficient. +1. Install `VREP `__ and `grl `__. -First install `VREP `__ and 'grl '__. +2. Start VREP and load the scene `RoboneSimulation_private.ttt `__. -The location of the 'fbs file '__. +3. Set flatbuffer limit in `robone.lua `__. + The hard limit for flatbuffer is 2 GB, but you can customize it based on your requirement in this project. + When flatbuffer size hits this limit, the data will be written to disk. Or when you click on the STOP button in VREP, the data will also be written to disk automaticly, regardless of the limit. + .. code-block:: bash + KUKA_single_buffer_limit_bytes = 256 -- MB + FB_single_buffer_limit_bytes = 1024 -- MB + +4. Start to record data while simulation is running in `robone.lua `__. + By defaut, program starts to collect data automatically. You can change it by following functions:: + .. code-block:: bash + simExtKukaLBRiiwaRecordWhileSimulationIsRunning(true, KUKA_single_buffer_limit_bytes) + simExtAtracsysFusionTrackRecordWhileSimulationIsRunning(true, FB_single_buffer_limit_bytes) + +5. Enable lua scripts StartRealArmDriverScript and Tracker in VREP, then program can communicate with Atracsys and Kuka. + Only after connecting the devices, it can start to record data correctly. + +6. Stop recording data. + There are two ways to stop the data collection process, one is the buffer hits the limit in Step 3, the other is to click on the STOP button in VREP, then it will stop and write the data to disk automatically. + + + + + +The location of the `fbs file `__. fbs files define the binary format we use for high performance data collection from devices, such as the Kuka LBR iiwa 14kg and Atracsys FusionTrack. The binary files are put in VREP data folder (i.e. ~/src/V-REP_PRO_EDU_V3_4_0_Linux/data/), @@ -32,7 +54,7 @@ When exporting the flatbuffer file to CSV, you need to follow the instructions b 1. Keep the binary files in the VREP data folder (i.e. ~/src/V-REP_PRO_EDU_V3_4_0_Linux/data/); -2. Run 'readFlatbufferTest '__ with the arguments of the name of binary file. +2. Run `readFlatbufferTest `__ with the arguments of the name of binary file. .. code-block:: bash ./readFlatbufferTest 2018_03_26_19_06_21_Kukaiiwa.iiwa 2018_03_26_19_06_21_FusionTrack.flik # You can also pass the arguments manually to the main() function in readFlatbufferTest.cpp. @@ -72,7 +94,7 @@ When exporting the flatbuffer file to CSV, you need to follow the instructions b Replay Process ================================== At this moment, the replay process is set and called in InverseKinematicsVrepPlug.cpp. -In future, It's better to creat a replay plugin to handl this process. +In future, It`s better to creat a replay plugin to handl this process. Before run it, you should copy the KUKA_Measured_Joint.csv and FT_Pose_Marker22.csv to the ~/src/V-REP_PRO_EDU_V3_4_0_Linux/data/data_in/. The result will be writen in ForwardKinematics_Pose.csv. From d026e4bddbbff1e61227a0ee035058840987bc06 Mon Sep 17 00:00:00 2001 From: Chunting Date: Thu, 19 Apr 2018 14:00:03 -0400 Subject: [PATCH 098/102] Edit the DataRecordingAndPlayback.rst --- doc/howto/DataRecordingAndPlayback.rst | 34 ++++++++++++++++---------- 1 file changed, 21 insertions(+), 13 deletions(-) diff --git a/doc/howto/DataRecordingAndPlayback.rst b/doc/howto/DataRecordingAndPlayback.rst index b73b4b3..12defbd 100644 --- a/doc/howto/DataRecordingAndPlayback.rst +++ b/doc/howto/DataRecordingAndPlayback.rst @@ -31,30 +31,25 @@ In this project, the messages are communicated by `FlatBuffers `__. -fbs files define the binary format we use for high performance data collection from devices, such as the Kuka LBR iiwa 14kg and Atracsys FusionTrack. - The binary files are put in VREP data folder (i.e. ~/src/V-REP_PRO_EDU_V3_4_0_Linux/data/), and are named by the time stamp (i.e. 2018_03_26_19_06_21_FusionTrack.flik, 2018_03_26_19_06_21_Kukaiiwa.iiwa). -The command to generate json file, the binary file and the fbs file should be put in the same folder: +The command to generate json file is as below, the binary file and the fbs file should be put in the same folder: flatc -I . --json LogKUKAiiwaFusionTrack.fbs -- 2018_03_26_19_06_21_FusionTrack.flik flatc -I . --json KUKAiiwa.fbs -- 2018_03_26_19_06_21_Kukaiiwa.iiwa -CSV file +How to export data ================================== When exporting the flatbuffer file to CSV, you need to follow the instructions below: 1. Keep the binary files in the VREP data folder (i.e. ~/src/V-REP_PRO_EDU_V3_4_0_Linux/data/); -2. Run `readFlatbufferTest `__ with the arguments of the name of binary file. +2. Run `readFlatbufferTest `__ in terminal with the arguments of the name of binary file. .. code-block:: bash ./readFlatbufferTest 2018_03_26_19_06_21_Kukaiiwa.iiwa 2018_03_26_19_06_21_FusionTrack.flik # You can also pass the arguments manually to the main() function in readFlatbufferTest.cpp. @@ -93,8 +88,21 @@ When exporting the flatbuffer file to CSV, you need to follow the instructions b Replay Process ================================== -At this moment, the replay process is set and called in InverseKinematicsVrepPlug.cpp. -In future, It`s better to creat a replay plugin to handl this process. +The replay process can perform the forward kinematics to get the cartesian pose of the end effector. -Before run it, you should copy the KUKA_Measured_Joint.csv and FT_Pose_Marker22.csv to the ~/src/V-REP_PRO_EDU_V3_4_0_Linux/data/data_in/. +1. Copy the KUKA_Measured_Joint.csv, KUKA_Command_Joint.csv and FT_Pose_Marker22.csv to the ~/src/V-REP_PRO_EDU_V3_4_0_Linux/data/data_in/. The result will be writen in ForwardKinematics_Pose.csv. + +2. Enable the CutBoneScript. + +3. Set the parameter of simExtGrlInverseKinematicsStart(...) to replay_mode in robone.lua. + You should run this function two times, one time commanddata is true, the other is false. Then you can get the cartesian pose for both command and measured data. + .. code-block:: bash + -- ik_mode, run real inverse kinematics algorith; + -- replay_mode, run the replay process; + -- otherwise, go to a test pose. + -- commanddata, only in replay_mode we need to set it to determine the joint data set. + commanddata = false + run_mode = { ik_mode = 1, replay_mode = 2, test_mode = 3} + print("Moving Robotiiwa arm along inversekinematics") + simExtGrlInverseKinematicsStart(run_mode.replay_mode, commanddata) \ No newline at end of file From a9bfa4e3c6bcec9a328026c510c0e56eea7e2075 Mon Sep 17 00:00:00 2001 From: Chunting Date: Thu, 19 Apr 2018 15:21:16 -0400 Subject: [PATCH 099/102] Edit the format --- doc/howto/DataRecordingAndPlayback.rst | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/doc/howto/DataRecordingAndPlayback.rst b/doc/howto/DataRecordingAndPlayback.rst index 12defbd..9603611 100644 --- a/doc/howto/DataRecordingAndPlayback.rst +++ b/doc/howto/DataRecordingAndPlayback.rst @@ -17,13 +17,12 @@ In this project, the messages are communicated by `FlatBuffers `__. The hard limit for flatbuffer is 2 GB, but you can customize it based on your requirement in this project. When flatbuffer size hits this limit, the data will be written to disk. Or when you click on the STOP button in VREP, the data will also be written to disk automaticly, regardless of the limit. - .. code-block:: bash + KUKA_single_buffer_limit_bytes = 256 -- MB FB_single_buffer_limit_bytes = 1024 -- MB 4. Start to record data while simulation is running in `robone.lua `__. By defaut, program starts to collect data automatically. You can change it by following functions:: - .. code-block:: bash simExtKukaLBRiiwaRecordWhileSimulationIsRunning(true, KUKA_single_buffer_limit_bytes) simExtAtracsysFusionTrackRecordWhileSimulationIsRunning(true, FB_single_buffer_limit_bytes) From e35a6859efc06bcd03832b01ec8ff6120eee7a2c Mon Sep 17 00:00:00 2001 From: Chunting Date: Thu, 19 Apr 2018 16:16:16 -0400 Subject: [PATCH 100/102] Edit the format --- doc/howto/DataRecordingAndPlayback.rst | 31 ++++++++++++++------------ 1 file changed, 17 insertions(+), 14 deletions(-) diff --git a/doc/howto/DataRecordingAndPlayback.rst b/doc/howto/DataRecordingAndPlayback.rst index 9603611..a390be3 100644 --- a/doc/howto/DataRecordingAndPlayback.rst +++ b/doc/howto/DataRecordingAndPlayback.rst @@ -14,9 +14,10 @@ In this project, the messages are communicated by `FlatBuffers `__. -3. Set flatbuffer limit in `robone.lua `__. +3. Set flatbuffer limit in `robone.lua `__. The hard limit for flatbuffer is 2 GB, but you can customize it based on your requirement in this project. - When flatbuffer size hits this limit, the data will be written to disk. Or when you click on the STOP button in VREP, the data will also be written to disk automaticly, regardless of the limit. + When flatbuffer size hits this limit, the data will be written to disk. Or when you click on the STOP button in VREP, + the data will also be written to disk automaticly, regardless of the limit:: KUKA_single_buffer_limit_bytes = 256 -- MB FB_single_buffer_limit_bytes = 1024 -- MB @@ -37,9 +38,9 @@ In this project, the messages are communicated by `FlatBuffers `__ in terminal with the arguments of the name of binary file. - .. code-block:: bash +2. Run `readFlatbufferTest `__ in terminal with the arguments of the name of binary file:: + ./readFlatbufferTest 2018_03_26_19_06_21_Kukaiiwa.iiwa 2018_03_26_19_06_21_FusionTrack.flik - # You can also pass the arguments manually to the main() function in readFlatbufferTest.cpp. + # You can also pass the arguments manually to the main() function in readFlatbufferTest.cpp. + 3. The generated csv files will be put into the VREP data folder in which the subfolder is named by time stamp. Label explanation: .. code-block:: bash @@ -61,7 +63,7 @@ When exporting the flatbuffer file to CSV, you need to follow the instructions b time_Y: time driftting, device_time_offset - local_request_time_offset; counter: the identifier of message, defined by devices; X Y Z A B C: the cartesian postion and oritation in Plucker coordinate system; - M_Pos_Ji: measured joint position of joint i from kuka; + M_Pos_J*: measured joint position of joint i from kuka; C_Pos_J*: command joint position of joint i to kuka; M_Tor_J*: measured joint torque of joint i form kuka; C_Tor_J*: comand joint torque of joint i to kuka; @@ -73,6 +75,7 @@ When exporting the flatbuffer file to CSV, you need to follow the instructions b local_request_offset = (local_request_time - initial_local_request_time) device_offset = (device_time - initial_device_time) time_Y = device_offset - local_request_offset + CSV file explanation: FTKUKA_TimeEvent.csv has the information from both Atracsys and kuka. It can help analysis time event. FT_Pose_Marker*.csv have the time event and pose (in Plucker coordinate) of the specific marker in Atracsys space; @@ -95,12 +98,12 @@ The result will be writen in ForwardKinematics_Pose.csv. 2. Enable the CutBoneScript. 3. Set the parameter of simExtGrlInverseKinematicsStart(...) to replay_mode in robone.lua. - You should run this function two times, one time commanddata is true, the other is false. Then you can get the cartesian pose for both command and measured data. - .. code-block:: bash - -- ik_mode, run real inverse kinematics algorith; + You should run this function two times, one time commanddata is true, the other is false. Then you can get the cartesian pose for both command and measured data:: + + -- ik_mode, run real inverse kinematics algorith; -- replay_mode, run the replay process; - -- otherwise, go to a test pose. - -- commanddata, only in replay_mode we need to set it to determine the joint data set. + -- test_mode, go to a test pose; + -- commanddata, only in replay_mode we need to set it to select the joint data set. commanddata = false run_mode = { ik_mode = 1, replay_mode = 2, test_mode = 3} print("Moving Robotiiwa arm along inversekinematics") From 5c8459f89ab3908c2b3f660e87ad266a4e1ca1e1 Mon Sep 17 00:00:00 2001 From: Chunting Date: Mon, 23 Apr 2018 16:57:47 -0400 Subject: [PATCH 101/102] FRI set KUKA FRI KONI send period to 1ms, configurable on ProcessDataqManager --- src/java/grl/src/RoboticsAPI.data.xml | 83 ++++++++++---------- src/java/grl/src/grl/ProcessDataManager.java | 23 ++++-- src/java/grl/src/grl/driver/GRL_Driver.java | 52 ++++++------ 3 files changed, 86 insertions(+), 72 deletions(-) mode change 100644 => 100755 src/java/grl/src/grl/driver/GRL_Driver.java diff --git a/src/java/grl/src/RoboticsAPI.data.xml b/src/java/grl/src/RoboticsAPI.data.xml index 2e70eb4..a5b2d9c 100755 --- a/src/java/grl/src/RoboticsAPI.data.xml +++ b/src/java/grl/src/RoboticsAPI.data.xml @@ -1,41 +1,42 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/java/grl/src/grl/ProcessDataManager.java b/src/java/grl/src/grl/ProcessDataManager.java index 9c0aaba..7a0506f 100755 --- a/src/java/grl/src/grl/ProcessDataManager.java +++ b/src/java/grl/src/grl/ProcessDataManager.java @@ -21,6 +21,7 @@ public class ProcessDataManager { private String _FRI_KONI_LaptopIPAddress; private String _ROS_MASTER_URI; private String _ZMQ_MASTER_URI; + private int _FRI_KONI_SendPeriodMilliseconds; private double _jointVelRel; private double _jointAccelRel; @@ -75,11 +76,6 @@ private void update_ZMQ_MASTER_URI() { this._ZMQ_MASTER_URI = "tcp://" + _controllingLaptopIPAddress + ":" + _controllingLaptopZMQPort; } - - public String get_ROS_MASTER_URI() { - return _ROS_MASTER_URI; - } - private void update_ROS_MASTER_URI() { this._ROS_MASTER_URI = "http://" + _controllingLaptopIPAddress + ":" + _controllingLaptop_ROS_MASTER_URI_Port; } @@ -98,6 +94,11 @@ public String get_controllingLaptopJAVAPort() { return _controllingLaptopZMQPort; } + + public int get_FRI_KONI_SendPeriodMilliseconds() { + return _FRI_KONI_SendPeriodMilliseconds; + } + public void set_controllingLaptopJAVAPort(String _controllingLaptopJAVAPort) { this._controllingLaptopZMQPort = _controllingLaptopJAVAPort; update_ZMQ_MASTER_URI(); @@ -120,9 +121,19 @@ public void set_RobotIPAddress(String _RobotIPAddress) { this._RobotIPAddress = _RobotIPAddress; } + + public void set_FRI_KONI_SendPeriodMilliseconds(int FRI_KONI_SendPeriodMilliseconds) { + this._FRI_KONI_SendPeriodMilliseconds = FRI_KONI_SendPeriodMilliseconds; + } + public String get_FRI_KONI_RobotIPAddress() { return _FRI_KONI_RobotIPAddress; } + + public String get_ROS_MASTER_URI() { + return _ROS_MASTER_URI; + } + public void set_FRI_KONI_RobotIPAddress(String _FRI_KONI_RobotIPAddress) { this._FRI_KONI_RobotIPAddress = _FRI_KONI_RobotIPAddress; @@ -221,6 +232,8 @@ public ProcessDataManager(final RoboticsAPIApplication app){ // ********************************************************************** _FRI_KONI_RobotIPAddress = _app.getApplicationData().getProcessData("Robot_KONI_FRI_IP").getValue(); //"tcp://172.31.1.100:30010"; + _FRI_KONI_SendPeriodMilliseconds = _app.getApplicationData().getProcessData("FRI_KONI_SendPeriodMilliseconds").getValue(); + _jointVelRel = _app.getApplicationData().getProcessData("JointVelRel").getValue(); _jointAccelRel = _app.getApplicationData().getProcessData("JointAccelRel").getValue(); diff --git a/src/java/grl/src/grl/driver/GRL_Driver.java b/src/java/grl/src/grl/driver/GRL_Driver.java old mode 100644 new mode 100755 index 104752c..83263f4 --- a/src/java/grl/src/grl/driver/GRL_Driver.java +++ b/src/java/grl/src/grl/driver/GRL_Driver.java @@ -166,7 +166,7 @@ public class GRL_Driver extends RoboticsAPIApplication // when we receive a message int message_counter = 0; int message_counter_since_last_mode_change = 0; - + private Lock configureSmartServoLock = new ReentrantLock(); // private FlexFellowIOGroup _flexFellowIOGroup; @@ -228,7 +228,7 @@ public void initialize() _teachModeThread.start(); - int millisecondsPerFRITimeStep = 4; + int millisecondsPerFRITimeStep = _processDataManager.get_FRI_KONI_SendPeriodMilliseconds(); _FRIModeRunnable = new FRIMode( _lbr, _processDataManager.get_FRI_KONI_LaptopIPAddress(), millisecondsPerFRITimeStep); @@ -262,9 +262,9 @@ public void run() while (!stop && !_startStopUI.is_stopped() && _lbr.getSafetyState().getEmergencyStopInt()==EmergencyStop.INACTIVE) { message_counter+=1; _currentKUKAiiwaState = udpMan.waitForNextMessage(); - + //Some bug in this, just set _previousKUKAiiwaState = _currentKUKAiiwaState before looping/continue? - _previousKUKAiiwaState = udpMan.getPrevMessage(); + _previousKUKAiiwaState = udpMan.getPrevMessage(); ////////////////////////////////////////// @@ -419,13 +419,13 @@ else if (_currentKUKAiiwaState.armControlState().stateType() == grl.flatbuffer.A try{ if(_smartServoMotion == null){ // Initialize Smart servo the first time - getLogger().info("Initializing Smart Servo in " + getLogger().info("Initializing Smart Servo in " + grl.flatbuffer.EControlMode.name(_currentKUKAiiwaState.armConfiguration().controlMode())); switchSmartServoMotion(_currentKUKAiiwaState.armConfiguration().controlMode()); continue; }else if(_currentKUKAiiwaState.setArmConfiguration()){ //If change in mode requested - - //TODO: bug, _previousKUKAiiwaState & _currentKUKAiiwaState are same. + + //TODO: bug, _previousKUKAiiwaState & _currentKUKAiiwaState are same. /* getLogger().info("Change controlMode requested: " +grl.flatbuffer.EControlMode.name(_previousKUKAiiwaState.armConfiguration().controlMode()) +" -> "+grl.flatbuffer.EControlMode.name(_currentKUKAiiwaState.armConfiguration().controlMode()));*/ @@ -434,7 +434,7 @@ else if (_currentKUKAiiwaState.armControlState().stateType() == grl.flatbuffer.A } catch (Exception e) { - getLogger().error(e.getMessage()); + getLogger().error(e.getMessage()); getLogger().error("Exception in Starting/Switching SmartServo" ); continue; } @@ -501,7 +501,7 @@ else if (_currentKUKAiiwaState.armControlState().stateType() == grl.flatbuffer.A } else { getLogger().error("Couldn't issue motion command, smartServo motion was most likely reset. retrying..."); } - + } else if (_currentKUKAiiwaState.armControlState().stateType() == grl.flatbuffer.ArmState.StopArm) { _smartServoRuntime.stopMotion(); @@ -528,7 +528,7 @@ else if (_currentKUKAiiwaState.armControlState().stateType() == grl.flatbuffer.A if (currentMotion != null) currentMotion.cancel(); if(!cancelTeachMode()) continue; if(!cancelSmartServo()) continue; - + if(message_counter_since_last_mode_change % 500 == 0) getLogger().info("StartArm mode active, connection established!\nHolding Position while waiting for mode change...\n"); PositionControlMode controlMode = new PositionControlMode(); @@ -540,7 +540,7 @@ else if (_currentKUKAiiwaState.armControlState().stateType() == grl.flatbuffer.A System.out.println("Unsupported Mode! stopping"); stop = true; } - + /////////////////////////////////////////////////////////////////////////// /// Sending commands back to the C++ interface here @@ -590,7 +590,7 @@ else if (_currentKUKAiiwaState.armControlState().stateType() == grl.flatbuffer.A } } - + {_previousKUKAiiwaState = _currentKUKAiiwaState;} } // end primary while loop @@ -721,7 +721,7 @@ private boolean isSameControlMode(IMotionControlMode kukacm, byte controlMode) { return roscmname.equals(kukacmname); } - + /** * Initialize the appropriate control mode based on passed parameters * @@ -735,7 +735,7 @@ public AbstractMotionControlMode getMotionControlMode(byte controlMode, boolean if(controlMode == grl.flatbuffer.EControlMode.POSITION_CONTROL_MODE){ mcm = new PositionControlMode(); } else if(controlMode==grl.flatbuffer.EControlMode.CART_IMP_CONTROL_MODE){ - + if(defaultParams){ // TODO: Get missing default params from processData /// @note setMaxCartesianVelocity STOPS THE ROBOT ABOVE THAT VELOCITY RATHER THAN CAPPING THE VELOCITY @@ -745,14 +745,14 @@ public AbstractMotionControlMode getMotionControlMode(byte controlMode, boolean cicm.setNullSpaceDamping(0.5); cicm.setNullSpaceStiffness(2); cicm.setMaxControlForce(200, 200, 200, 200, 200, 200, true); - + cicm.parametrize(CartDOF.X).setStiffness(_processDataManager.get_CartesianImpedenceStiffnessX()); cicm.parametrize(CartDOF.Y).setStiffness(_processDataManager.get_CartesianImpedenceStiffnessY()); cicm.parametrize(CartDOF.Z).setStiffness(_processDataManager.get_CartesianImpedenceStiffnessZ()); cicm.parametrize(CartDOF.A).setStiffness(_processDataManager.get_CartesianImpedenceStiffnessA()); cicm.parametrize(CartDOF.B).setStiffness(_processDataManager.get_CartesianImpedenceStiffnessB()); cicm.parametrize(CartDOF.C).setStiffness(_processDataManager.get_CartesianImpedenceStiffnessC()); - + cicm.parametrize(CartDOF.X).setDamping(_processDataManager.get_CartesianImpedenceDampingX()); cicm.parametrize(CartDOF.Y).setDamping(_processDataManager.get_CartesianImpedenceDampingY()); cicm.parametrize(CartDOF.Z).setDamping(_processDataManager.get_CartesianImpedenceDampingZ()); @@ -762,9 +762,9 @@ public AbstractMotionControlMode getMotionControlMode(byte controlMode, boolean mcm = cicm; } else{ - + CartesianImpedanceControlMode cicm = new CartesianImpedanceControlMode(); - + cicm.setMaxCartesianVelocity(_currentKUKAiiwaState.armConfiguration().setCartImpedance().maxCartesianVelocity().position().x(), _currentKUKAiiwaState.armConfiguration().setCartImpedance().maxCartesianVelocity().position().y(), _currentKUKAiiwaState.armConfiguration().setCartImpedance().maxCartesianVelocity().position().z(), @@ -785,14 +785,14 @@ public AbstractMotionControlMode getMotionControlMode(byte controlMode, boolean _currentKUKAiiwaState.armConfiguration().setCartImpedance().maxControlForce().rotation().r3(), _currentKUKAiiwaState.armConfiguration().setCartImpedance().maxControlForce().rotation().r2(), _currentKUKAiiwaState.armConfiguration().setCartImpedance().maxControlForce().rotation().r1(), true); - + cicm.parametrize(CartDOF.X).setStiffness(_currentKUKAiiwaState.armConfiguration().setCartImpedance().stiffness().position().x()); cicm.parametrize(CartDOF.Y).setStiffness(_currentKUKAiiwaState.armConfiguration().setCartImpedance().stiffness().position().y()); cicm.parametrize(CartDOF.Z).setStiffness(_currentKUKAiiwaState.armConfiguration().setCartImpedance().stiffness().position().z()); cicm.parametrize(CartDOF.A).setStiffness(_currentKUKAiiwaState.armConfiguration().setCartImpedance().stiffness().rotation().r3()); cicm.parametrize(CartDOF.B).setStiffness(_currentKUKAiiwaState.armConfiguration().setCartImpedance().stiffness().rotation().r2()); cicm.parametrize(CartDOF.C).setStiffness(_currentKUKAiiwaState.armConfiguration().setCartImpedance().stiffness().rotation().r1()); - + cicm.parametrize(CartDOF.X).setDamping(_currentKUKAiiwaState.armConfiguration().setCartImpedance().damping().position().x()); cicm.parametrize(CartDOF.Y).setDamping(_currentKUKAiiwaState.armConfiguration().setCartImpedance().damping().position().y()); cicm.parametrize(CartDOF.Z).setDamping(_currentKUKAiiwaState.armConfiguration().setCartImpedance().damping().position().z()); @@ -811,12 +811,12 @@ public AbstractMotionControlMode getMotionControlMode(byte controlMode, boolean //cicm.parametrize(CartDOF.X).setStiffness(stiffnessX); //cicm.parametrize(CartDOF.Y).setStiffness(stiffnessY); //cicm.parametrize(CartDOF.Z).setStiffness(stiffnessZ); - + mcm = jicm; } else{ JointImpedanceControlMode jicm = new JointImpedanceControlMode(_lbr.getJointCount()); - + if(_lbr.getJointCount() != _currentKUKAiiwaState.armConfiguration().setJointImpedance().stiffnessLength()) { getLogger().info("stiffness/damping vector size is not correct. Set default values for now"); @@ -832,7 +832,7 @@ public AbstractMotionControlMode getMotionControlMode(byte controlMode, boolean damping[k] = _currentKUKAiiwaState.armConfiguration().setJointImpedance().damping(k); } jicm.setStiffness(stiffness); - jicm.setDamping(damping); + jicm.setDamping(damping); } mcm = jicm; } @@ -867,7 +867,7 @@ private void switchSmartServoMotion(byte controlMode) { if(_smartServoMotion!=null){ if(isSameControlMode(_smartServoMotion.getMode(), controlMode)) { // We can just change the parameters if the control strategy is the same. if (!(_smartServoMotion.getMode() instanceof PositionControlMode)) { // We are in PositioControlMode and the request was for the same mode, there are no parameters to change. - getLogger().info("Changing parameters of Smart Servo in " + getLogger().info("Changing parameters of Smart Servo in " + grl.flatbuffer.EControlMode.name(controlMode)); // With the changeControlModeSettings(...) method, it is possible to change the controller parameters with which the // servo motion was started. The control type itself cannot be changed during the servo motion. @@ -875,7 +875,7 @@ private void switchSmartServoMotion(byte controlMode) { configureSmartServoLock.unlock(); return; } - } + } getLogger().info("switching controlMode requested: " + _smartServoMotion.getMode().getClass().getSimpleName() +" -> "+grl.flatbuffer.EControlMode.name(controlMode)); @@ -947,7 +947,7 @@ private void switchSmartServoMotion(byte controlMode) { configureSmartServoLock.unlock(); return; } - + configureSmartServoLock.unlock(); } From b978292264db3862d7a9ece03ab4bf581345b58e Mon Sep 17 00:00:00 2001 From: Chunting Date: Mon, 30 Apr 2018 12:47:30 -0400 Subject: [PATCH 102/102] Remove some std::cout --- include/grl/vrep/InverseKinematicsVrepPlugin.hpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/include/grl/vrep/InverseKinematicsVrepPlugin.hpp b/include/grl/vrep/InverseKinematicsVrepPlugin.hpp index 0f44b00..3510071 100644 --- a/include/grl/vrep/InverseKinematicsVrepPlugin.hpp +++ b/include/grl/vrep/InverseKinematicsVrepPlugin.hpp @@ -488,7 +488,6 @@ class InverseKinematicsVrepPlugin if( dummy_world_frame ) { // visualize each joint position in world frame - std::cout << linkNames_[i] << std::endl; sva::PTransform plinkWorld = simArmConfig.bodyPosW[simArmMultiBody.bodyIndexByName(linkNames_[i])]; Eigen::Affine3d linkWorld = PTranformToEigenAffine(plinkWorld); std::string dummyName(("Dummy0"+ boost::lexical_cast(i+1))); @@ -497,8 +496,6 @@ class InverseKinematicsVrepPlugin setObjectTransform(currentDummy,-1,linkWorld); prevDummy=currentDummy; // Transformation from parent(i) to i in body coordinate (Xj*Xt). - std::cout << linkNames_[i] << std::endl; - std::cout << jointNames_[i] << std::endl; sva::PTransform plinkToSon = simArmConfig.parentToSon[simArmMultiBody.bodyIndexByName(linkNames_[i])]; Eigen::Affine3d linkToSon = PTranformToEigenAffine(plinkToSon); if(print) logger_->info("{} RBDyn ParentLinkToSon\n{}",dummyName, linkToSon.matrix()); @@ -530,7 +527,6 @@ void ForwardKinematicsFromCSV(int time_index, bool commanddata=false){ auto& simArmMultiBody = rbd_mbs_[simulatedRobotIndex]; auto& simArmConfig = rbd_mbcs_[simulatedRobotIndex]; Eigen::VectorXf joint_angles = Eigen::VectorXf::Zero(7); - // std::cout << "\nBefore: " << joint_angles << std::endl; std::string homePath = std::getenv("HOME"); std::string vrepDataPath = homePath + "/src/V-REP_PRO_EDU_V3_4_0_Linux/data/"; std::string data_inPath = vrepDataPath + "data_in"; @@ -556,9 +552,7 @@ void ForwardKinematicsFromCSV(int time_index, bool commanddata=false){ angle = joint_angles[jointIdx++]; simSetJointPosition(i,angle); vrepJointAngles.push_back(angle); - // std::cout << angle << " "; } - // std::cout << std::endl; // Set RBDyn joint angles from vrep joint angles. SetRBDynArmFromVrep(jointNames_,jointHandles_,simArmMultiBody,simArmConfig);