diff --git a/source/RobotAPI/libraries/armem_robot/types.h b/source/RobotAPI/libraries/armem_robot/types.h index 2bc45c144b53e3900e8c7041c54d33ee0d205f56..2eeff803f8bdd243aeb275c004c6d08b0f31e701 100644 --- a/source/RobotAPI/libraries/armem_robot/types.h +++ b/source/RobotAPI/libraries/armem_robot/types.h @@ -37,7 +37,9 @@ namespace armarx::armem::robot struct ForceTorque { Eigen::Vector3f force; + Eigen::Vector3f gravityCompensatedForce; Eigen::Vector3f torque; + Eigen::Vector3f gravityCompensatedTorque; }; using ToFArray = Eigen::MatrixXf; diff --git a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp index 3749c892d5e46e6ce0f7a848a86e8059ed70991c..901d98dcb8f649b0968694f2eefc13888bde91cb 100644 --- a/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp +++ b/source/RobotAPI/libraries/armem_robot_state/aron_conversions.cpp @@ -88,14 +88,18 @@ namespace armarx::armem fromAron(const armarx::armem::prop::arondto::ForceTorque& dto, robot::ForceTorque& bo) { aron::fromAron(dto.force, bo.force); + aron::fromAron(dto.gravityCompensationForce, bo.gravityCompensatedForce); aron::fromAron(dto.torque, bo.torque); + aron::fromAron(dto.gravityCompensationTorque, bo.gravityCompensatedTorque); } void toAron(armarx::armem::prop::arondto::ForceTorque& dto, const robot::ForceTorque& bo) { aron::toAron(dto.force, bo.force); + aron::toAron(dto.gravityCompensationForce, bo.gravityCompensatedForce); aron::toAron(dto.torque, bo.torque); + aron::toAron(dto.gravityCompensationTorque, bo.gravityCompensatedTorque); } diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp index 76fa10a4c0186acecdb969ec69af75d97975708f..416585111bfbd96cca816211285faf9ea3715792 100644 --- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp +++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.cpp @@ -117,5 +117,16 @@ namespace armarx::armem::robot_state return nullptr; } + std::optional<std::map<RobotReader::Hand, robot::ForceTorque>> + VirtualRobotReader::queryForceTorque(const std::string& name, const Time& timestamp) + { + const auto description = queryDescription(name, timestamp); + if (not description.has_value()) + { + return std::nullopt; + } + return RobotReader::queryForceTorque(description.value(), timestamp); + } + } // namespace armarx::armem::robot_state diff --git a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h index 3de8fbe8b49c453a79a7cafbcf7904713f4d756f..a80d9b02871c65998c75c3ee5522ef4f71af9a06 100644 --- a/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h +++ b/source/RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h @@ -68,6 +68,9 @@ namespace armarx::armem::robot_state VirtualRobot::RobotIO::RobotDescription::eStructure, bool blocking = true); + std::optional<std::map<RobotReader::Hand, robot::ForceTorque>> + queryForceTorque(const std::string& name, const armem::Time& timestamp); + using RobotReader::queryForceTorque; private: [[nodiscard]] VirtualRobot::RobotPtr