chompPlanningGroup.hpp

//
//  chompPlanningGroup.hpp
//  libmove3d-motion
//
//  Created by Jim Mainprice on 04/07/11.
//  Copyright 2011 LAAS/CNRS. All rights reserved.
//

#ifndef CHOMP_PLANNING_GROUP_HPP
#define CHOMP_PLANNING_GROUP_HPP

#include "API/Device/joint.hpp"
#include "API/Device/robot.hpp"

#include "planner/Greedy/CollisionPoint.hpp"

struct ChompJoint
{
  const Joint* move3d_joint_;                                 
  int move3d_joint_index_;                                    
  int move3d_dof_index_;                                       
  int chomp_joint_index_;                                     
  std::string joint_name_;                                    
  std::string link_name_;                                     
  bool wrap_around_;                                          
  bool has_joint_limits_;                                     
  double joint_limit_min_;                                    
  double joint_limit_max_;                                    
  double joint_update_limit_;                                 
};

class ChompPlanningGroup
{
public:
  
  // Creates the planning group
  ChompPlanningGroup(Robot* rob, const std::vector<int>& active_joints );
  
  Robot* robot_;                                            
  std::string name_;                                          
  int num_joints_;                                            
  std::vector<ChompJoint> chomp_joints_;                      
  std::vector<std::string> link_names_;                       
  std::vector<std::string> collision_link_names_;             
  std::vector<CollisionPoint> collision_points_;              
  template <typename Derived>
  void getRandomState(Eigen::MatrixBase<Derived>& state_vec) const;
  
  bool addCollisionPoint(CollisionPoint& collision_point);
  
  void draw() const;
  void draw(std::vector<Eigen::Affine3d>& segment) const;
  void draw(std::vector<std::vector<double> >& segment) const;
};

template <typename Derived>
void ChompPlanningGroup::ChompPlanningGroup::getRandomState(Eigen::MatrixBase<Derived>& state_vec) const
{
  for (int i=0; i<num_joints_; i++)
  {
    double min = chomp_joints_[i].joint_limit_min_;
    double max = chomp_joints_[i].joint_limit_max_;
    
    if (!chomp_joints_[i].has_joint_limits_)
    {
      min = -M_PI/2.0;
      max = M_PI/2.0;
    }

    state_vec(i) = ((((double)rand())/RAND_MAX) * (max-min)) + min;
  }
}

#endif