Skip to content
Snippets Groups Projects
Commit d13e8751 authored by Fabian Reister's avatar Fabian Reister
Browse files

aron conversions

parent b47f6236
No related branches found
No related tags found
1 merge request!12Feature/open articulated objects
Hide whitespace changes
Inline Side-by-side
source/armarx/manipulation/core/aron_conversions.cpp
+ 169
− 82
View file @ d13e8751
......@@ -20,96 +20,111 @@
* GNU General Public License
*/
#include <RobotAPI/libraries/armem_robot/types.h>
#include "aron_conversions.h"
#include "ArmarXCore/core/logging/Logging.h"
#include <RobotAPI/libraries/armem_robot/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot/types.h>
#include <RobotAPI/libraries/aron/common/aron_conversions.h>
#include <armarx/manipulation/core/aron/Affordance.aron.generated.h>
#include <armarx/manipulation/core/aron/ActionHypothesis.aron.generated.h>
#include <armarx/manipulation/core/aron/ExecutableAction.aron.generated.h>
#include <armarx/manipulation/core/aron/ExecutedAction.aron.generated.h>
#include <armarx/manipulation/core/aron/FramedUncertainPose.aron.generated.h>
#include <armarx/manipulation/core/aron/HandTrajectory.aron.generated.h>
#include <armarx/manipulation/core/aron/SituatedAffordance.aron.generated.h>
#include <armarx/manipulation/core/aron/Arm.aron.generated.h>
#include <armarx/manipulation/core/aron/Hand.aron.generated.h>
#include <armarx/manipulation/core/aron/ManipulationProcess.aron.generated.h>
#include <armarx/manipulation/core/Affordance.h>
#include <armarx/manipulation/core/ActionHypothesis.h>
#include <armarx/manipulation/core/Affordance.h>
#include <armarx/manipulation/core/AffordanceTypes.h>
#include <armarx/manipulation/core/ExecutableAction.h>
#include <armarx/manipulation/core/ExecutedAction.h>
#include <armarx/manipulation/core/FramedUncertainPose.h>
#include <armarx/manipulation/core/HandTrajectory.h>
#include <armarx/manipulation/core/SituatedAffordance.h>
#include <armarx/manipulation/core/ManipulationProcess.h>
#include <armarx/manipulation/core/SituatedAffordance.h>
#include <armarx/manipulation/core/arm.h>
#include <armarx/manipulation/core/aron/ActionHypothesis.aron.generated.h>
#include <armarx/manipulation/core/aron/Affordance.aron.generated.h>
#include <armarx/manipulation/core/aron/Arm.aron.generated.h>
#include <armarx/manipulation/core/aron/ExecutableAction.aron.generated.h>
#include <armarx/manipulation/core/aron/ExecutedAction.aron.generated.h>
#include <armarx/manipulation/core/aron/FramedUncertainPose.aron.generated.h>
#include <armarx/manipulation/core/aron/Hand.aron.generated.h>
#include <armarx/manipulation/core/aron/HandTrajectory.aron.generated.h>
#include <armarx/manipulation/core/aron/ManipulationProcess.aron.generated.h>
#include <armarx/manipulation/core/aron/SituatedAffordance.aron.generated.h>
#include <armarx/manipulation/meta/transform_container.h>
#include "ArmarXCore/core/logging/Logging.h"
#include "aron_conversions.h"
namespace armarx::manipulation::core
{
// namespace aron_helpers
// {
armarx::FramedPose toFramedPose(const arondto::FramedUncertainPose& pose)
armarx::FramedPose
toFramedPose(const arondto::FramedUncertainPose& pose)
{
// TODO: should agent be reference frame. In FramedPose agent is not used for anything (as far as I can tell)
return {pose.uncertainPose.pose, pose.header.frame, pose.header.agent};
}
armarx::FramedPose toFramedPose(const FramedUncertainPose& pose)
armarx::FramedPose
toFramedPose(const FramedUncertainPose& pose)
{
return toFramedPose(toAron(pose));
}
arondto::Affordance toAron(const Affordance& bo)
arondto::Affordance
toAron(const Affordance& bo)
{
arondto::Affordance aff;
toAron(aff, bo);
return aff;
}
Affordance fromAron(const arondto::Affordance& dto)
Affordance
fromAron(const arondto::Affordance& dto)
{
armem::robot::RobotDescription description;
armem::robot::fromAron(dto.robotDescription, description);
return {dto.actionType, description};
}
arondto::FramedUncertainPose toAron(const FramedUncertainPose& bo)
arondto::FramedUncertainPose
toAron(const FramedUncertainPose& bo)
{
arondto::FramedUncertainPose pose;
toAron(pose, bo);
return pose;
}
FramedUncertainPose fromAron(const arondto::FramedUncertainPose& dto)
FramedUncertainPose
fromAron(const arondto::FramedUncertainPose& dto)
{
return {toFramedPose(dto), dto.uncertainPose.covariance};
}
arondto::ActionHypothesis toAron(const ActionHypothesis& bo)
arondto::ActionHypothesis
toAron(const ActionHypothesis& bo)
{
arondto::ActionHypothesis dto;
toAron(dto, bo);
return dto;
}
ActionHypothesis fromAron(const arondto::ActionHypothesis& dto)
ActionHypothesis
fromAron(const arondto::ActionHypothesis& dto)
{
return {fromAron(dto.affordance), fromAron(dto.framedPose), dto.observationTime, dto.handedness,
return {fromAron(dto.affordance),
fromAron(dto.framedPose),
dto.observationTime,
dto.handedness,
dto.existenceCertainty};
}
arondto::ExecutableAction toAron(const ExecutableAction& bo)
arondto::ExecutableAction
toAron(const ExecutableAction& bo)
{
arondto::ExecutableAction dto;
toAron(dto, bo);
return dto;
}
ExecutableAction fromAron(const arondto::ExecutableAction& dto)
ExecutableAction
fromAron(const arondto::ExecutableAction& dto)
{
std::optional<armarx::FramedPose> platformPose;
......@@ -122,7 +137,7 @@ namespace armarx::manipulation::core
}
ARMARX_CHECK_NOT_EMPTY(dto.unimanual);
std::vector<ExecutableAction::Unimanual> unimanuals;
for (const auto& um: dto.unimanual)
for (const auto& um : dto.unimanual)
{
std::optional<armarx::FramedPose> execPose;
std::optional<armarx::FramedPose> prePose;
......@@ -162,35 +177,44 @@ namespace armarx::manipulation::core
}
// TODO: this shold work under c++20
// unimanuals.emplace_back(hypothesis, prePose, retractPose, trajectory);
unimanuals.push_back(
{.hypothesis=hypothesis, .execPose=execPose, .prePose=prePose, .retractPose=retractPose, .actionTrajectory=trajectory, .handedness=handedness});
unimanuals.push_back({.hypothesis = hypothesis,
.execPose = execPose,
.prePose = prePose,
.retractPose = retractPose,
.actionTrajectory = trajectory,
.handedness = handedness});
}
return {unimanuals,
{.platformPose=platformPose, .platformPlanningStrategy=dto.common.platformPlanningStrategy},
{.platformPose = platformPose,
.platformPlanningStrategy = dto.common.platformPlanningStrategy},
dto.planningInformation};
}
arondto::SituatedAffordance toAron(const SituatedAffordance& bo)
arondto::SituatedAffordance
toAron(const SituatedAffordance& bo)
{
arondto::SituatedAffordance dto;
toAron(dto, bo);
return dto;
}
SituatedAffordance fromAron(const arondto::SituatedAffordance& dto)
SituatedAffordance
fromAron(const arondto::SituatedAffordance& dto)
{
return {dto.frame, fromAron(dto.affordance), dto.keyPoint, dto.pointCloud};
}
// }
void toAron(arondto::Affordance& dto, const Affordance& bo)
void
toAron(arondto::Affordance& dto, const Affordance& bo)
{
dto.actionType = bo.affordance_type->getID();
toAron(dto.robotDescription, bo.getRobotDescription());
}
void fromAron(const arondto::Affordance& dto, Affordance& bo)
void
fromAron(const arondto::Affordance& dto, Affordance& bo)
{
armem::robot::RobotDescription id;
fromAron(dto.robotDescription, id);
......@@ -198,7 +222,8 @@ namespace armarx::manipulation::core
bo = Affordance(dto.actionType, id);
}
void toAron(arondto::FramedUncertainPose& dto, const FramedUncertainPose& bo)
void
toAron(arondto::FramedUncertainPose& dto, const FramedUncertainPose& bo)
{
dto.header.frame = bo.pose->getFrame();
dto.header.agent = bo.pose->agent;
......@@ -206,12 +231,14 @@ namespace armarx::manipulation::core
dto.uncertainPose.covariance = bo.covariance;
}
void fromAron(const arondto::FramedUncertainPose& dto, FramedUncertainPose& bo)
void
fromAron(const arondto::FramedUncertainPose& dto, FramedUncertainPose& bo)
{
bo = fromAron(dto);
}
void toAron(arondto::ActionHypothesis& dto, const ActionHypothesis& bo)
void
toAron(arondto::ActionHypothesis& dto, const ActionHypothesis& bo)
{
toAron(dto.affordance, *bo.affordance);
toAron(dto.framedPose, *bo.framedPose);
......@@ -220,14 +247,16 @@ namespace armarx::manipulation::core
dto.handedness = bo.handedness;
}
void fromAron(const arondto::ActionHypothesis& dto, ActionHypothesis& bo)
void
fromAron(const arondto::ActionHypothesis& dto, ActionHypothesis& bo)
{
bo = fromAron(dto);
}
void toAron(arondto::ExecutableAction& dto, const ExecutableAction& bo)
void
toAron(arondto::ExecutableAction& dto, const ExecutableAction& bo)
{
for (const auto& um: bo.unimanual)
for (const auto& um : bo.unimanual)
{
auto& dto_um = dto.unimanual.emplace_back();
if (um.hypothesis.has_value())
......@@ -250,12 +279,14 @@ namespace armarx::manipulation::core
dto.planningInformation = bo.planningInformation;
}
void fromAron(const arondto::ExecutableAction& dto, ExecutableAction& bo)
void
fromAron(const arondto::ExecutableAction& dto, ExecutableAction& bo)
{
bo = fromAron(dto);
}
void toAron(arondto::SituatedAffordance& dto, const SituatedAffordance& bo)
void
toAron(arondto::SituatedAffordance& dto, const SituatedAffordance& bo)
{
dto.frame = bo.frame;
dto.pointCloud = bo.pointCloud;
......@@ -266,19 +297,22 @@ namespace armarx::manipulation::core
toAron(dto.affordance, bo.affordance);
}
void fromAron(const arondto::SituatedAffordance& dto, SituatedAffordance& bo)
void
fromAron(const arondto::SituatedAffordance& dto, SituatedAffordance& bo)
{
bo = fromAron(dto);
}
arondto::ExecutedAction toAron(const ExecutedAction& bo)
arondto::ExecutedAction
toAron(const ExecutedAction& bo)
{
arondto::ExecutedAction exAction;
toAron(exAction, bo);
return exAction;
}
void toAron(arondto::ExecutedAction& dto, const ExecutedAction& bo)
void
toAron(arondto::ExecutedAction& dto, const ExecutedAction& bo)
{
dto.action = toAron(*bo.action);
dto.startTime = *bo.startTime;
......@@ -293,25 +327,34 @@ namespace armarx::manipulation::core
dto.debugOutput = bo.debugOutput;
}
ExecutedAction fromAron(const arondto::ExecutedAction& dto)
ExecutedAction
fromAron(const arondto::ExecutedAction& dto)
{
return {fromAron(dto.action), dto.startTime, dto.endTime, dto.success, dto.outcome, dto.comment,
return {fromAron(dto.action),
dto.startTime,
dto.endTime,
dto.success,
dto.outcome,
dto.comment,
dto.debugOutput};
}
void fromAron(const arondto::ExecutedAction& dto, ExecutedAction& bo)
void
fromAron(const arondto::ExecutedAction& dto, ExecutedAction& bo)
{
bo = fromAron(dto);
}
arondto::HandTrajectoryKeypoint toAron(const HandTrajectoryKeypoint& bo)
arondto::HandTrajectoryKeypoint
toAron(const HandTrajectoryKeypoint& bo)
{
auto kp = arondto::HandTrajectoryKeypoint();
toAron(kp, bo);
return kp;
}
void toAron(arondto::HandTrajectoryKeypoint& dto, const HandTrajectoryKeypoint& bo)
void
toAron(arondto::HandTrajectoryKeypoint& dto, const HandTrajectoryKeypoint& bo)
{
dto.pose = bo.pose.toEigen();
dto.frame.frame = bo.pose.getFrame();
......@@ -319,52 +362,58 @@ namespace armarx::manipulation::core
dto.fingerValues = bo.fingerValues;
}
HandTrajectoryKeypoint fromAron(const arondto::HandTrajectoryKeypoint& dto)
HandTrajectoryKeypoint
fromAron(const arondto::HandTrajectoryKeypoint& dto)
{
return {dto.pose, dto.frame.frame, dto.fingerValues};
}
void fromAron(const arondto::HandTrajectoryKeypoint& dto, HandTrajectoryKeypoint& bo)
void
fromAron(const arondto::HandTrajectoryKeypoint& dto, HandTrajectoryKeypoint& bo)
{
bo = fromAron(dto);
}
arondto::HandTrajectory toAron(const HandTrajectory& bo)
arondto::HandTrajectory
toAron(const HandTrajectory& bo)
{
auto dto = arondto::HandTrajectory();
toAron(dto, bo);
return dto;
}
void toAron(arondto::HandTrajectory& dto, const HandTrajectory& bo)
void
toAron(arondto::HandTrajectory& dto, const HandTrajectory& bo)
{
std::transform(bo.keypoints.begin(), bo.keypoints.end(), std::back_inserter(dto.keypoints),
std::transform(bo.keypoints.begin(),
bo.keypoints.end(),
std::back_inserter(dto.keypoints),
[](const auto& kp) -> arondto::HandTrajectoryKeypoint
{
return toAron(kp);
});
{ return toAron(kp); });
dto.duration = bo.duration;
}
HandTrajectory fromAron(const arondto::HandTrajectory& dto)
HandTrajectory
fromAron(const arondto::HandTrajectory& dto)
{
auto traj = HandTrajectory();
traj.duration = dto.duration;
std::transform(dto.keypoints.begin(), dto.keypoints.end(), std::back_inserter(traj.keypoints),
[](const auto& kp) -> HandTrajectoryKeypoint
{
return fromAron(kp);
});
std::transform(dto.keypoints.begin(),
dto.keypoints.end(),
std::back_inserter(traj.keypoints),
[](const auto& kp) -> HandTrajectoryKeypoint { return fromAron(kp); });
return traj;
}
void fromAron(const arondto::HandTrajectory& dto, HandTrajectory& bo)
void
fromAron(const arondto::HandTrajectory& dto, HandTrajectory& bo)
{
bo = fromAron(dto);
}
void fromAron(const arondto::Arm& dto, Arm& bo)
void
fromAron(const arondto::Arm& dto, Arm& bo)
{
switch (dto.value)
{
......@@ -377,7 +426,8 @@ namespace armarx::manipulation::core
}
}
void fromAron(const arondto::Hand& dto, Hand& bo)
void
fromAron(const arondto::Hand& dto, Hand& bo)
{
switch (dto.value)
{
......@@ -392,35 +442,72 @@ namespace armarx::manipulation::core
}
}
arondto::ManipulationProcess toAron(const ManipulationProcess& bo)
void
toAron(arondto::Arm& dto, const Arm& bo)
{
switch (bo)
{
case Arm::Left:
dto = arondto::Arm::Left;
break;
case Arm::Right:
dto = arondto::Arm::Right;
break;
default:
ARMARX_ERROR << "Invalid arm";
}
}
void
toAron(arondto::Hand& dto, const Hand& bo)
{
switch (bo)
{
case Hand::Left:
dto = arondto::Hand::Left;
break;
case Hand::Right:
dto = arondto::Hand::Right;
break;
default:
ARMARX_ERROR << "Invalid hand";
}
}
arondto::ManipulationProcess
toAron(const ManipulationProcess& bo)
{
auto dto = arondto::ManipulationProcess();
toAron(dto, bo);
return dto;
}
void toAron(arondto::ManipulationProcess& dto, const ManipulationProcess& bo)
void
toAron(arondto::ManipulationProcess& dto, const ManipulationProcess& bo)
{
dto.actionHypotheses = meta::transform_container(bo.actionHypotheses, [](const auto& el)
{
return toAron(el);
});
dto.actionHypotheses = meta::transform_container(bo.actionHypotheses,
[](const auto& el) { return toAron(el); });
dto.executedAction = toAron(*bo.executedAction);
dto.pointCloud = bo.pointCloud;
}
ManipulationProcess fromAron(const arondto::ManipulationProcess& dto)
ManipulationProcess
fromAron(const arondto::ManipulationProcess& dto)
{
std::optional<armarx::manipulation::core::ExecutedAction> action;
if (dto.executedAction.has_value()) action = fromAron(dto.executedAction.value());
return ManipulationProcess{.actionHypotheses=meta::transform_container(dto.actionHypotheses, [](const auto& el)
{
return fromAron(el);
}), .executedAction=action, .pointCloud=dto.pointCloud};
if (dto.executedAction.has_value())
action = fromAron(dto.executedAction.value());
return ManipulationProcess{
.actionHypotheses = meta::transform_container(
dto.actionHypotheses, [](const auto& el) { return fromAron(el); }),
.executedAction = action,
.pointCloud = dto.pointCloud};
}
void fromAron(const arondto::ManipulationProcess& dto, ManipulationProcess& bo)
void
fromAron(const arondto::ManipulationProcess& dto, ManipulationProcess& bo)
{
bo = fromAron(dto);
}
}
} // namespace armarx::manipulation::core
source/armarx/manipulation/core/aron_conversions.h
+ 3
− 0
View file @ d13e8751
......@@ -78,6 +78,9 @@ namespace armarx::manipulation::core
void fromAron(const arondto::Arm& dto, Arm& bo);
void fromAron(const arondto::Hand& dto, Hand& bo);
void toAron(arondto::Arm& dto, const Arm& bo);
void toAron(arondto::Hand& dto, const Hand& bo);
namespace arondto
{
using ::armarx::manipulation::core::fromAron;
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment