Goal Tool Position Constraint Examples

Available Methods

Add the constraint using c++.

Run the example by executing the command below.

 roslaunch constrained_ik goal_tool_position_example.launch use_yaml:=false 

Files:

• goal_tool_position_example.cpp

  #include "constrained_ik/constrained_ik.h"
  #include "constrained_ik/enum_types.h"
  #include "constrained_ik/constraints/tool_position.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, "goal_tool_position_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::ToolPosition *constraint = new constraints::ToolPosition;
    constraint->setDebug(false);
    constraint->setWeight(Eigen::VectorXd::Ones(3));
    constraint->setTolerance(0.001);
  
    // 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 goal_tool_position_example.launch use_yaml:=true 

Files:

• goal_tool_position_example.yaml

  example_yaml:
    constraints:
    -
      class: constrained_ik/ToolPosition
      primary: true
      weight: [1.0, 1.0, 1.0]
      position_tolerance: 0.001
      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;
  }