avoid_joint_vel_limits_example.cpp
00001 #include "constrained_ik/constrained_ik.h"
00002 #include "constrained_ik/enum_types.h"
00003 #include "constrained_ik/constraints/joint_vel_limits.h"
00004 #include "example_utils.h"
00005 #include <moveit/robot_model_loader/robot_model_loader.h>
00006 #include <moveit/planning_scene/planning_scene.h>
00007 
00008 using namespace constrained_ik;
00009 using namespace constrained_ik::basic_kin;
00010 using namespace Eigen;
00011 
00013 int main(int argc, char *argv[])
00014 {
00015   ros::init(argc, argv, "avoid_joint_vel_limits_example");
00016   Constrained_IK ik;
00017   BasicKin kin;
00018   robot_model_loader::RobotModelLoaderPtr loader;
00019 
00020   // Load example robot model
00021   getExampleRobotData(loader);
00022 
00023   // Initialize kinematic model
00024   kin.init(loader->getModel()->getJointModelGroup("manipulator"));
00025 
00026   // Create constraint
00027   constraints::JointVelLimits *constraint = new constraints::JointVelLimits;
00028   constraint->setDebug(false);
00029   constraint->setWeight(1.0);
00030   constraint->setTimestep(0.1);
00031 
00032   // Add constraint to solver
00033   ik.addConstraint(constraint, constraint_types::Primary);
00034 
00035   // Initialize Solver
00036   ik.init(kin);
00037 
00038   // Calculate IK for sample case
00039   evaluteExampleIK(kin, ik);
00040 
00041   return 0;
00042 }


constrained_ik
Author(s): Chris Lewis , Jeremy Zoss , Dan Solomon
autogenerated on Sat Jun 8 2019 19:23:45