bool unigripper_motoman_plant_t::IK_solver(const config_t& effector_config, space_point_t* computed_state, bool set_grasping, const space_point_t* seed_state, bool do_min)
{
double qx,qy,qz,qw;
double x,y,z;
effector_config.get_orientation().get(qx,qy,qz,qw);
effector_config.get_position(x,y,z);
KDL::Chain chain1;
my_tree->getChain("base_link","vacuum_volume",chain1);
KDL::JntArray q(chain1.getNrOfJoints());
KDL::JntArray q_out(chain1.getNrOfJoints());
KDL::JntArray q_min(chain1.getNrOfJoints());
KDL::JntArray q_max(chain1.getNrOfJoints());
std::vector< double > state_var;
if( seed_state != NULL )
state_space->copy_point_to_vector( seed_state, state_var );
else
state_space->copy_to_vector( state_var );
q(0) = state_var[0];
q(1) = state_var[1];
q(2) = state_var[2];
q(3) = state_var[3];
q(4) = state_var[4];
q(5) = state_var[5];
q(6) = state_var[6];
q(7) = state_var[7];
q(8) = state_var[15];
q_min(0) = state_space->get_bounds()[0]->get_lower_bound();
q_min(1) = state_space->get_bounds()[1]->get_lower_bound();
q_min(2) = state_space->get_bounds()[2]->get_lower_bound();
q_min(3) = state_space->get_bounds()[3]->get_lower_bound();
q_min(4) = state_space->get_bounds()[4]->get_lower_bound();
q_min(5) = state_space->get_bounds()[5]->get_lower_bound();
q_min(6) = state_space->get_bounds()[6]->get_lower_bound();
q_min(7) = state_space->get_bounds()[7]->get_lower_bound();
q_min(8) = state_space->get_bounds()[15]->get_lower_bound();
q_max(0) = state_space->get_bounds()[0]->get_upper_bound();
q_max(1) = state_space->get_bounds()[1]->get_upper_bound();
q_max(2) = state_space->get_bounds()[2]->get_upper_bound();
q_max(3) = state_space->get_bounds()[3]->get_upper_bound();
q_max(4) = state_space->get_bounds()[4]->get_upper_bound();
q_max(5) = state_space->get_bounds()[5]->get_upper_bound();
q_max(6) = state_space->get_bounds()[6]->get_upper_bound();
q_max(7) = state_space->get_bounds()[7]->get_upper_bound();
q_max(8) = state_space->get_bounds()[15]->get_upper_bound();
KDL::ChainFkSolverPos_recursive fk_solver(chain1);
KDL::ChainIkSolverVel_pinv ik_solver_vel(chain1);
KDL::ChainIkSolverPos_NR_JL ik_solver(chain1,q_min,q_max,fk_solver,ik_solver_vel,1000,1e-6);
KDL::Frame F(KDL::Rotation::Quaternion(qx,qy,qz,qw),KDL::Vector(x,y,z));
bool ik_res = (ik_solver.CartToJnt(q,F,q_out)>=0);
if(ik_res)
{
std::vector<double> state_vec;
state_vec = state_var;
state_vec[0] = q_out(0);
state_vec[1] = q_out(1);
state_vec[2] = q_out(2);
state_vec[3] = q_out(3);
state_vec[4] = q_out(4);
state_vec[5] = q_out(5);
state_vec[6] = q_out(6);
state_vec[7] = q_out(7);
state_vec[15] = q_out(8);
state_vec[16] = set_grasping ? (double)GRIPPER_CLOSED : GRIPPER_OPEN;
state_space->copy_vector_to_point( state_vec, computed_state );
}
return ik_res;
}
bool KDLKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
const std::vector<double> &ik_seed_state,
double timeout,
std::vector<double> &solution,
const IKCallbackFn &solution_callback,
moveit_msgs::MoveItErrorCodes &error_code,
const std::vector<double> &consistency_limits,
const kinematics::KinematicsQueryOptions &options) const
{
ros::WallTime n1 = ros::WallTime::now();
if(!active_)
{
ROS_ERROR_NAMED("kdl","kinematics not active");
error_code.val = error_code.NO_IK_SOLUTION;
return false;
}
if(ik_seed_state.size() != dimension_)
{
ROS_ERROR_STREAM_NAMED("kdl","Seed state must have size " << dimension_ << " instead of size " << ik_seed_state.size());
error_code.val = error_code.NO_IK_SOLUTION;
return false;
}
if(!consistency_limits.empty() && consistency_limits.size() != dimension_)
{
ROS_ERROR_STREAM_NAMED("kdl","Consistency limits be empty or must have size " << dimension_ << " instead of size " << consistency_limits.size());
error_code.val = error_code.NO_IK_SOLUTION;
return false;
}
KDL::JntArray jnt_seed_state(dimension_);
KDL::JntArray jnt_pos_in(dimension_);
KDL::JntArray jnt_pos_out(dimension_);
KDL::ChainFkSolverPos_recursive fk_solver(kdl_chain_);
KDL::ChainIkSolverVel_pinv_mimic ik_solver_vel(kdl_chain_, joint_model_group_->getMimicJointModels().size(), redundant_joint_indices_.size(), position_ik_);
KDL::ChainIkSolverPos_NR_JL_Mimic ik_solver_pos(kdl_chain_, joint_min_, joint_max_, fk_solver, ik_solver_vel, max_solver_iterations_, epsilon_, position_ik_);
ik_solver_vel.setMimicJoints(mimic_joints_);
ik_solver_pos.setMimicJoints(mimic_joints_);
if ((redundant_joint_indices_.size() > 0) && !ik_solver_vel.setRedundantJointsMapIndex(redundant_joints_map_index_))
{
ROS_ERROR_NAMED("kdl","Could not set redundant joints");
return false;
}
if(options.lock_redundant_joints)
{
ik_solver_vel.lockRedundantJoints();
}
solution.resize(dimension_);
KDL::Frame pose_desired;
tf::poseMsgToKDL(ik_pose, pose_desired);
ROS_DEBUG_STREAM_NAMED("kdl","searchPositionIK2: Position request pose is " <<
ik_pose.position.x << " " <<
ik_pose.position.y << " " <<
ik_pose.position.z << " " <<
ik_pose.orientation.x << " " <<
ik_pose.orientation.y << " " <<
ik_pose.orientation.z << " " <<
ik_pose.orientation.w);
//Do the IK
for(unsigned int i=0; i < dimension_; i++)
jnt_seed_state(i) = ik_seed_state[i];
jnt_pos_in = jnt_seed_state;
unsigned int counter(0);
while(1)
{
// ROS_DEBUG_NAMED("kdl","Iteration: %d, time: %f, Timeout: %f",counter,(ros::WallTime::now()-n1).toSec(),timeout);
counter++;
if(timedOut(n1,timeout))
{
ROS_DEBUG_NAMED("kdl","IK timed out");
error_code.val = error_code.TIMED_OUT;
ik_solver_vel.unlockRedundantJoints();
return false;
}
int ik_valid = ik_solver_pos.CartToJnt(jnt_pos_in, pose_desired, jnt_pos_out);
ROS_DEBUG_NAMED("kdl","IK valid: %d", ik_valid);
if(!consistency_limits.empty())
{
getRandomConfiguration(jnt_seed_state, consistency_limits, jnt_pos_in, options.lock_redundant_joints);
if( (ik_valid < 0 && !options.return_approximate_solution) || !checkConsistency(jnt_seed_state, consistency_limits, jnt_pos_out))
{
ROS_DEBUG_NAMED("kdl","Could not find IK solution: does not match consistency limits");
continue;
}
}
else
{
getRandomConfiguration(jnt_pos_in, options.lock_redundant_joints);
ROS_DEBUG_NAMED("kdl","New random configuration");
for(unsigned int j=0; j < dimension_; j++)
ROS_DEBUG_NAMED("kdl","%d %f", j, jnt_pos_in(j));
if(ik_valid < 0 && !options.return_approximate_solution)
{
ROS_DEBUG_NAMED("kdl","Could not find IK solution");
continue;
}
}
ROS_DEBUG_NAMED("kdl","Found IK solution");
for(unsigned int j=0; j < dimension_; j++)
solution[j] = jnt_pos_out(j);
if(!solution_callback.empty())
solution_callback(ik_pose,solution,error_code);
else
error_code.val = error_code.SUCCESS;
if(error_code.val == error_code.SUCCESS)
{
ROS_DEBUG_STREAM_NAMED("kdl","Solved after " << counter << " iterations");
ik_solver_vel.unlockRedundantJoints();
return true;
}
}
ROS_DEBUG_NAMED("kdl","An IK that satisifes the constraints and is collision free could not be found");
error_code.val = error_code.NO_IK_SOLUTION;
ik_solver_vel.unlockRedundantJoints();
return false;
}
int main(int argc, char *argv[])
{
ros::init(argc, argv, "kinematics_publisher");
ros::NodeHandle nh;
ros::Publisher joint_publisher;
ros::Publisher frame_publisher;
joint_publisher = nh.advertise<sensor_msgs::JointState>("kinematics/joint_states", 1, true);
frame_publisher = nh.advertise<geometry_msgs::PoseStamped>("/frame_result", 1, true);
std::vector<std::string> names;
names.resize(2);
names.at(0) = "base_to_body";
names.at(1) = "body_to_head";
sensor_msgs::JointState my_joints;
my_joints.name = names;
my_joints.position.resize(2);
my_joints.position.at(0) = 0.2;
my_joints.position.at(1) = 0,2;
joint_publisher.publish(my_joints);
ros::spinOnce();
std::cin.get();
bool exit_value;
KDL::Tree my_tree;
urdf::Model my_model;
if (!kdl_parser::treeFromParam("/robot_description", my_tree)){
ROS_ERROR("Failed to construct kdl tree");
return false;
}
std::cout << "Number of links of the urdf:" << my_tree.getNrOfSegments() << std::endl;
std::cout << "Number of Joints of the urdf:" << my_tree.getNrOfJoints() << std::endl;
KDL::Chain my_chain;
my_tree.getChain("world", "webcam", my_chain);
std::cout << "Number of links of the CHAIN:" << my_chain.getNrOfSegments() << std::endl;
std::cout << "Number of Joints of the CHAIN:" << my_chain.getNrOfJoints() << std::endl;
KDL::ChainFkSolverPos_recursive fksolver(my_chain);
KDL::JntArray q(my_chain.getNrOfJoints());
q(0) = my_joints.position.at(0);
q(1) = my_joints.position.at(1);
KDL::Frame webcam;
fksolver.JntToCart(q,webcam);
geometry_msgs::Pose webcam_pose;
tf::poseKDLToMsg(webcam, webcam_pose);
geometry_msgs::PoseStamped webcam_stamped;
webcam_stamped.pose = webcam_pose;
webcam_stamped.header.frame_id = "world";
webcam_stamped.header.stamp = ros::Time::now();
frame_publisher.publish(webcam_stamped);
ros::spinOnce();
KDL::ChainJntToJacSolver jacobian_solver(my_chain);
KDL::Jacobian J;
J.resize(2);
jacobian_solver.JntToJac(q, J);
std::cout << J.data << std::endl;
webcam.M.DoRotY(-20*KDL::deg2rad);
tf::poseKDLToMsg(webcam, webcam_pose);
webcam_stamped.pose = webcam_pose;
webcam_stamped.header.stamp = ros::Time::now();
frame_publisher.publish(webcam_stamped);
ros::spinOnce();
std::cin.get();
KDL::ChainIkSolverVel_pinv ik_solver_vel(my_chain);
KDL::ChainIkSolverPos_NR ik_solver_pos(my_chain,fksolver,ik_solver_vel);
KDL::JntArray q_sol(2);
ik_solver_pos.CartToJnt(q, webcam,q_sol);
my_joints.position.at(0) = q_sol(0);
my_joints.position.at(1) = q_sol(1);
joint_publisher.publish(my_joints);
ros::spinOnce();
std::cin.get();
KDL::Vector g (0.0,0.0,-9.81);
KDL::ChainDynParam my_chain_params(my_chain, g);
KDL::ChainIdSolver_RNE id_solver(my_chain, g);
KDL::JntArray G(2);
my_chain_params.JntToGravity(q_sol,G);
KDL::JntSpaceInertiaMatrix H(2);
my_chain_params.JntToMass(q_sol, H);
KDL::JntArray C(2);
KDL::JntArray q_dot(2);
q_dot(0) = 0.02;
q_dot(1) = 0.04;
my_chain_params.JntToCoriolis(q_sol,q_dot, C);
std::cout << "g(q): " <<G.data << std::endl;
std::cout << "M(q): " <<H.data << std::endl;
std::cout << "C(q,q_dot): " <<C.data << std::endl;
KDL::JntArray q_dotdot(2);
q_dotdot(0) = -0.02;
q_dotdot(1) = 0.02;
KDL::Wrenches f(4);
KDL::JntArray tau(2);
id_solver.CartToJnt(q_sol, q_dot, q_dotdot, f, tau);
std::cout << "Tau: " <<tau.data << std::endl;
std::cin.get();
moveit::planning_interface::MoveGroup my_group("webcam_robot");
my_group.setNamedTarget("look_left");
my_group.move();
return false;
}