45#include <gtest/gtest.h>
53#include <moveit_msgs/msg/constraints.hpp>
56#include <ompl/util/Exception.h>
57#include <ompl/base/spaces/RealVectorStateSpace.h>
58#include <ompl/base/spaces/constraint/ProjectedStateSpace.h>
64 return moveit::getLogger(
"moveit.planners.ompl.test_constrained_planning_state_space");
71 DummyConstraint(
const unsigned int num_dofs) : ompl::base::Constraint(num_dofs, 1)
74 void function(
const Eigen::Ref<const Eigen::VectorXd>& , Eigen::Ref<Eigen::VectorXd> out)
const override
74 void function(
const Eigen::Ref<const Eigen::VectorXd>& , Eigen::Ref<Eigen::VectorXd> out)
const override {
…}
103 moveit_state_space_ = std::make_shared<ompl_interface::ConstrainedPlanningStateSpace>(space_spec);
110 auto con = std::make_shared<DummyConstraint>(
num_dofs_);
116 SCOPED_TRACE(
"testGetValueAddressAtIndex");
120 auto state_ptr = ompl_state->as<ompl::base::ConstrainedStateSpace::StateType>()->
getState();
122 EXPECT_FALSE(out_joint_positions ==
nullptr);
124 for (std::size_t i = 0; i <
num_dofs_; ++i)
126 *(out_joint_positions + i) = joint_positions[i];
133 for (std::size_t i = 0; i <
num_dofs_; ++i)
141 SCOPED_TRACE(
"testCopyToRobotState");
149 auto state_ptr = ompl_state->as<ompl::base::ConstrainedStateSpace::StateType>()->
getState();
151 EXPECT_FALSE(out_joint_positions ==
nullptr);
153 for (std::size_t i = 0; i <
num_dofs_; ++i)
155 *(out_joint_positions + i) = joint_positions[i];
164 Eigen::VectorXd out_joint_position(
num_dofs_);
167 EXPECT_EQ(joint_positions.size(), out_joint_position.size());
169 for (std::size_t i = 0; i <
num_dofs_; ++i)
171 EXPECT_EQ(joint_positions[i], out_joint_position[i]);
177 SCOPED_TRACE(
"testCopyToOMPLState");
191 auto state_ptr = ompl_state->as<ompl::base::ConstrainedStateSpace::StateType>()->
getState();
193 EXPECT_FALSE(out_joint_positions ==
nullptr);
195 for (std::size_t i = 0; i <
num_dofs_; ++i)
197 EXPECT_EQ(joint_positions[i], *(out_joint_positions + i));
203 SCOPED_TRACE(
"testCopyJointToOMPLState");
216 for (std::size_t joint_index = 0; joint_index <
num_dofs_; ++joint_index)
219 EXPECT_FALSE(joint_model ==
nullptr);
221 RCLCPP_DEBUG_STREAM(
getLogger(),
"Joint model: " << joint_model->
getName() <<
" index: " << joint_index);
225 moveit_state_space_->copyJointToOMPLState(ompl_state.get(), moveit_state, joint_model, joint_index);
229 auto state_ptr = ompl_state->as<ompl::base::ConstrainedStateSpace::StateType>()->
getState();
231 EXPECT_FALSE(out_joint_positions ==
nullptr);
233 for (std::size_t i = 0; i <
num_dofs_; ++i)
235 EXPECT_EQ(joint_positions[i], *(out_joint_positions + i));
257 testGetValueAddressAtIndex();
262 testCopyToRobotState();
267 testCopyToOMPLState();
272 testCopyJointToOMPLState();
288 testGetValueAddressAtIndex();
293 testCopyToRobotState();
298 testCopyToOMPLState();
303 testCopyJointToOMPLState();
319 testGetValueAddressAtIndex();
324 testCopyToRobotState();
329 testCopyToOMPLState();
334 testCopyJointToOMPLState();
342 testing::InitGoogleTest(&argc, argv);
343 return RUN_ALL_TESTS();
Dummy constraint for testing, always satisfied. We need this to create and OMPL ConstrainedStateSpace...
DummyConstraint(const unsigned int num_dofs)
void function(const Eigen::Ref< const Eigen::VectorXd > &, Eigen::Ref< Eigen::VectorXd > out) const override
Robot independent implementation of the tests.
TestConstrainedStateSpace(const std::string &robot_name, const std::string &group_name)
ompl_interface::ConstrainedPlanningStateSpacePtr moveit_state_space_
void testGetValueAddressAtIndex()
ompl::base::ConstrainedStateSpacePtr constrained_state_space_
void setupMoveItStateSpace()
void testCopyToRobotState()
void setupOMPLStateSpace()
void testCopyJointToOMPLState()
void testCopyToOMPLState()
const std::vector< std::string > & getActiveJointModelNames() const
Get the names of the active joints in this group. These are the names of the joints returned by getJo...
const JointModel * getJointModel(const std::string &joint) const
Get a joint by its name. Throw an exception if the joint is not part of this group.
A joint from the robot. Models the transform that this joint applies in the kinematic chain....
size_t getFirstVariableIndex() const
Get the index of this joint's first variable within the full robot state.
std::size_t getVariableCount() const
Get the number of variables that describe this joint.
const std::string & getName() const
Get the name of the joint.
Representation of a robot's state. This includes position, velocity, acceleration and effort.
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 setToDefaultValues()
Set all joints to their default positions. The default position is 0, or if that is not within bounds...
Robot independent test class setup.
Eigen::VectorXd getDeterministicState() const
Create a joint position vector with values 0.1, 0.2, 0.3, ... where the length depends on the number ...
moveit::core::RobotModelPtr robot_model_
LoadTestRobot(const std::string &robot_name, const std::string &group_name)
const moveit::core::JointModelGroup * joint_model_group_
rclcpp::Logger getLogger(const std::string &name)
Creates a namespaced logger.
TEST_F(PandaCopyStateTest, testGetValueAddressAtIndex)
int main(int argc, char **argv)
rclcpp::Logger getLogger()
bool getState(const std::shared_ptr< moveit_msgs::srv::GetRobotStateFromWarehouse::Request > &request, const std::shared_ptr< moveit_msgs::srv::GetRobotStateFromWarehouse::Response > &response, moveit_warehouse::RobotStateStorage &rs)