44 #include <kdl/jntarray.hpp> 46 #include <kdl/chainiksolverpos_nr_jl.hpp> 47 #include <kdl/chainiksolvervel_pinv.hpp> 48 #include <kdl/chainfksolverpos_recursive.hpp> 49 #include <kdl/chainjnttojacsolver.hpp> 51 #include <gtest/gtest.h> 55 int rand_num = rand()%100+1;
56 double result = min + (double)((max-min)*rand_num)/101.0;
60 bool NOT_NEAR(
const double &v1,
const double &v2,
const double &NEAR)
62 if(fabs(v1-v2) > NEAR)
67 void testJacobian(std::string group_name, std::string base_link, std::string tip_link)
69 SCOPED_TRACE(group_name +
": " + base_link +
" to " + tip_link);
73 robot_model::RobotModelPtr kinematic_model;
74 kinematic_model.reset(
new robot_model::RobotModel(model_loader.
getURDF(),model_loader.
getSRDF()));
76 robot_state::RobotStatePtr kinematic_state;
77 kinematic_state.reset(
new robot_state::RobotState(kinematic_model));
78 kinematic_state->setToDefaultValues();
82 std::string link_name = tip_link;
83 std::vector<double> joint_angles(7,0.0);
84 geometry_msgs::Point ref_position;
85 Eigen::MatrixXd jacobian;
86 Eigen::Vector3d point(0.0,0.0,0.0);
87 kinematic_state->setJointGroupPositions(joint_state_group, &joint_angles[0]);
88 ASSERT_TRUE(kinematic_state->getJacobian(joint_state_group, kinematic_state->getRobotModel()->
getLinkModel(link_name),point,jacobian));
93 ROS_ERROR(
"Could not initialize tree object");
96 std::string base_frame(base_link);
97 std::string tip_frame(tip_link);
98 if (!tree.
getChain(base_frame, tip_frame, kdl_chain))
100 ROS_ERROR(
"Could not initialize chain object");
107 unsigned int NUM_TESTS = 10000;
108 for(
unsigned int i=0; i < NUM_TESTS; i++)
110 for(
int j=0; j < 7; j++)
113 joint_angles[j] = q_in(j);
116 kinematic_state->setJointGroupPositions(joint_state_group, &joint_angles[0]);
117 EXPECT_TRUE(kinematic_state->getJacobian(joint_state_group, kinematic_state->getRobotModel()->
getLinkModel(link_name), point, jacobian));
118 for(
unsigned int k=0; k < 6; k++)
120 for(
unsigned int m=0; m < 7; m++)
130 testJacobian(
"right_arm",
"torso_lift_link",
"r_wrist_roll_link");
135 testJacobian(
"left_arm",
"torso_lift_link",
"l_wrist_roll_link");
138 int main(
int argc,
char **argv)
140 testing::InitGoogleTest(&argc, argv);
141 ros::init(argc, argv,
"test_jacobian_solver");
142 return RUN_ALL_TESTS();
KDL_PARSER_PUBLIC bool treeFromUrdfModel(const urdf::ModelInterface &robot_model, KDL::Tree &tree)
const srdf::ModelSharedPtr & getSRDF() const
const LinkModel * getLinkModel(const std::string &link) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
#define EXPECT_FALSE(args)
int main(int argc, char **argv)
virtual int JntToJac(const JntArray &q_in, Jacobian &jac, int segmentNR=-1)
bool getChain(const std::string &chain_root, const std::string &chain_tip, Chain &chain) const
double gen_rand(double min, double max)
#define EXPECT_TRUE(args)
bool NOT_NEAR(const double &v1, const double &v2, const double &NEAR)
void testJacobian(std::string group_name, std::string base_link, std::string tip_link)
TEST(JacobianSolver, solver)
const urdf::ModelInterfaceSharedPtr & getURDF() const