make aborting while computing trajectory possible

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

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

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