moveit2
The MoveIt Motion Planning Framework for ROS 2.
chomp_optimizer.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2009, 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: Mrinal Kalakrishnan */
36 
37 #pragma once
38 
46 
47 #include <Eigen/Core>
48 #include <Eigen/StdVector>
49 #include <vector>
50 
51 namespace chomp
52 {
54 {
55 public:
56  ChompOptimizer(ChompTrajectory* trajectory, const planning_scene::PlanningSceneConstPtr& planning_scene,
57  const std::string& planning_group, const ChompParameters* parameters,
58  const moveit::core::RobotState& start_state);
59 
60  virtual ~ChompOptimizer();
61 
66  bool optimize();
67 
68  inline void destroy()
69  {
70  // Nothing for now.
71  }
72 
73  bool isInitialized() const
74  {
75  return initialized_;
76  }
77 
78  bool isCollisionFree() const
79  {
80  return is_collision_free_;
81  }
82 
83 private:
84  inline double getPotential(double field_distance, double radius, double clearance)
85  {
86  double d = field_distance - radius;
87 
88  if (d >= clearance) // everything is fine
89  {
90  return 0.0;
91  }
92  else if (d >= 0.0) // transition phase, no collision yet
93  {
94  const double diff = (d - clearance);
95  const double gradient_magnitude = diff / clearance;
96  return 0.5 * gradient_magnitude * diff; // 0.5 * (d - clearance)^2 / clearance
97  }
98  else // d < 0.0: collision
99  {
100  return -d + 0.5 * clearance; // linearly increase, starting from 0.5 * clearance
101  }
102  }
103  template <typename Derived>
104  void getJacobian(int trajectoryPoint, Eigen::Vector3d& collision_point_pos, std::string& jointName,
105  Eigen::MatrixBase<Derived>& jacobian) const;
106 
107  // void getRandomState(const moveit::core::RobotState& currentState,
108  // const std::string& group_name,
109  // Eigen::VectorXd& state_vec);
110 
111  void setRobotStateFromPoint(ChompTrajectory& group_trajectory, int i);
112 
113  // collision_proximity::CollisionProximitySpace::TrajectorySafety checkCurrentIterValidity();
114 
115  int num_joints_;
116  int num_vars_free_;
117  int num_vars_all_;
118  int num_collision_points_;
119  int free_vars_start_;
120  int free_vars_end_;
121  int iteration_;
122  unsigned int collision_free_iteration_;
123 
124  ChompTrajectory* full_trajectory_;
125  const moveit::core::RobotModelConstPtr& robot_model_;
126  std::string planning_group_;
127  const ChompParameters* parameters_;
128  ChompTrajectory group_trajectory_;
129  planning_scene::PlanningSceneConstPtr planning_scene_;
131  moveit::core::RobotState start_state_;
132  const moveit::core::JointModelGroup* joint_model_group_;
134 
135  std::vector<ChompCost> joint_costs_;
136  collision_detection::GroupStateRepresentationPtr gsr_;
137  bool initialized_;
138 
139  std::vector<std::vector<std::string> > collision_point_joint_names_;
140  std::vector<EigenSTL::vector_Vector3d> collision_point_pos_eigen_;
141  std::vector<EigenSTL::vector_Vector3d> collision_point_vel_eigen_;
142  std::vector<EigenSTL::vector_Vector3d> collision_point_acc_eigen_;
143  std::vector<std::vector<double> > collision_point_potential_;
144  std::vector<std::vector<double> > collision_point_vel_mag_;
145  std::vector<EigenSTL::vector_Vector3d> collision_point_potential_gradient_;
146  std::vector<EigenSTL::vector_Vector3d> joint_axes_;
147  std::vector<EigenSTL::vector_Vector3d> joint_positions_;
148  Eigen::MatrixXd group_trajectory_backup_;
149  Eigen::MatrixXd best_group_trajectory_;
150  double best_group_trajectory_cost_;
151  int last_improvement_iteration_;
152  unsigned int num_collision_free_iterations_;
153 
154  // HMC stuff:
155  Eigen::MatrixXd momentum_;
156  Eigen::MatrixXd random_momentum_;
157  Eigen::VectorXd random_joint_momentum_; // temporary variable
158  std::vector<MultivariateGaussian> multivariate_gaussian_;
159  double stochasticity_factor_;
160 
161  std::vector<int> state_is_in_collision_;
163  std::vector<std::vector<int> > point_is_in_collision_;
164  bool is_collision_free_;
165  double worst_collision_cost_state_;
166 
167  Eigen::MatrixXd smoothness_increments_;
168  Eigen::MatrixXd collision_increments_;
169  Eigen::MatrixXd final_increments_;
170 
171  // temporary variables for all functions:
172  Eigen::VectorXd smoothness_derivative_;
173  Eigen::MatrixXd jacobian_;
174  Eigen::MatrixXd jacobian_pseudo_inverse_;
175  Eigen::MatrixXd jacobian_jacobian_tranpose_;
176  Eigen::VectorXd random_state_;
177  Eigen::VectorXd joint_state_velocities_;
178 
179  std::vector<std::string> joint_names_;
180  std::map<std::string, std::map<std::string, bool> > joint_parent_map_;
181 
182  inline bool isParent(const std::string& childLink, const std::string& parentLink) const
183  {
184  if (childLink == parentLink)
185  {
186  return true;
187  }
188 
189  if (joint_parent_map_.find(childLink) == joint_parent_map_.end())
190  {
191  // ROS_ERROR("%s was not in joint parent map! for lookup of %s", childLink.c_str(), parentLink.c_str());
192  return false;
193  }
194  const std::map<std::string, bool>& parents = joint_parent_map_.at(childLink);
195  return (parents.find(parentLink) != parents.end() && parents.at(parentLink));
196  }
197 
198  void registerParents(const moveit::core::JointModel* model);
199  void initialize();
200  void calculateSmoothnessIncrements();
201  void calculateCollisionIncrements();
202  void calculateTotalIncrements();
203  void performForwardKinematics();
204  void addIncrementsToTrajectory();
205  void updateFullTrajectory();
206  void debugCost();
207  void handleJointLimits();
208  double getTrajectoryCost();
209  double getSmoothnessCost();
210  double getCollisionCost();
211  void perturbTrajectory();
212  void getRandomMomentum();
213  void updateMomentum();
214  void updatePositionFromMomentum();
215  void calculatePseudoInverse();
216  void computeJointProperties(int trajectoryPoint);
217  bool isCurrentTrajectoryMeshToMeshCollisionFree() const;
218 };
219 } // namespace chomp
bool isCollisionFree() const
ChompOptimizer(ChompTrajectory *trajectory, const planning_scene::PlanningSceneConstPtr &planning_scene, const std::string &planning_group, const ChompParameters *parameters, const moveit::core::RobotState &start_state)
bool isInitialized() const
Represents a discretized joint-space trajectory for CHOMP.
This hybrid collision environment combines FCL and a distance field. Both can be used to calculate co...
A joint from the robot. Models the transform that this joint applies in the kinematic chain....
Definition: joint_model.h:117
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:90
Vec3fX< details::Vec3Data< double > > Vector3d
Definition: fcl_compat.h:89
This namespace includes the central class for representing planning contexts.
d
Definition: setup.py:4