42#include <gtest/gtest.h>
47#include <tf2_eigen/tf2_eigen.hpp>
52TEST(ServoUtilsUnitTests, JointLimitVelocityScaling)
56 const auto joint_model_group = robot_model->getJointModelGroup(
"panda_arm");
57 const auto joint_bounds = joint_model_group->getActiveJointModelsBounds();
60 Eigen::VectorXd incoming_velocities(joint_bounds.size());
61 for (
size_t i = 0; i < joint_bounds.size(); ++i)
63 const auto joint_bound = (*joint_bounds[i])[0];
64 if (joint_bound.velocity_bounded_)
66 incoming_velocities(i) = joint_bound.max_velocity_;
72 incoming_velocities(0) *= 1.1;
73 incoming_velocities(1) *= 1.05;
74 incoming_velocities.tail<5>() *= 0.7;
76 constexpr double tol = 0.001;
79 double user_velocity_override = 0.0;
80 double scaling_factor =
82 ASSERT_NEAR(scaling_factor, 1.0 / 1.1, tol);
85 user_velocity_override = 0.5;
88 ASSERT_NEAR(scaling_factor, 0.5, tol);
92 user_velocity_override = 1.0;
95 ASSERT_NEAR(scaling_factor, 1.0 / 1.1, tol);
98TEST(ServoUtilsUnitTests, validVector)
100 Eigen::VectorXd valid_vector(7);
101 valid_vector << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
105TEST(ServoUtilsUnitTests, invalidVector)
107 Eigen::VectorXd invalid_vector(6);
108 invalid_vector << 0.0, 0.0, 0.0, 0.0, std::nan(
""), 0.0;
112TEST(ServoUtilsUnitTests, validTwist)
114 Eigen::VectorXd valid_vector(6);
115 valid_vector << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
120TEST(ServoUtilsUnitTests, emptyTwistFrame)
122 Eigen::VectorXd invalid_vector(6);
123 invalid_vector << 0.0, 0.0, 0.0, 0.0, std::nan(
""), 0.0;
129TEST(ServoUtilsUnitTests, invalidTwistVelocities)
131 Eigen::VectorXd invalid_vector(6);
132 invalid_vector << 0.0, 0.0, 0.0, 0.0, 0.0, std::nan(
"");
137TEST(ServoUtilsUnitTests, validIsometry)
139 Eigen::Isometry3d valid_isometry;
140 valid_isometry.setIdentity();
144TEST(ServoUtilsUnitTests, invalidIsometry)
146 Eigen::Isometry3d invalid_isometry;
147 invalid_isometry.setIdentity();
148 invalid_isometry.translation().z() = std::nan(
"");
152TEST(ServoUtilsUnitTests, validPose)
154 Eigen::Isometry3d valid_isometry;
155 valid_isometry.setIdentity();
160TEST(ServoUtilsUnitTests, ApproachingSingularityScaling)
164 moveit::core::RobotStatePtr robot_state = std::make_shared<moveit::core::RobotState>(robot_model);
166 servo::Params servo_params;
167 servo_params.move_group_name =
"panda_arm";
168 const auto joint_model_group = robot_state->getJointModelGroup(servo_params.move_group_name);
169 robot_state->setToDefaultValues();
171 Eigen::Vector<double, 6> cartesian_delta{ 0.005, 0.0, 0.0, 0.0, 0.0, 0.0 };
173 Eigen::Vector<double, 7> state_ready{ 0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785 };
174 robot_state->setJointGroupActivePositions(joint_model_group, state_ready);
179 Eigen::Vector<double, 7> state_approaching_singularity{ 0.0, 0.334, 0.0, -1.177, 0.0, 1.510, 0.785 };
180 robot_state->setJointGroupActivePositions(joint_model_group, state_approaching_singularity);
185TEST(ServoUtilsUnitTests, HaltForSingularityScaling)
189 moveit::core::RobotStatePtr robot_state = std::make_shared<moveit::core::RobotState>(robot_model);
191 servo::Params servo_params;
192 servo_params.move_group_name =
"panda_arm";
193 const auto joint_model_group = robot_state->getJointModelGroup(servo_params.move_group_name);
194 robot_state->setToDefaultValues();
196 Eigen::Vector<double, 6> cartesian_delta{ 0.005, 0.0, 0.0, 0.0, 0.0, 0.0 };
199 Eigen::Vector<double, 7> state_ready{ 0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785 };
200 robot_state->setJointGroupActivePositions(joint_model_group, state_ready);
205 Eigen::Vector<double, 7> singular_state{ -0.0001, 0.5690, 0.0005, -0.7782, 0.0, 1.3453, 0.7845 };
206 robot_state->setJointGroupActivePositions(joint_model_group, singular_state);
211TEST(ServoUtilsUnitTests, LeavingSingularityScaling)
215 moveit::core::RobotStatePtr robot_state = std::make_shared<moveit::core::RobotState>(robot_model);
217 servo::Params servo_params;
218 servo_params.move_group_name =
"panda_arm";
219 const auto joint_model_group = robot_state->getJointModelGroup(servo_params.move_group_name);
220 robot_state->setToDefaultValues();
222 Eigen::Vector<double, 6> cartesian_delta{ -0.005, 0.0, 0.0, 0.0, 0.0, 0.0 };
225 Eigen::Vector<double, 7> state_ready{ 0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785 };
226 robot_state->setJointGroupActivePositions(joint_model_group, state_ready);
231 Eigen::Vector<double, 7> state_leaving_singularity{ 0.0, 0.3458, 0.0, -1.1424, 0.0, 1.4865, 0.785 };
232 robot_state->setJointGroupActivePositions(joint_model_group, state_leaving_singularity);
237TEST(ServoUtilsUnitTests, ExtractRobotState)
241 moveit::core::RobotStatePtr robot_state = std::make_shared<moveit::core::RobotState>(robot_model);
242 std::string joint_model_group =
"panda_arm";
244 current_state.
positions = Eigen::VectorXd::Ones(7);
245 robot_state->setJointGroupPositions(joint_model_group, current_state.
positions);
248 ASSERT_EQ(current_state.
positions, new_state.positions);
251TEST(ServoUtilsUnitTests, SlidingWinodw)
255 current_state.
positions = Eigen::VectorXd::Ones(7);
256 current_state.
joint_names = {
"j1",
"j2",
"j3",
"j4",
"j5",
"j6",
"j7" };
257 std::deque<moveit_servo::KinematicState> window;
258 for (
size_t i = 0; i < 10; ++i)
265 ASSERT_EQ(window.size(), 7ul);
266 servo::Params params;
268 ASSERT_TRUE(msg.has_value());
269 ASSERT_EQ(msg.value().points.size(), 6ul);
272 constexpr int min_points_for_traj_msg = 3;
273 while (window.size() > min_points_for_traj_msg - 1)
279 ASSERT_FALSE(msg.has_value());
286 rclcpp::init(argc, argv);
287 ::testing::InitGoogleTest(&argc, argv);
288 int result = RUN_ALL_TESTS();
int main(int argc, char **argv)
moveit::core::RobotModelPtr loadTestingRobotModel(const std::string &robot_name)
Loads a robot from moveit_resources.
std::optional< trajectory_msgs::msg::JointTrajectory > composeTrajectoryMessage(const servo::Params &servo_params, const std::deque< KinematicState > &joint_cmd_rolling_window)
Create a trajectory message from a rolling window queue of joint state commands. Method optionally re...
std::pair< double, StatusCode > velocityScalingFactorForSingularity(const moveit::core::RobotStatePtr &robot_state, const Eigen::VectorXd &target_delta_x, const servo::Params &servo_params)
Computes scaling factor for velocity when the robot is near a singularity.
@ DECELERATE_FOR_APPROACHING_SINGULARITY
@ DECELERATE_FOR_LEAVING_SINGULARITY
double jointLimitVelocityScalingFactor(const Eigen::VectorXd &velocities, const moveit::core::JointBoundsVector &joint_bounds, double scaling_override)
Apply velocity scaling based on joint limits. If the robot model does not have velocity limits define...
bool isValidCommand(const Eigen::VectorXd &command)
Checks if a given VectorXd is a valid command.
void updateSlidingWindow(KinematicState &next_joint_state, std::deque< KinematicState > &joint_cmd_rolling_window, double max_expected_latency, const rclcpp::Time &cur_time)
Adds a new joint state command to a queue containing commands over a time window. Also modifies the v...
KinematicState extractRobotState(const moveit::core::RobotStatePtr &robot_state, const std::string &move_group_name)
Extract the state from a RobotStatePtr instance.
Eigen::VectorXd positions
std::vector< std::string > joint_names
Eigen::Vector< double, 6 > velocities
TEST(AllValid, Instantiate)