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 #ifndef CHOMP_OPTIMIZER_H_
38 #define CHOMP_OPTIMIZER_H_
39 
48 
49 #include <Eigen/Core>
50 #include <Eigen/StdVector>
51 #include <vector>
52 
53 namespace chomp
54 {
56 {
57 public:
58  ChompOptimizer(ChompTrajectory* trajectory, const planning_scene::PlanningSceneConstPtr& planning_scene,
59  const std::string& planning_group, const ChompParameters* parameters,
60  const moveit::core::RobotState& start_state);
61 
62  virtual ~ChompOptimizer();
63 
68  bool optimize();
69 
70  inline void destroy()
71  {
72  // Nothing for now.
73  }
74 
75  bool isInitialized() const
76  {
77  return initialized_;
78  }
79 
80  bool isCollisionFree() const
81  {
82  return is_collision_free_;
83  }
84 
85 private:
86  inline double getPotential(double field_distance, double radius, double clearence)
87  {
88  double d = field_distance - radius;
89  double potential = 0.0;
90 
91  // three cases below:
92  if (d >= clearence)
93  {
94  potential = 0.0;
95  }
96  else if (d >= 0.0)
97  {
98  double diff = (d - clearence);
99  double gradient_magnitude = diff * clearence; // (diff / clearance)
100  potential = 0.5 * gradient_magnitude * diff;
101  }
102  else // if d < 0.0
103  {
104  potential = -d + 0.5 * clearence;
105  }
106 
107  return potential;
108  }
109  template <typename Derived>
110  void getJacobian(int trajectoryPoint, Eigen::Vector3d& collision_point_pos, std::string& jointName,
111  Eigen::MatrixBase<Derived>& jacobian) const;
112 
113  // void getRandomState(const moveit::core::RobotState& currentState,
114  // const std::string& group_name,
115  // Eigen::VectorXd& state_vec);
116 
117  void setRobotStateFromPoint(ChompTrajectory& group_trajectory, int i);
118 
119  // collision_proximity::CollisionProximitySpace::TrajectorySafety checkCurrentIterValidity();
120 
129 
131  const moveit::core::RobotModelConstPtr& kmodel_;
132  std::string planning_group_;
135  planning_scene::PlanningSceneConstPtr planning_scene_;
141 
142  std::vector<ChompCost> joint_costs_;
143  collision_detection::GroupStateRepresentationPtr gsr_;
145 
146  std::vector<std::vector<std::string> > collision_point_joint_names_;
147  std::vector<EigenSTL::vector_Vector3d> collision_point_pos_eigen_;
148  std::vector<EigenSTL::vector_Vector3d> collision_point_vel_eigen_;
149  std::vector<EigenSTL::vector_Vector3d> collision_point_acc_eigen_;
150  std::vector<std::vector<double> > collision_point_potential_;
151  std::vector<std::vector<double> > collision_point_vel_mag_;
152  std::vector<EigenSTL::vector_Vector3d> collision_point_potential_gradient_;
153  std::vector<EigenSTL::vector_Vector3d> joint_axes_;
154  std::vector<EigenSTL::vector_Vector3d> joint_positions_;
155  Eigen::MatrixXd group_trajectory_backup_;
156  Eigen::MatrixXd best_group_trajectory_;
160 
161  // HMC stuff:
162  Eigen::MatrixXd momentum_;
163  Eigen::MatrixXd random_momentum_;
164  Eigen::VectorXd random_joint_momentum_; // temporary variable
165  std::vector<MultivariateGaussian> multivariate_gaussian_;
167 
168  std::vector<int> state_is_in_collision_;
170  std::vector<std::vector<int> > point_is_in_collision_;
173 
174  Eigen::MatrixXd smoothness_increments_;
175  Eigen::MatrixXd collision_increments_;
176  Eigen::MatrixXd final_increments_;
177 
178  // temporary variables for all functions:
179  Eigen::VectorXd smoothness_derivative_;
180  Eigen::MatrixXd jacobian_;
181  Eigen::MatrixXd jacobian_pseudo_inverse_;
183  Eigen::VectorXd random_state_;
184  Eigen::VectorXd joint_state_velocities_;
185 
186  std::vector<std::string> joint_names_;
187  std::map<std::string, std::map<std::string, bool> > joint_parent_map_;
188 
189  inline bool isParent(const std::string& childLink, const std::string& parentLink) const
190  {
191  if (childLink == parentLink)
192  {
193  return true;
194  }
195 
196  if (joint_parent_map_.find(childLink) == joint_parent_map_.end())
197  {
198  // ROS_ERROR("%s was not in joint parent map! for lookup of %s", childLink.c_str(), parentLink.c_str());
199  return false;
200  }
201  const std::map<std::string, bool>& parents = joint_parent_map_.at(childLink);
202  return (parents.find(parentLink) != parents.end() && parents.at(parentLink));
203  }
204 
205  void registerParents(const moveit::core::JointModel* model);
206  void initialize();
212  void updateFullTrajectory();
213  void debugCost();
214  void handleJointLimits();
215  double getTrajectoryCost();
216  double getSmoothnessCost();
217  double getCollisionCost();
218  void perturbTrajectory();
219  void getRandomMomentum();
220  void updateMomentum();
222  void calculatePseudoInverse();
223  void computeJointProperties(int trajectoryPoint);
225 };
226 }
227 
228 #endif /* CHOMP_OPTIMIZER_H_ */
d
const ChompParameters * parameters_
void getJacobian(int trajectoryPoint, Eigen::Vector3d &collision_point_pos, std::string &jointName, Eigen::MatrixBase< Derived > &jacobian) const
std::map< std::string, std::map< std::string, bool > > joint_parent_map_
unsigned int num_collision_free_iterations_
bool isInitialized() const
Eigen::MatrixXd jacobian_pseudo_inverse_
std::vector< EigenSTL::vector_Vector3d > collision_point_potential_gradient_
ChompTrajectory group_trajectory_
std::vector< std::string > joint_names_
std::vector< MultivariateGaussian > multivariate_gaussian_
const collision_detection::CollisionWorldHybrid * hy_world_
Eigen::MatrixXd smoothness_increments_
collision_detection::GroupStateRepresentationPtr gsr_
unsigned int collision_free_iteration_
const moveit::core::JointModelGroup * joint_model_group_
std::vector< EigenSTL::vector_Vector3d > collision_point_acc_eigen_
std::vector< std::vector< double > > collision_point_vel_mag_
ChompOptimizer(ChompTrajectory *trajectory, const planning_scene::PlanningSceneConstPtr &planning_scene, const std::string &planning_group, const ChompParameters *parameters, const moveit::core::RobotState &start_state)
const moveit::core::RobotModelConstPtr & kmodel_
std::vector< EigenSTL::vector_Vector3d > joint_axes_
planning_scene::PlanningSceneConstPtr planning_scene_
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
double getPotential(double field_distance, double radius, double clearence)
void setRobotStateFromPoint(ChompTrajectory &group_trajectory, int i)
ChompTrajectory * full_trajectory_
Eigen::VectorXd random_joint_momentum_
std::vector< std::vector< std::string > > collision_point_joint_names_
Eigen::MatrixXd jacobian_jacobian_tranpose_
bool isCurrentTrajectoryMeshToMeshCollisionFree() const
Eigen::MatrixXd best_group_trajectory_
const collision_detection::CollisionRobotHybrid * hy_robot_
void updatePositionFromMomentum()
std::vector< EigenSTL::vector_Vector3d > collision_point_vel_eigen_
bool isParent(const std::string &childLink, const std::string &parentLink) const
Eigen::MatrixXd random_momentum_
std::vector< EigenSTL::vector_Vector3d > collision_point_pos_eigen_
std::vector< int > state_is_in_collision_
Eigen::MatrixXd group_trajectory_backup_
robot_trajectory::RobotTrajectory trajectory(rmodel,"right_arm")
Eigen::MatrixXd collision_increments_
moveit::core::RobotState state_
std::vector< std::vector< double > > collision_point_potential_
Eigen::MatrixXd jacobian_
bool isCollisionFree() const
Represents a discretized joint-space trajectory for CHOMP.
moveit::core::RobotState start_state_
std::vector< std::vector< int > > point_is_in_collision_
std::vector< EigenSTL::vector_Vector3d > joint_positions_
std::vector< ChompCost > joint_costs_
void computeJointProperties(int trajectoryPoint)
void registerParents(const moveit::core::JointModel *model)
Eigen::VectorXd smoothness_derivative_
Eigen::MatrixXd final_increments_
Eigen::VectorXd joint_state_velocities_
Eigen::MatrixXd momentum_
Eigen::VectorXd random_state_


chomp_motion_planner
Author(s): Gil Jones , Mrinal Kalakrishnan
autogenerated on Sun Oct 18 2020 13:17:08