pr2_arm_ik.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
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 Willow Garage 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: Sachin Chitta */
36 
37 #pragma once
38 
39 #include <urdf_model/model.h>
40 #include <urdf/model.h>
41 #include <Eigen/Geometry>
42 #include <Eigen/LU> // provides LU decomposition
43 #include <kdl/chainiksolver.hpp>
44 #include <moveit_msgs/GetPositionFK.h>
45 #include <moveit_msgs/GetPositionIK.h>
46 #include <moveit_msgs/KinematicSolverInfo.h>
47 
49 {
50 static const int NUM_JOINTS_ARM7DOF = 7;
51 
52 static const double IK_EPS = 1e-5;
53 
54 inline double distance(const urdf::Pose& transform)
55 {
56  return sqrt(transform.position.x * transform.position.x + transform.position.y * transform.position.y +
57  transform.position.z * transform.position.z);
58 }
59 
60 inline bool solveQuadratic(const double& a, const double& b, const double& c, double* x1, double* x2)
61 {
62  double discriminant = b * b - 4 * a * c;
63  if (fabs(a) < IK_EPS)
64  {
65  *x1 = -c / b;
66  *x2 = *x1;
67  return true;
68  }
69  // ROS_DEBUG("Discriminant: %f\n",discriminant);
70  if (discriminant >= 0)
71  {
72  *x1 = (-b + sqrt(discriminant)) / (2 * a);
73  *x2 = (-b - sqrt(discriminant)) / (2 * a);
74  return true;
75  }
76  else if (fabs(discriminant) < IK_EPS)
77  {
78  *x1 = -b / (2 * a);
79  *x2 = -b / (2 * a);
80  return true;
81  }
82  else
83  {
84  *x1 = -b / (2 * a);
85  *x2 = -b / (2 * a);
86  return false;
87  }
88 }
89 
90 inline bool solveCosineEqn(const double& a, const double& b, const double& c, double& soln1, double& soln2)
91 {
92  double theta1 = atan2(b, a);
93  double denom = sqrt(a * a + b * b);
94 
95  if (fabs(denom) < IK_EPS) // should never happen, wouldn't make sense but make sure it is checked nonetheless
96  {
97 #ifdef DEBUG
98  std::cout << "denom: " << denom << std::endl;
99 #endif
100  return false;
101  }
102  double rhs_ratio = c / denom;
103  if (rhs_ratio < -1 || rhs_ratio > 1)
104  {
105 #ifdef DEBUG
106  std::cout << "rhs_ratio: " << rhs_ratio << std::endl;
107 #endif
108  return false;
109  }
110  double acos_term = acos(rhs_ratio);
111  soln1 = theta1 + acos_term;
112  soln2 = theta1 - acos_term;
113 
114  return true;
115 }
116 
117 class PR2ArmIK
118 {
119 public:
124  PR2ArmIK();
125  ~PR2ArmIK(){};
126 
134  bool init(const urdf::ModelInterface& robot_model, const std::string& root_name, const std::string& tip_name);
135 
141  void computeIKShoulderPan(const Eigen::Isometry3f& g_in, const double& shoulder_pan_initial_guess,
142  std::vector<std::vector<double> >& solution) const;
143 
149  void computeIKShoulderRoll(const Eigen::Isometry3f& g_in, const double& shoulder_roll_initial_guess,
150  std::vector<std::vector<double> >& solution) const;
151 
157  void getSolverInfo(moveit_msgs::KinematicSolverInfo& info);
158 
162  moveit_msgs::KinematicSolverInfo solver_info_;
163 
164  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
165 
166 private:
167  void addJointToChainInfo(const urdf::JointConstSharedPtr& joint, moveit_msgs::KinematicSolverInfo& info);
168 
169  bool checkJointLimits(const std::vector<double>& joint_values) const;
170 
171  bool checkJointLimits(const double& joint_value, const int& joint_num) const;
172 
173  Eigen::Isometry3f grhs_, gf_, home_inv_;
174 
175  std::vector<double> angle_multipliers_;
176 
177  std::vector<double> solution_;
178 
181 
182  std::vector<double> min_angles_;
183 
184  std::vector<double> max_angles_;
185 
186  std::vector<bool> continuous_joint_;
187 };
188 } // namespace pr2_arm_kinematics
pr2_arm_kinematics::PR2ArmIK::upperarm_elbow_offset_
double upperarm_elbow_offset_
Definition: pr2_arm_ik.h:211
moveit::core
Core components of MoveIt.
Definition: kinematics_base.h:83
pr2_arm_kinematics
Definition: pr2_arm_ik.h:48
pr2_arm_kinematics::PR2ArmIK::addJointToChainInfo
void addJointToChainInfo(const urdf::JointConstSharedPtr &joint, moveit_msgs::KinematicSolverInfo &info)
Definition: pr2_arm_ik.cpp:154
pr2_arm_kinematics::solveCosineEqn
bool solveCosineEqn(const double &a, const double &b, const double &c, double &soln1, double &soln2)
Definition: pr2_arm_ik.h:122
pr2_arm_kinematics::PR2ArmIK::min_angles_
std::vector< double > min_angles_
Definition: pr2_arm_ik.h:214
pr2_arm_kinematics::PR2ArmIK::torso_shoulder_offset_y_
double torso_shoulder_offset_y_
Definition: pr2_arm_ik.h:212
pr2_arm_kinematics::PR2ArmIK::torso_shoulder_offset_x_
double torso_shoulder_offset_x_
Definition: pr2_arm_ik.h:212
pr2_arm_kinematics::PR2ArmIK::angle_multipliers_
std::vector< double > angle_multipliers_
Definition: pr2_arm_ik.h:207
pr2_arm_kinematics::NUM_JOINTS_ARM7DOF
static const int NUM_JOINTS_ARM7DOF
Definition: pr2_arm_ik.h:82
pr2_arm_kinematics::PR2ArmIK::home_inv_
Eigen::Isometry3f home_inv_
Definition: pr2_arm_ik.h:205
pr2_arm_kinematics::solveQuadratic
bool solveQuadratic(const double &a, const double &b, const double &c, double *x1, double *x2)
Definition: pr2_arm_ik.h:92
transform
template Halfspace< double > transform(const Halfspace< double > &a, const Transform3< double > &tf)
pr2_arm_kinematics::PR2ArmIK::shoulder_elbow_offset_
double shoulder_elbow_offset_
Definition: pr2_arm_ik.h:212
pr2_arm_kinematics::PR2ArmIK::computeIKShoulderRoll
void computeIKShoulderRoll(const Eigen::Isometry3f &g_in, const double &shoulder_roll_initial_guess, std::vector< std::vector< double > > &solution) const
compute IK based on an initial guess for the shoulder roll angle.
Definition: pr2_arm_ik.cpp:462
pr2_arm_kinematics::PR2ArmIK::continuous_joint_
std::vector< bool > continuous_joint_
Definition: pr2_arm_ik.h:218
pr2_arm_kinematics::PR2ArmIK::solution_
std::vector< double > solution_
Definition: pr2_arm_ik.h:209
pr2_arm_kinematics::PR2ArmIK::init
bool init(const urdf::ModelInterface &robot_model, const std::string &root_name, const std::string &tip_name)
Initialize the solver by providing a urdf::Model and a root and tip name.
Definition: pr2_arm_ik.cpp:56
pr2_arm_kinematics::PR2ArmIK::grhs_
Eigen::Isometry3f grhs_
Definition: pr2_arm_ik.h:205
pr2_arm_kinematics::PR2ArmIK::getSolverInfo
void getSolverInfo(moveit_msgs::KinematicSolverInfo &info)
get chain information about the arm. This populates the IK query response, filling in joint level inf...
Definition: pr2_arm_ik.cpp:192
pr2_arm_kinematics::PR2ArmIK::computeIKShoulderPan
void computeIKShoulderPan(const Eigen::Isometry3f &g_in, const double &shoulder_pan_initial_guess, std::vector< std::vector< double > > &solution) const
compute IK based on an initial guess for the shoulder pan angle.
Definition: pr2_arm_ik.cpp:197
pr2_arm_kinematics::PR2ArmIK::shoulder_wrist_offset_
double shoulder_wrist_offset_
Definition: pr2_arm_ik.h:211
pr2_arm_kinematics::PR2ArmIK::gf_
Eigen::Isometry3f gf_
Definition: pr2_arm_ik.h:205
model.h
pr2_arm_kinematics::PR2ArmIK::PR2ArmIK
PR2ArmIK()
Inverse kinematics for the PR2 arm.
Definition: pr2_arm_ik.cpp:52
pr2_arm_kinematics::PR2ArmIK::shoulder_upperarm_offset_
double shoulder_upperarm_offset_
Definition: pr2_arm_ik.h:211
pr2_arm_kinematics::PR2ArmIK::torso_shoulder_offset_z_
double torso_shoulder_offset_z_
Definition: pr2_arm_ik.h:212
pr2_arm_kinematics::PR2ArmIK::checkJointLimits
bool checkJointLimits(const std::vector< double > &joint_values) const
Definition: pr2_arm_ik.cpp:766
pr2_arm_kinematics::PR2ArmIK::elbow_wrist_offset_
double elbow_wrist_offset_
Definition: pr2_arm_ik.h:211
pr2_arm_kinematics::IK_EPS
static const double IK_EPS
Definition: pr2_arm_ik.h:84
pr2_arm_kinematics::distance
double distance(const urdf::Pose &transform)
Definition: pr2_arm_ik.h:86
pr2_arm_kinematics::PR2ArmIK::solver_info_
moveit_msgs::KinematicSolverInfo solver_info_
get chain information about the arm.
Definition: pr2_arm_ik.h:194
pr2_arm_kinematics::PR2ArmIK::max_angles_
std::vector< double > max_angles_
Definition: pr2_arm_ik.h:216
pr2_arm_kinematics::PR2ArmIK::~PR2ArmIK
~PR2ArmIK()
Definition: pr2_arm_ik.h:157


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sat Dec 21 2024 03:23:42