void testJacobian(std::string group_name, std::string base_link, std::string tip_link)
{
SCOPED_TRACE(group_name + ": " + base_link + " to " + tip_link);
srand ( time(NULL) ); // initialize random seed:
rdf_loader::RDFLoader model_loader("robot_description");
robot_model::RobotModelPtr kinematic_model;
kinematic_model.reset(new robot_model::RobotModel(model_loader.getURDF(),model_loader.getSRDF()));
robot_state::RobotStatePtr kinematic_state;
kinematic_state.reset(new robot_state::RobotState(kinematic_model));
kinematic_state->setToDefaultValues();
const moveit::core::JointModelGroup* joint_state_group = kinematic_state->getRobotModel()->getJointModelGroup(group_name);
std::string link_name = tip_link;
std::vector<double> joint_angles(7,0.0);
geometry_msgs::Point ref_position;
Eigen::MatrixXd jacobian;
Eigen::Vector3d point(0.0,0.0,0.0);
kinematic_state->setJointGroupPositions(joint_state_group, &joint_angles[0]);
ASSERT_TRUE(kinematic_state->getJacobian(joint_state_group, kinematic_state->getRobotModel()->getLinkModel(link_name),point,jacobian));
KDL::Tree tree;
if (!kdl_parser::treeFromUrdfModel(*model_loader.getURDF(), tree))
{
ROS_ERROR("Could not initialize tree object");
}
KDL::Chain kdl_chain;
std::string base_frame(base_link);
std::string tip_frame(tip_link);
if (!tree.getChain(base_frame, tip_frame, kdl_chain))
{
ROS_ERROR("Could not initialize chain object");
}
KDL::ChainJntToJacSolver kdl_solver(kdl_chain);
KDL::Jacobian jacobian_kdl(7);
KDL::JntArray q_in(7);
EXPECT_TRUE(kdl_solver.JntToJac(q_in,jacobian_kdl) >= 0);
unsigned int NUM_TESTS = 10000;
for(unsigned int i=0; i < NUM_TESTS; i++)
{
for(int j=0; j < 7; j++)
{
q_in(j) = gen_rand(-M_PI,M_PI);
joint_angles[j] = q_in(j);
}
EXPECT_TRUE(kdl_solver.JntToJac(q_in,jacobian_kdl) >= 0);
kinematic_state->setJointGroupPositions(joint_state_group, &joint_angles[0]);
EXPECT_TRUE(kinematic_state->getJacobian(joint_state_group, kinematic_state->getRobotModel()->getLinkModel(link_name), point, jacobian));
for(unsigned int k=0; k < 6; k++)
{
for(unsigned int m=0; m < 7; m++)
{
EXPECT_FALSE(NOT_NEAR(jacobian_kdl(k,m),jacobian(k,m),1e-10));
}
}
}
}
bool leatherman::getChainTip(const KDL::Tree &tree, const std::vector<std::string> &segments, std::string chain_root, std::string &chain_tip)
{
KDL::Chain chain;
// compute # of links each link would include if tip of chain
for(size_t i = 0; i < segments.size(); ++i)
{
// create chain with link i as the tip
if (!tree.getChain(chain_root, segments[i], chain))
{
ROS_ERROR("Failed to fetch the KDL chain. This code only supports a set of segments that live within a single kinematic chain. (root: %s, tip: %s)", chain_root.c_str(), segments[i].c_str());
return false;
}
int index;
size_t num_segments_included = 0;
for(size_t j = 0; j < segments.size(); ++j)
{
if(leatherman::getSegmentIndex(chain, segments[j], index))
num_segments_included++;
}
if(num_segments_included == segments.size())
{
chain_tip = segments[i];
return true;
}
}
return false;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "cob_ik_solver");
ros::NodeHandle n;
ros::NodeHandle node;
std::string robot_desc_string;
node.param("/robot_description", robot_desc_string, string());
if (!kdl_parser::treeFromString(robot_desc_string, my_tree)){
ROS_ERROR("Failed to construct kdl tree");
return false;
}
my_tree.getChain("base_link","arm_7_link", chain);
ros::ServiceServer get_ik_service = n.advertiseService("get_ik", ik_solve);
ros::ServiceServer get_constraint_aware_ik_service = n.advertiseService("get_constraint_aware_ik", constraint_aware_ik_solve);
ros::ServiceServer get_ik_solver_info_service = n.advertiseService("get_ik_solver_info", getIKSolverInfo);
ros::ServiceServer get_fk_service = n.advertiseService("get_fk", fk_solve);
ros::ServiceServer get_fk_tcp_service = n.advertiseService("get_fk_tcp", fk_solve_TCP);
ros::ServiceServer get_fk_all_service = n.advertiseService("get_fk_all", fk_solve_all);
ros::ServiceServer get_fk_solver_info_service = n.advertiseService("get_fk_solver_info", getFKSolverInfo);
ROS_INFO("IK Server Running.");
ros::spin();
return 0;
}
void computeRNEDynamicsForChain(KDL::Tree& a_tree, const std::string& rootLink, const std::string& tipLink, KDL::Vector& grav,
std::vector<kdle::JointState>& jointState, std::vector<kdle::SegmentState>& linkState)
{
KDL::Chain achain;
a_tree.getChain(rootLink, tipLink, achain);
KDL::JntArray q(achain.getNrOfJoints());
KDL::JntArray q_dot(achain.getNrOfJoints());
KDL::JntArray q_dotdot(achain.getNrOfJoints());
JntArray torques(achain.getNrOfJoints());
KDL::Wrenches f_ext;
f_ext.resize(achain.getNrOfSegments());
std::cout << endl << endl;
printf("RNE dynamics values \n");
KDL::ChainIdSolver_RNE *rneDynamics = new ChainIdSolver_RNE(achain, -grav);
for (unsigned int i = 0; i < achain.getNrOfJoints(); ++i)
{
q(i) = jointState[i].q;
q_dot(i) = jointState[i].qdot;
q_dotdot(i) = jointState[i].qdotdot;
printf("q, qdot %f, %f\n", q(i), q_dot(i));
}
rneDynamics->CartToJnt(q, q_dot, q_dotdot, f_ext, torques);
for (unsigned int i = 0; i < achain.getNrOfJoints(); ++i)
{
printf("index, q, qdot, torques %d, %f, %f, %f\n", i, q(i), q_dot(i), torques(i));
}
return;
}
TEST(JacobianSolver, solver)
{
srand ( time(NULL) ); // initialize random seed:
rdf_loader::RDFLoader model_loader("robot_description");
robot_model::RobotModelPtr kinematic_model;
kinematic_model.reset(new robot_model::RobotModel(model_loader.getURDF(),model_loader.getSRDF()));
robot_state::RobotStatePtr kinematic_state;
kinematic_state.reset(new robot_state::RobotState(kinematic_model));
kinematic_state->setToDefaultValues();
robot_state::JointStateGroup* joint_state_group = kinematic_state->getJointStateGroup("right_arm");
std::string link_name = "r_wrist_roll_link";
std::vector<double> joint_angles(7,0.0);
geometry_msgs::Point ref_position;
Eigen::MatrixXd jacobian;
Eigen::Vector3d point(0.0,0.0,0.0);
joint_state_group->setVariableValues(joint_angles);
ASSERT_TRUE(joint_state_group->getJacobian(link_name,point,jacobian));
KDL::Tree tree;
if (!kdl_parser::treeFromUrdfModel(*model_loader.getURDF(), tree))
{
ROS_ERROR("Could not initialize tree object");
}
KDL::Chain kdl_chain;
std::string base_frame("torso_lift_link");
std::string tip_frame("r_wrist_roll_link");
if (!tree.getChain(base_frame, tip_frame, kdl_chain))
{
ROS_ERROR("Could not initialize chain object");
}
KDL::ChainJntToJacSolver kdl_solver(kdl_chain);
KDL::Jacobian jacobian_kdl(7);
KDL::JntArray q_in(7);
EXPECT_TRUE(kdl_solver.JntToJac(q_in,jacobian_kdl) >= 0);
unsigned int NUM_TESTS = 1000000;
for(unsigned int i=0; i < NUM_TESTS; i++)
{
for(int j=0; j < 7; j++)
{
q_in(j) = gen_rand(-M_PI,M_PI);
joint_angles[j] = q_in(j);
}
EXPECT_TRUE(kdl_solver.JntToJac(q_in,jacobian_kdl) >= 0);
joint_state_group->setVariableValues(joint_angles);
EXPECT_TRUE(joint_state_group->getJacobian(link_name,point,jacobian));
for(unsigned int k=0; k < 6; k++)
{
for(unsigned int m=0; m < 7; m++)
{
EXPECT_FALSE(NOT_NEAR(jacobian_kdl(k,m),jacobian(k,m),1e-10));
}
}
}
}
static inline YAML::Node extract(const std::string& start_link, const std::string& end_link, const KDL::Tree& robot_tree)
{
KDL::Chain chain;
if (!robot_tree.getChain(start_link, end_link, chain))
{
throw InvalidChain(start_link, end_link);
}
return extract(start_link, end_link, chain);
}
FkLookup::ChainFK::ChainFK(const KDL::Tree& tree, const std::string& root, const std::string& tip):root_name(root),tip_name(tip){
KDL::Chain chain;
tree.getChain(root_name, tip_name, chain);
solver = new KDL::ChainFkSolverPos_recursive(chain);
unsigned int num = chain.getNrOfSegments();
for(unsigned int i = 0; i < num; ++i){
const KDL::Joint &joint = chain.getSegment(i).getJoint();
if (joint.getType() != KDL::Joint::None) joints.insert(std::make_pair(joint.getName(),joints.size()));
}
ROS_ASSERT(joints.size() == chain.getNrOfJoints());
}
bool kdl_urdf_tools::initialize_kinematics_from_urdf(
const std::string &robot_description,
const std::string &root_link,
const std::string &tip_link,
unsigned int &n_dof,
KDL::Chain &kdl_chain,
KDL::Tree &kdl_tree,
urdf::Model &urdf_model)
{
if(robot_description.length() == 0) {
ROS_ERROR("URDF string is empty.");
return false;
}
// Construct an URDF model from the xml string
urdf_model.initString(robot_description);
// Get a KDL tree from the robot URDF
if (!kdl_parser::treeFromUrdfModel(urdf_model, kdl_tree)){
ROS_ERROR("Failed to construct kdl tree");
return false;
}
// Populate the KDL chain
if(!kdl_tree.getChain(root_link, tip_link, kdl_chain))
{
ROS_ERROR_STREAM("Failed to get KDL chain from tree: ");
ROS_ERROR_STREAM(" "<<root_link<<" --> "<<tip_link);
ROS_ERROR_STREAM(" Tree has "<<kdl_tree.getNrOfJoints()<<" joints");
ROS_ERROR_STREAM(" Tree has "<<kdl_tree.getNrOfSegments()<<" segments");
ROS_ERROR_STREAM(" The segments are:");
KDL::SegmentMap segment_map = kdl_tree.getSegments();
KDL::SegmentMap::iterator it;
for( it=segment_map.begin();
it != segment_map.end();
it++ )
{
ROS_ERROR_STREAM( " "<<(*it).first);
}
return false;
}
// Store the number of degrees of freedom of the chain
n_dof = kdl_chain.getNrOfJoints();
return true;
}
bool getKDLChain(const std::string &xml_string, const std::string &root_name, const std::string &tip_name, KDL::Chain &kdl_chain)
{
// create robot chain from root to tip
KDL::Tree tree;
if (!kdl_parser::treeFromString(xml_string, tree))
{
ROS_ERROR("Could not initialize tree object");
return false;
}
if (!tree.getChain(root_name, tip_name, kdl_chain))
{
ROS_ERROR("Could not initialize chain object");
return false;
}
return true;
}
bool getKDLChain(const urdf::ModelInterface& model, const std::string &root_name, const std::string &tip_name, KDL::Chain &kdl_chain)
{
// create robot chain from root to tip
KDL::Tree tree;
if (!kdl_parser::treeFromUrdfModel(model, tree))
{
ROS_ERROR("Could not initialize tree object");
return false;
}
if (!tree.getChain(root_name, tip_name, kdl_chain))
{
ROS_ERROR_STREAM("Could not initialize chain object for base " << root_name << " tip " << tip_name);
return false;
}
return true;
}
KDL::Chain ArmKinematics::readChainFromUrdf(const urdf::ModelInterface& robot_model,
const std::string &root_name, const std::string &tip_name)
{
KDL::Tree tree;
KDL::Chain chain;
if (!kdl_parser::treeFromUrdfModel(robot_model, tree)) {
cout << "Could not parse robot model into a kdl_tree.\n";
throw;
}
if (!tree.getChain(root_name, tip_name, chain)) {
cout << "Could not initialize chain object\n";
throw;
}
return chain;
}
/* Method to load all the values from the parameter server
* @returns true is successful
*/
bool Kinematics::loadModel(const std::string xml) {
urdf::Model robot_model;
KDL::Tree tree;
if (!robot_model.initString(xml)) {
ROS_FATAL("Could not initialize robot model");
return -1;
}
if (!kdl_parser::treeFromString(xml, tree)) {
ROS_ERROR("Could not initialize tree object");
return false;
}
if (!tree.getChain(root_name, tip_name, chain)) {
ROS_ERROR("Could not initialize chain object for root_name %s and tip_name %s",root_name.c_str(), tip_name.c_str());
return false;
}
if (!readJoints(robot_model)) {
ROS_FATAL("Could not read information about the joints");
return false;
}
return true;
}
bool LegKinematics::loadModel(const std::string xml) {
KDL::Tree tree;
KDL::Chain chain;
std::string tip_name_result;
if (!kdl_parser::treeFromString(xml, tree)) {
ROS_ERROR("Could not initialize tree object");
return false;
}
ROS_INFO("Construct tree");
for (int i=0; i<num_legs; i++){
tip_name_result = tip_name + suffixes[i];
if (!tree.getChain(root_name, tip_name_result, chain)) {
ROS_ERROR("Could not initialize chain_%s object", suffixes[i].c_str());
return false;
}
chains_ptr[i] = new KDL::Chain(chain);
}
ROS_INFO("Construct chains");
return true;
}
bool ExcavaROBArmKinematics::loadModel(const std::string xml) {
urdf::Model robot_model;
KDL::Tree tree;
if (!robot_model.initString(xml)) {
ROS_FATAL("Could not initialize robot model");
return false;
}
if (!kdl_parser::treeFromString(xml, tree)) {
ROS_ERROR("Could not initialize tree object");
return false;
}
if (!tree.getChain(root_name_, tip_name_, arm_chain_)) {
ROS_ERROR("Could not initialize chain object");
return false;
}
if (!readJoints(robot_model, tip_name_, root_name_, &num_joints_arm_)) {
ROS_FATAL("Could not read information about the joints");
return false;
}
return true;
}
void LaserJointProjector::configure(const KDL::Tree& tree,
const std::string& root, const std::string& tip)
{
bool success;
success = tree.getChain(root, tip, chain_);
if (!success)
ROS_ERROR("Error extracting chain from [%s] to [%s]\n", root.c_str(), tip.c_str());
KDL::Segment angle_segment("laser_angle_segment",
KDL::Joint("laser_angle_joint", KDL::Joint::RotZ));
KDL::Segment range_segment("laser_range_segment",
KDL::Joint("laser_range_joint", KDL::Joint::TransX));
chain_.addSegment(angle_segment);
chain_.addSegment(range_segment);
for (unsigned int i=0; i < chain_.segments.size(); i++)
{
printf("%2u) %s -> %s\n", i, chain_.segments[i].getName().c_str(),
chain_.segments[i].getJoint().getName().c_str());
}
solver_.reset(new KDL::ChainFkSolverPos_recursive(chain_));
}
SNS_IK::SNS_IK(const std::string& base_link, const std::string& tip_link,
const std::string& URDF_param, double loopPeriod, double eps,
sns_ik::VelocitySolveType type) :
m_initialized(false),
m_eps(eps),
m_loopPeriod(loopPeriod),
m_nullspaceGain(1.0),
m_solvetype(type)
{
ros::NodeHandle node_handle("~");
urdf::Model robot_model;
std::string xml_string;
std::string urdf_xml, full_urdf_xml;
node_handle.param("urdf_param",urdf_xml,URDF_param);
node_handle.searchParam(urdf_xml,full_urdf_xml);
ROS_DEBUG_NAMED("sns_ik","Reading xml file from parameter server");
if (!node_handle.getParam(full_urdf_xml, xml_string)) {
ROS_FATAL_NAMED("sns_ik","Could not load the xml from parameter server: %s", urdf_xml.c_str());
return;
}
node_handle.param(full_urdf_xml, xml_string, std::string());
robot_model.initString(xml_string);
ROS_DEBUG_STREAM_NAMED("sns_ik","Reading joints and links from URDF");
KDL::Tree tree;
if (!kdl_parser::treeFromUrdfModel(robot_model, tree)) {
ROS_FATAL("Failed to extract kdl tree from xml robot description.");
return;
}
if(!tree.getChain(base_link, tip_link, m_chain)) {
ROS_FATAL("Couldn't find chain %s to %s",base_link.c_str(),tip_link.c_str());
}
std::vector<KDL::Segment> chain_segments = m_chain.segments;
boost::shared_ptr<const urdf::Joint> joint;
m_lower_bounds.resize(m_chain.getNrOfJoints());
m_upper_bounds.resize(m_chain.getNrOfJoints());
m_velocity.resize(m_chain.getNrOfJoints());
m_acceleration.resize(m_chain.getNrOfJoints());
m_jointNames.resize(m_chain.getNrOfJoints());
unsigned int joint_num=0;
for(std::size_t i = 0; i < chain_segments.size(); ++i) {
joint = robot_model.getJoint(chain_segments[i].getJoint().getName());
if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) {
double lower=0; //TODO Better default values? Error if these arent found?
double upper=0;
double velocity=0;
double acceleration=0;
if ( joint->type == urdf::Joint::CONTINUOUS ) {
lower=std::numeric_limits<float>::lowest();
upper=std::numeric_limits<float>::max();
} else {
if(joint->safety) {
lower = std::max(joint->limits->lower, joint->safety->soft_lower_limit);
upper = std::min(joint->limits->upper, joint->safety->soft_upper_limit);
} else {
lower = joint->limits->lower;
upper = joint->limits->upper;
}
velocity = std::fabs(joint->limits->velocity);
}
// Checking the Param server for limit modifications
// and acceleration limits
std::string prefix = urdf_xml + "_planning/joint_limits/" + joint->name + "/";
double ul;
if(node_handle.getParam(prefix + "max_position", ul)){
upper = std::min(upper, ul);
}
double ll;
if(node_handle.getParam(prefix + "min_position", ll)){
lower = std::max(lower, ll);
}
double vel;
if(node_handle.getParam(prefix + "max_velocity", vel)){
if (velocity > 0)
velocity = std::min(velocity, std::fabs(vel));
else
velocity = std::fabs(vel);
}
node_handle.getParam(prefix + "max_acceleration", acceleration);
m_lower_bounds(joint_num)=lower;
m_upper_bounds(joint_num)=upper;
m_velocity(joint_num) = velocity;
m_acceleration(joint_num) = std::fabs(acceleration);
m_jointNames[joint_num] = joint->name;
ROS_INFO("sns_ik: Using joint %s lb: %.3f, ub: %.3f, v: %.3f, a: %.3f", joint->name.c_str(),
m_lower_bounds(joint_num), m_upper_bounds(joint_num), m_velocity(joint_num), m_acceleration(joint_num));
joint_num++;
}
}
initialize();
}
bool JacoIKSolver::initFromURDF(const std::string urdf, const std::string root_name,
const std::string tip_name, unsigned int max_iter, std::string error)
{
urdf::Model robot_model;
KDL::Tree tree;
if (!robot_model.initFile(urdf))
{
error += "Could not initialize robot model";
return false;
}
if (!kdl_parser::treeFromFile(urdf, tree))
{
error += "Could not initialize tree object";
return false;
}
if (tree.getSegment(root_name) == tree.getSegments().end())
{
error += "Could not find root link '" + root_name + "'.";
return false;
}
if (tree.getSegment(tip_name) == tree.getSegments().end())
{
error += "Could not find tip link '" + tip_name + "'.";
return false;
}
if (!tree.getChain(root_name, tip_name, chain_))
{
error += "Could not initialize chain object";
return false;
}
// Get the joint limits from the robot model
q_min_.resize(chain_.getNrOfJoints());
q_max_.resize(chain_.getNrOfJoints());
q_seed_.resize(chain_.getNrOfJoints());
joint_names_.resize(chain_.getNrOfJoints());
unsigned int j = 0;
for(unsigned int i = 0; i < chain_.getNrOfSegments(); ++i)
{
const KDL::Joint& kdl_joint = chain_.getSegment(i).getJoint();
if (kdl_joint.getType() != KDL::Joint::None)
{
// std::cout << chain_.getSegment(i).getName() << " -> " << kdl_joint.getName() << " -> " << chain_.getSegment(i + 1).getName() << std::endl;
boost::shared_ptr<const urdf::Joint> joint = robot_model.getJoint(kdl_joint.getName());
if (joint && joint->limits)
{
q_min_(j) = joint->limits->lower;
q_max_(j) = joint->limits->upper;
q_seed_(j) = (q_min_(j) + q_max_(j)) / 2;
}
else
{
q_min_(j) = -1e9;
q_max_(j) = 1e9;
q_seed_(j) = 0;
}
joint_names_[j] = kdl_joint.getName();
++j;
}
}
;
fk_solver_.reset(new KDL::ChainFkSolverPos_recursive(chain_));
ik_vel_solver_.reset(new KDL::ChainIkSolverVel_pinv(chain_));
ik_solver_.reset(new KDL::ChainIkSolverPos_NR_JL(chain_, q_min_, q_max_, *fk_solver_, *ik_vel_solver_, max_iter));
std::cout << "Using normal solver" << std::endl;
jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(chain_));
return true;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "leg_kinematics");
KDL::Tree tree;
KDL::Chain chain;
ros::NodeHandle node;
KDL::ChainFkSolverPos_recursive* fk_solver;
KDL::HP_ChainIkSolverPos_NR_JL *ik_solver_pos;
KDL::ChainIkSolverVel_pinv* ik_solver_vel;
std::string robot_desc_string;
node.param("robot_description", robot_desc_string, std::string("robot_description"));
if (!kdl_parser::treeFromString(robot_desc_string, tree)) {
ROS_ERROR("Failed to construct kdl tree");
return false;
}
if (!tree.getChain("base_link", "tibia_foot_r1", chain)) {
ROS_ERROR("Failed to construct kdl chain");
return false;
}
ROS_INFO("Construct kdl chain");
// unsigned int nj = chain.getNrOfJoints();
// unsigned int js = chain.getNrOfSegments();
// std_msgs::String msg;
// std::stringstream ss;
// ss << "# J: " << nj << " # S: " << js;
// msg.data = ss.str();
// ROS_INFO("Construct kdl tree");
// ROS_INFO("%s", msg.data.c_str()) ;
fk_solver = new KDL::ChainFkSolverPos_recursive(chain);
ik_solver_vel = new KDL::ChainIkSolverVel_pinv(chain);
// Eigen::MatrixXd matrix_Mx = Eigen::MatrixXd::Identity(6,6);
// matrix_Mx(3,3) = 0; matrix_Mx(4,4) = 0; matrix_Mx(5,5) = 0;
// ik_solver_vel -> setWeightTS(matrix_Mx);
KDL::JntArray joint_min(chain.getNrOfJoints());
KDL::JntArray joint_max(chain.getNrOfJoints());
joint_min(0) = -1.57;
joint_min(1) = -1.57;
joint_min(2) = -1.57;
joint_max (0) = 1.57;
joint_max (1) = 1.57;
joint_max (2) = 1.57;
ik_solver_pos = new KDL::HP_ChainIkSolverPos_NR_JL (chain, joint_min, joint_max,
*fk_solver, *ik_solver_vel, 20, 0.0001);
KDL::JntArray q_init(chain.getNrOfJoints());
q_init (0) = 0;
q_init (1) = 0;
q_init (2) = 0;
KDL::JntArray q_out(chain.getNrOfJoints());
// KDL::Segment my_seg = chain.getSegment(3);
// KDL::Frame my_fr;
//
// if (fk_solver.JntToCart(q_init, my_fr) >= 0){
//
// std_msgs::String msg;
// std::stringstream ss;
// ss << "# 0: " << my_fr.p[0] << " # 1: " << my_fr.p[1] << " # 2: " << my_fr.p[2];
// msg.data = ss.str();
// ROS_INFO("Construct kdl tree");
// ROS_INFO("%s", msg.data.c_str()) ;
// }
double x = 0.16509, y = -0.18567, z = 0.053603; //0.16509; -0.18567; 0.053603
KDL::Frame p_in (KDL::Vector(0.12291, -0.085841, -0.16766));
int ik_valid = ik_solver_pos -> CartToJnt(q_init, p_in, q_out);
if (ik_valid >= 0) {
ROS_INFO("\nJoint 1: %f\nJoint 2: %f\nJoint 3: %f\n", q_out(0),q_out(1),q_out(2));
}
else {
ROS_ERROR("IK not found");
}
return 0;
}
bool ik_solve(kinematics_msgs::GetPositionIK::Request &req,
kinematics_msgs::GetPositionIK::Response &res )
{
ROS_INFO("get_ik_service has been called!");
if(req.ik_request.ik_link_name.length() == 0)
my_tree.getChain("base_link","arm_7_link", chain);
else
my_tree.getChain("base_link",req.ik_request.ik_link_name, chain);
unsigned int nj = chain.getNrOfJoints();
JntArray q_min(nj);
JntArray q_max(nj);
//ToDo: get joint limits from robot_description on parameter_server or use /cob_arm_kinematics/get_ik_solver_info!!!
for(int i = 0; i < nj; i+=2)
{
q_min(i) =-2.9670; //adjusted due to cob_description/lbr.urdf.xacro
q_max(i) = 2.9670;
}
for(int i = 1; i < nj; i+=2)
{
q_min(i) =-2.0943951; //adjusted due to cob_description/lbr.urdf.xacro
q_max(i) = 2.0943951;
}
ChainFkSolverPos_recursive fksolver1(chain);//Forward position solver
ChainIkSolverVel_pinv iksolver1v(chain);//Inverse velocity solver
ChainIkSolverPos_NR_JL iksolverpos(chain, q_min, q_max, fksolver1,iksolver1v,1000,1e-6);//Maximum 100 iterations, stop at accuracy 1e-6
JntArray q(nj);
JntArray q_init(nj);
for(int i = 0; i < nj; i++)
q_init(i) = req.ik_request.ik_seed_state.joint_state.position[i];
Frame F_dest;
Frame F_ist;
fksolver1.JntToCart(q_init, F_ist);
tf::PoseMsgToKDL(req.ik_request.pose_stamped.pose, F_dest);
std::cout << "Getting Goal\n";
std::cout << F_dest <<"\n";
std::cout << "Calculated Position out of Seed-Configuration:\n";
std::cout << F_ist <<"\n";
//uhr-fm: here comes the actual IK-solver-call -> could be replaced by analytical-IK-solver (cob)
int ret = iksolverpos.CartToJnt(q_init,F_dest,q);
//res.solution.joint_state.name = req.ik_request.ik_seed_state.joint_state.name;
res.solution.joint_state.name.resize(nj);
res.solution.joint_state.name[0]="arm_1_joint";
res.solution.joint_state.name[1]="arm_2_joint";
res.solution.joint_state.name[2]="arm_3_joint";
res.solution.joint_state.name[3]="arm_4_joint";
res.solution.joint_state.name[4]="arm_5_joint";
res.solution.joint_state.name[5]="arm_6_joint";
res.solution.joint_state.name[6]="arm_7_joint";
res.solution.joint_state.position.resize(nj);
res.solution.joint_state.velocity.resize(nj);
res.solution.joint_state.effort.resize(nj);
if(ret < 0)
{
//res.error_code.val = 0;
res.error_code.val = res.error_code.NO_IK_SOLUTION;
ROS_INFO("Inverse Kinematic found no solution");
std::cout << "RET: " << ret << std::endl;
for(int i = 0; i < nj; i++)
{
res.solution.joint_state.position[i] = q_init(i);
res.solution.joint_state.velocity[i] = 0.0;
res.solution.joint_state.effort[i] = 0.0;
}
}
else
{
ROS_INFO("Inverse Kinematic found a solution");
//res.error_code.val = 1;
res.error_code.val = res.error_code.SUCCESS;
for(int i = 0; i < nj; i++)
{
res.solution.joint_state.position[i] = q(i);
res.solution.joint_state.velocity[i] = 0.0;
res.solution.joint_state.effort[i] = 0.0;
}
}
//std::cout << "q_init\n";
ROS_INFO("q_init: %f %f %f %f %f %f %f", q_init(0), q_init(1), q_init(2), q_init(3), q_init(4), q_init(5), q_init(6));
ROS_INFO("q_out: %f %f %f %f %f %f %f", q(0), q(1), q(2), q(3), q(4), q(5), q(6));
//std::cout << "Solved with " << ret << " as return\n";
//std::cout << q(0) << " " << q(1) << " " << q(2) << " " << q(3) << " " << q(4) << " " << q(5) << " " << q(6) << "\n";
return true;
}