Skip to content
Snippets Groups Projects
Commit 991c106c authored by Christoph Pohl's avatar Christoph Pohl Committed by ARMAR-DE
Browse files

Improve Force-Torque querying for RobotReader

Signed-off-by: ARMAR-DE <>
parent dd73ed6d
No related branches found
No related tags found
No related merge requests found
......@@ -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;
......
......@@ -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);
}
......
......@@ -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
......@@ -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
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment