Avoid Obstacles Constraint Examples

Available Methods

Add the constraint using c++.

Run the example by executing the command below.

 roslaunch constrained_ik avoid_obstacles_example.launch use_yaml:=false 

Files:

• avoid_obstacles_example.cpp

  #include "constrained_ik/constrained_ik.h"
  #include "constrained_ik/enum_types.h"
  #include "constrained_ik/constraints/avoid_obstacles.h"
  #include "example_utils.h"
  #include <moveit/robot_model_loader/robot_model_loader.h>
  #include <moveit/planning_scene/planning_scene.h>
  
  using namespace constrained_ik;
  using namespace constrained_ik::basic_kin;
  using namespace Eigen;
  
  int main(int argc, char *argv[])
  {
    ros::init(argc, argv, "avoid_obstacles_example");
    Constrained_IK ik;
    BasicKin kin;
    robot_model_loader::RobotModelLoaderPtr loader;
    planning_scene::PlanningScenePtr planning_scene;
  
    // Load example robot model
    planning_scene = getExampleRobotData(loader);
  
    // Initialize kinematic model
    kin.init(loader->getModel()->getJointModelGroup("manipulator"));
  
    // Create constraint
    std::vector<std::string> link_names = boost::assign::list_of("upper_arm_link")("wrist_3_link");
    constraints::AvoidObstacles *constraint = new constraints::AvoidObstacles;
    constraint->setAvoidanceLinks(link_names);
    constraint->setAmplitude(link_names[0], 0.01);
    constraint->setAmplitude(link_names[1], 0.01);
    constraint->setMinDistance(link_names[0], 0.01);
    constraint->setMinDistance(link_names[1], 0.01);
    constraint->setAvoidanceDistance(link_names[0], 1.0);
    constraint->setAvoidanceDistance(link_names[1], 1.0);
    constraint->setWeight(link_names[0], 1.0);
    constraint->setWeight(link_names[1], 1.0);
    constraint->setDebug(false);
  
    // Add constraint to solver
    ik.addConstraint(constraint, constraint_types::Primary);
  
    // Initialize Solver
    ik.init(kin);
  
    // Calculate IK for sample case
    evaluteExampleIK(kin, ik, planning_scene);
  
    return 0;
  }
  

Add the constraint using yaml file.

Run the example by executing the command below.

 roslaunch constrained_ik avoid_obstacles_example.launch use_yaml:=true 

Files:

• avoid_obstacles_example.yaml

  example_yaml:
    constraints:
    -
      class: constrained_ik/AvoidObstacles
      primary: true
      link_names:         [upper_arm_link, wrist_3_link]
      amplitude:          [          0.01,         0.01]
      minimum_distance:   [          0.01,         0.01]
      avoidance_distance: [           1.0,          1.0]
      weight:             [             1,            1]
      debug: true
  

• constraints_from_yaml_example.cpp

  #include "constrained_ik/constrained_ik.h"
  #include "constrained_ik/enum_types.h"
  #include "example_utils.h"
  #include <moveit/robot_model_loader/robot_model_loader.h>
  
  using namespace constrained_ik;
  using namespace constrained_ik::basic_kin;
  using namespace Eigen;
  
  int main(int argc, char *argv[])
  {
    ros::init(argc, argv, "constraints_from_yaml_example");
    Constrained_IK ik;
    BasicKin kin;
    robot_model_loader::RobotModelLoaderPtr loader;
    planning_scene::PlanningScenePtr planning_scene;
  
    // Load example robot model
    planning_scene = getExampleRobotData(loader);
  
    // Initialize kinematic model
    kin.init(loader->getModel()->getJointModelGroup("manipulator"));
  
    // Add constraint/constraints from ros parameter server
    std::string param = "example_namespace/constraints";
    ik.addConstraintsFromParamServer(param);
  
    // Initialize Solver
    ik.init(kin);
  
    // Calculate IK for sample case
    evaluteExampleIK(kin, ik, planning_scene);
  
    return 0;
  }