Avoid Singularities Constraint Examples

Available Methods

Add the constraint using c++.

Run the example by executing the command below.

 roslaunch constrained_ik avoid_singularities_example.launch use_yaml:=false 

Files:

• avoid_singularities_example.cpp

  #include "constrained_ik/constrained_ik.h"
  #include "constrained_ik/enum_types.h"
  #include "constrained_ik/constraints/avoid_singularities.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_singularities_example");
    Constrained_IK ik;
    BasicKin kin;
    robot_model_loader::RobotModelLoaderPtr loader;
  
    // Load example robot model
    getExampleRobotData(loader);
  
    // Initialize kinematic model
    kin.init(loader->getModel()->getJointModelGroup("manipulator"));
  
    // Create constraint
    constraints::AvoidSingularities *constraint = new constraints::AvoidSingularities;
    constraint->setDebug(false);
    constraint->setWeight(1.0);
    constraint->setEnableThreshold(0.01);
    constraint->setIgnoreThreshold(1e-5);
  
    // Add constraint to solver
    ik.addConstraint(constraint, constraint_types::Primary);
  
    // Initialize Solver
    ik.init(kin);
  
    // Calculate IK for sample case
    evaluteExampleIK(kin, ik);
  
    return 0;
  }
  

Add the constraint using yaml file.

Run the example by executing the command below.

 roslaunch constrained_ik avoid_singularities_example.launch use_yaml:=true 

Files:

• avoid_singularities_example.yaml

  example_yaml:
    constraints:
    -
      class: constrained_ik/AvoidSingularities
      primary: true
      enable_threshold: 0.01
      ignore_threshold: 1e-5
      weight: 1.0
      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;
  }