roslaunch constrained_ik goal_tool_orientation_example.launch use_yaml:=false
#include "constrained_ik/constrained_ik.h" #include "constrained_ik/enum_types.h" #include "constrained_ik/constraints/goal_tool_orientation.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_orientation_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::GoalToolOrientation *constraint = new constraints::GoalToolOrientation; constraint->setDebug(false); constraint->setWeight(Eigen::Vector3d::Ones()); constraint->setTolerance(0.009); // 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; }
roslaunch constrained_ik goal_tool_orientation_example.launch use_yaml:=true
example_yaml: constraints: - class: constrained_ik/GoalToolOrientation primary: true weight: [1.0, 1.0, 1.0] orientation_tolerance: 0.009 debug: true
#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; }