36#include <gtest/gtest.h>
43constexpr double DEFAULT_TIMESTEP = 0.1;
44constexpr char JOINT_GROUP[] =
"panda_arm";
46class RuckigTests :
public testing::Test
52 trajectory_ = std::make_shared<robot_trajectory::RobotTrajectory>(robot_model_, JOINT_GROUP);
55 moveit::core::RobotModelPtr robot_model_;
56 robot_trajectory::RobotTrajectoryPtr trajectory_;
69 trajectory_->addSuffixWayPoint(robot_state, DEFAULT_TIMESTEP);
72 std::vector<double> joint_positions;
74 joint_positions.at(0) += 0.05;
76 trajectory_->addSuffixWayPoint(robot_state, DEFAULT_TIMESTEP);
79 smoother_.applySmoothing(*trajectory_, 1.0 , 1.0 ));
82TEST_F(RuckigTests, basic_trajectory_with_custom_limits)
91 trajectory_->addSuffixWayPoint(robot_state, DEFAULT_TIMESTEP);
94 std::vector<double> joint_positions;
96 joint_positions.at(0) += 0.05;
98 trajectory_->addSuffixWayPoint(robot_state, DEFAULT_TIMESTEP);
101 std::unordered_map<std::string, double> vel_limits{ {
"panda_joint1", 1.3 } };
102 std::unordered_map<std::string, double> accel_limits{ {
"panda_joint2", 2.3 }, {
"panda_joint3", 3.3 } };
103 std::unordered_map<std::string, double> jerk_limits{ {
"panda_joint5", 100.0 } };
105 EXPECT_TRUE(smoother_.applySmoothing(*trajectory_, vel_limits, accel_limits, jerk_limits));
113 const double ideal_duration = 0.210;
122 trajectory_->addSuffixWayPoint(robot_state, 0.0);
125 trajectory_->addSuffixWayPoint(robot_state, DEFAULT_TIMESTEP);
128 smoother_.applySmoothing(*trajectory_, 1.0 , 1.0 ));
131 for (
size_t waypoint_idx = 1; waypoint_idx < trajectory_->getWayPointCount() - 1; ++waypoint_idx)
133 EXPECT_NE(trajectory_->getWayPointDurationFromPrevious(waypoint_idx), 0);
138 EXPECT_GT(trajectory_->getWayPointDurationFromStart(trajectory_->getWayPointCount() - 1), 0.9999 * ideal_duration);
139 EXPECT_LT(trajectory_->getWayPointDurationFromStart(trajectory_->getWayPointCount() - 1), 1.11 * ideal_duration);
152 trajectory_->addSuffixWayPoint(robot_state, DEFAULT_TIMESTEP);
155 auto first_waypoint_input = robot_state;
159 smoother_.applySmoothing(*trajectory_, 1.0 , 1.0 ));
161 const auto new_first_waypoint = trajectory_->getFirstWayPointPtr();
162 const auto& variable_names = new_first_waypoint->getVariableNames();
163 for (
const std::string& variable_name : variable_names)
165 EXPECT_EQ(first_waypoint_input.getVariablePosition(variable_name),
166 new_first_waypoint->getVariablePosition(variable_name));
172 testing::InitGoogleTest(&argc, argv);
173 return RUN_ALL_TESTS();
Representation of a robot's state. This includes position, velocity, acceleration and effort.
void zeroVelocities()
Set all velocities to 0.0.
void setVariablePosition(const std::string &variable, double value)
Set the position of a single variable. An exception is thrown if the variable name is not known.
void setJointGroupPositions(const std::string &joint_group_name, const double *gstate)
Given positions for the variables that make up a group, in the order found in the group (including va...
void copyJointGroupPositions(const std::string &joint_group_name, std::vector< double > &gstate) const
For a given group, copy the position values of the variables that make up the group into another loca...
void zeroAccelerations()
Set all accelerations to 0.0.
void setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
moveit::core::RobotModelPtr loadTestingRobotModel(const std::string &robot_name)
Loads a robot from moveit_resources.
TEST_F(RuckigTests, basic_trajectory)
int main(int argc, char **argv)