test_planar_joint_jacobian.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2019, PickNik LLC.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the PickNik nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ivo Vatavuk */
36 
42 
43 using namespace moveit::core;
44 
45 class SimplePlanarRobot : public testing::Test
46 {
47 protected:
48  void SetUp() override
49  {
50  RobotModelBuilder builder("simple", "a");
51  builder.addChain("a->b", "planar");
52  builder.addGroupChain("a", "b", "group");
53  robot_model_ = builder.build();
54  robot_state_ = std::make_shared<RobotState>(robot_model_);
55  }
56 
57  void TearDown() override
58  {
59  }
60 
61 protected:
62  RobotModelPtr robot_model_;
63  RobotStatePtr robot_state_;
64 };
65 
66 TEST_F(SimplePlanarRobot, testSimplePlanarRobot)
67 {
68  std::cout << "Testing simple planar robot jacobian\n";
69 
70  Eigen::Vector3d reference_point_position(0.0, 0.0, 0.0);
71  auto joint_model_group = robot_model_->getJointModelGroup("group");
72 
73  std::vector<double> q_test{ 0.0, 0.0, 0.0 };
74  //-----------------------Set robot state-----------------------
75  robot_state_->setJointGroupPositions(joint_model_group, q_test);
76  robot_state_->updateLinkTransforms();
77 
78  //-----------------------Calculate Jacobian-----------------------
79  Eigen::MatrixXd jacobian;
80  robot_state_->getJacobian(joint_model_group, robot_state_->getLinkModel("b"), reference_point_position, jacobian);
81 
82  //-----------------------Move first axis to 10 m-----------------------
83  q_test[0] = 10.0;
84  //-----------------------Set robot state-----------------------
85  robot_state_->setJointGroupPositions(joint_model_group, q_test);
86  robot_state_->updateLinkTransforms();
87 
88  //-----------------------Calculate Jacobian-----------------------
89  Eigen::MatrixXd jacobian_2;
90  robot_state_->getJacobian(joint_model_group, robot_state_->getLinkModel("b"), reference_point_position, jacobian_2);
91 
92  std::cout << "First Jacobian = \n" << jacobian << "\n";
93  std::cout << "Second Jacobian = \n" << jacobian_2 << "\n";
94 
95  EXPECT_EIGEN_NEAR(jacobian, jacobian_2, 1e-5);
96 }
97 
98 int main(int argc, char** argv)
99 {
100  testing::InitGoogleTest(&argc, argv);
101  return RUN_ALL_TESTS();
102 }
moveit::core
Core components of MoveIt.
Definition: kinematics_base.h:83
SimplePlanarRobot::TearDown
void TearDown() override
Definition: test_planar_joint_jacobian.cpp:57
moveit::core::RobotModelBuilder::addGroupChain
RobotModelBuilder & addGroupChain(const std::string &base_link, const std::string &tip_link, const std::string &name="")
Adds a new group using a chain of links. The group is the parent joint of each link in the chain.
Definition: robot_model_test_utils.cpp:436
SimplePlanarRobot::SetUp
void SetUp() override
Definition: test_planar_joint_jacobian.cpp:48
SimplePlanarRobot::robot_model_
RobotModelPtr robot_model_
Definition: test_planar_joint_jacobian.cpp:62
robot_model.h
main
int main(int argc, char **argv)
Definition: test_planar_joint_jacobian.cpp:98
SimplePlanarRobot::robot_state_
RobotStatePtr robot_state_
Definition: test_planar_joint_jacobian.cpp:63
cartesian_interpolator.h
moveit::core::RobotModelBuilder::build
moveit::core::RobotModelPtr build()
Builds and returns the robot model added to the builder.
Definition: robot_model_test_utils.cpp:484
robot_model_test_utils.h
EXPECT_EIGEN_NEAR
#define EXPECT_EIGEN_NEAR(val1, val2, prec_)
Definition: eigen_test_utils.h:76
TEST_F
TEST_F(SimplePlanarRobot, testSimplePlanarRobot)
Definition: test_planar_joint_jacobian.cpp:66
moveit::core::RobotModelBuilder
Easily build different robot models for testing. Essentially a programmer-friendly light wrapper arou...
Definition: robot_model_test_utils.h:158
moveit::core::RobotModelBuilder::addChain
RobotModelBuilder & addChain(const std::string &section, const std::string &type, const std::vector< geometry_msgs::Pose > &joint_origins={}, urdf::Vector3 joint_axis=urdf::Vector3(1.0, 0.0, 0.0))
Adds a chain of links and joints to the builder. The joint names are generated automatically as "<par...
Definition: robot_model_test_utils.cpp:204
eigen_test_utils.h
SimplePlanarRobot
Definition: test_planar_joint_jacobian.cpp:45
robot_state.h
fcl::Vector3d
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:89


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sat Jul 6 2024 02:26:32