diff --git a/source/armarx/control/pointing/core/Pointing.cpp b/source/armarx/control/pointing/core/Pointing.cpp index f10ca43c321c3d6189d5119144088162f7873ebf..91f13a70df14b0934aec97f56f7fd5361fb2abf3 100644 --- a/source/armarx/control/pointing/core/Pointing.cpp +++ b/source/armarx/control/pointing/core/Pointing.cpp @@ -38,10 +38,17 @@ namespace armarx::control::pointing::core addHandTrajectory(robot, traj, shapeJointValues); } - traj = traj->calculateTimeOptimalTrajectory( - params.maxVelocity, params.maxAcceleration, 0.0, IceUtil::Time::milliSeconds(10)); + if (traj->dim() > 0) + { + traj = traj->calculateTimeOptimalTrajectory( + params.maxVelocity, params.maxAcceleration, 0.0, IceUtil::Time::milliSeconds(10)); - playTrajectory(traj); + playTrajectory(traj); + } + else + { + ARMARX_INFO << "Already pointing at target"; + } } void @@ -89,7 +96,7 @@ namespace armarx::control::pointing::core } ARMARX_VERBOSE << "Done computing trajectory"; - if (traj->getLength() == 1) // already pointing at target + if (traj->size() == 1) // already pointing at target { return armarx::TrajectoryPtr(new Trajectory); } diff --git a/source/armarx/control/pointing/core/PointingIK.h b/source/armarx/control/pointing/core/PointingIK.h index 028e5b4b97dcf70250b25b1b6ec59f27307a7647..8660d52ab28d4f5ce26be320a44e13c42146d7e0 100644 --- a/source/armarx/control/pointing/core/PointingIK.h +++ b/source/armarx/control/pointing/core/PointingIK.h @@ -47,7 +47,8 @@ namespace armarx::control::pointing::core * * @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. + * pointed at the target, the trajectory only contains the initial timestep (i.e. + * size()==1). */ TrajectoryPtr computeTrajectory(Eigen::Vector3f& target);