diff --git a/source/armarx/control/pointing/core/Pointing.cpp b/source/armarx/control/pointing/core/Pointing.cpp
index 1f6426fe5f3f14f5e73678689b1ca270d648b002..7352a446d0e65af31d8f26ffcb1a34e39609ef24 100644
--- a/source/armarx/control/pointing/core/Pointing.cpp
+++ b/source/armarx/control/pointing/core/Pointing.cpp
@@ -84,20 +84,28 @@ namespace armarx::control::pointing::core
Pointing::computTrajectory(VirtualRobot::RobotPtr robot, Side side, Eigen::Vector3f target)
{
ARMARX_VERBOSE << "Computing trajectory...";
- auto ik = PointingIK(robot, side);
- armarx::TrajectoryPtr traj = ik.computeTrajectory(target);
- if (not traj)
- {
- ARMARX_WARNING << "IK didn't find a solution";
- throw PointingFailedException();
- }
- ARMARX_VERBOSE << "Done computing trajectory";
+ auto ik = PointingIK(robot, side, target);
- if (traj->size() == 1) // forearm already aligned with target
+ if (ik.isTargetReached()) // forearm already aligned with target
{
return armarx::TrajectoryPtr(new Trajectory);
}
- return traj;
+
+ do
+ {
+ if (ik.optimize() < 1e-6)
+ {
+ ARMARX_WARNING << "PointingIK didn't find a solution";
+ throw PointingFailedException();
+ }
+ if (aborted)
+ {
+ ARMARX_INFO << "Pointing aborted.";
+ throw PointingAbortedException();
+ }
+ } while (not ik.isTargetReached());
+
+ return ik.getTrajectory();
}
void
diff --git a/source/armarx/control/pointing/core/PointingIK.cpp b/source/armarx/control/pointing/core/PointingIK.cpp
index c7b903cbfcadb43d3302a90380567edd29643cf4..d29958582af69c86c5ae0314a42086dafce2d729 100644
--- a/source/armarx/control/pointing/core/PointingIK.cpp
+++ b/source/armarx/control/pointing/core/PointingIK.cpp
@@ -6,7 +6,7 @@
namespace armarx::control::pointing::core
{
- PointingIK::PointingIK(VirtualRobot::RobotPtr robot, Side side)
+ PointingIK::PointingIK(VirtualRobot::RobotPtr robot, Side side, Eigen::Vector3f& target)
{
auto humanMapping = robot->getHumanMapping();
ARMARX_CHECK(humanMapping) << "Robot '" << robot->getName() << "' has no human mapping";
@@ -31,39 +31,41 @@ namespace armarx::control::pointing::core
*reducedRobot_->getNode(elbow),
side == LEFT ? reducedRobot_->humanMapping()->leftArm
: reducedRobot_->humanMapping()->rightArm);
- }
- TrajectoryPtr
- PointingIK::computeTrajectory(Eigen::Vector3f& target)
- {
ik_->setPointingTarget(target.cast<double>() / 1000); // simox uses meters
- // actuated joints x timesteps
- std::vector<std::vector<double>> configs(ik_->getNodeNames().size());
- auto appendConfig = [&configs](Eigen::VectorXd config)
- {
- for (size_t i = 0; i < configs.size(); i++)
- {
- configs[i].push_back(config[i]);
- }
- };
+ trajData.assign(ik_->getNodeNames().size(), std::vector<double>());
+ appendTrajData(ik_->getJointValues());
+ }
- appendConfig(ik_->getJointValues());
+ bool
+ PointingIK::isTargetReached()
+ {
+ return ik_->isTargetReached();
+ }
+ double
+ PointingIK::optimize()
+ {
Eigen::VectorXd prevJointValues = ik_->getJointValues();
- while (not ik_->isTargetReached())
- {
- ik_->optimize();
- if ((ik_->getJointValues() - prevJointValues).norm() < 1e-6)
- {
- return nullptr;
- }
+ ik_->optimize();
+ appendTrajData(ik_->getJointValues());
+ return (ik_->getJointValues() - prevJointValues).norm();
+ }
- appendConfig(ik_->getJointValues());
- prevJointValues = ik_->getJointValues();
- }
+ TrajectoryPtr
+ PointingIK::getTrajectory()
+ {
+ return new Trajectory(trajData, Ice::DoubleSeq(), ik_->getNodeNames());
+ }
- return new Trajectory(configs, Ice::DoubleSeq(), ik_->getNodeNames());
+ void
+ PointingIK::appendTrajData(const Eigen::VectorXd& jointValues)
+ {
+ for (size_t i = 0; i < trajData.size(); i++)
+ {
+ trajData[i].push_back(jointValues[i]);
+ }
}
} // namespace armarx::control::pointing::core
diff --git a/source/armarx/control/pointing/core/PointingIK.h b/source/armarx/control/pointing/core/PointingIK.h
index 8660d52ab28d4f5ce26be320a44e13c42146d7e0..eb9942f3735343a8ec1b258543b0832aff448b62 100644
--- a/source/armarx/control/pointing/core/PointingIK.h
+++ b/source/armarx/control/pointing/core/PointingIK.h
@@ -10,11 +10,29 @@ namespace armarx::control::pointing::core
{
/**
- * @brief IK to compute joint configurations for a pointing gesture.
+ * @brief IK to compute trajectory for a pointing gesture.
*
- * Wrapper for simox-control's method::example::PointingIK. Computes pointing gesture joint
- * configurations that line up the forearm with the target. Uses the robot's human mapping to
- * obtain relevant joints: All arm-joints until (exluding) the wrist are beeing used.
+ * Wrapper for simox-control's method::example::PointingIK. Computes a trajectory for a
+ * pointing gesture that lines up the forearm with a target. Uses the robot's human mapping to
+ * obtain relevant joints: All arm-joints until (excluding) the wrist are being used.
+ *
+ * This class keeps an internal robot model. Each call to @ref optimize() optimizes the joint
+ * configuration regarding pointing towards the target. @ref isTargetReached() returns whether
+ * the robot in the latest configuration already points at the target. @ref getTrajectory()
+ * returns the sequence of generated configurations as Trajectory.
+ *
+ * Example usage:
+ *
+ * @code
+ * ik = PointingIK(...);
+ *
+ * while (not ik.isTargetReached()) {
+ * if (ik.optimize) < 1e-6 {
+ * return nullptr;
+ * }
+ * }
+ * return ik.getTrajectory();
+ * @endcode
*
* @ingroup Library-pointing
*/
@@ -26,35 +44,45 @@ namespace armarx::control::pointing::core
* @param robot Robot model with current robot configuration to be used by the IK. Must
* provide a human mapping.
* @param side Side of the arm to point with.
+ * @param target Target position to point at in the global coordinate system (mm).
*/
- PointingIK(VirtualRobot::RobotPtr robot, Side side);
+ PointingIK(VirtualRobot::RobotPtr robot, Side side, Eigen::Vector3f& target);
+ /**
+ * @return Whether the robot in the latest joint configuration (from @ref optimize())
+ * already points at the target.
+ */
+ bool isTargetReached();
/**
- * @brief Compute a joint trajectory ending in a configuration in which the robot points in
- * the direction of the passed target position.
- *
- * The initial timestep of the trajectory contains the current joint configuration of the
- * robot passed to the constructor. The following timesteps contain the step-by-step
- * optimized configurations until the end configuration is reached. The trajectory is
- * constructed without timestamps, i.e., has a duration of 1s with equidistant timesteps.
+ * @brief Optimizes the joint configuration of the internal robot model regarding pointing
+ * towards the target.
+ * @return Difference between the previous and new joint configurations. May be used to
+ * determine if the IK is unable to find a solution.
+ */
+ double optimize();
+
+ /**
+ * @brief Constructs a trajectory containing the sequence of joint configurations from the
+ * calls to @ref optimize().
*
- * Neither the generated trajectory nor the end configuration are guaranteed to be self-
- * collision-free. Furthermore, generated trajectories aren't efficient in reaching the end
+ * Neither the trajectory nor the end configuration are guaranteed to be self-
+ * collision free. Furthermore, the trajectories aren't efficient in reaching the end
* configuration.
*
- * This method may only be called once to ensure a well-defined initial robot configuration.
+ * `getTrajectory()->size()` will equal the number of calls to @ref optimize() plus one for
+ * the initial joint configuration
*
- * @param target Target position to point at in the global coordinate system (mm).
- * @return The computed trajectory. Null if no solution was found. If the robot has already
- * pointed at the target, the trajectory only contains the initial timestep (i.e.
- * size()==1).
+ * @return The constructed trajectory.
*/
- TrajectoryPtr computeTrajectory(Eigen::Vector3f& target);
+ armarx::TrajectoryPtr getTrajectory();
private:
std::unique_ptr<simox::control::simox::robot::Robot> reducedRobot_;
std::unique_ptr<simox::control::method::example::PointingIK> ik_;
+
+ std::vector<std::vector<double>> trajData; // actuated joints x timesteps
+ void appendTrajData(const Eigen::VectorXd& jointValues);
};
} // namespace armarx::control::pointing::core