void getKDLChainInfo(kinematics_msgs::KinematicSolverInfo &chain_info)
{
unsigned int nj = chain.getNrOfJoints();
unsigned int nl = chain.getNrOfSegments();
ROS_DEBUG("nj: %d", nj);
ROS_DEBUG("nl: %d", nl);
//---setting up response
//joint_names
for(unsigned int i=0; i<nj; i++)
{
ROS_DEBUG("joint_name[%d]: %s", i, chain.getSegment(i).getJoint().getName().c_str());
chain_info.joint_names.push_back(chain.getSegment(i).getJoint().getName());
}
//limits
//joint limits are only saved in KDL::ChainIkSolverPos_NR_JL iksolverpos -> ik_solve().....but this is not visible here!
/*for(unsigned int i=0; i<nj; i++)
{
chain_info.limits[i].joint_name=chain.getSegment(i).getJoint().getName();
chain_info.limits[i].has_position_limits=
chain_info.limits[i].min_position=
chain_info.limits[i].max_position=
chain_info.limits[i].has_velocity_limits=
chain_info.limits[i].max_velocity=
chain_info.limits[i].has_acceleration_limits=
chain_info.limits[i].max_acceleration=
}*/
//link_names
for(unsigned int i=0; i<nl; i++)
{
chain_info.link_names.push_back(chain.getSegment(i).getName());
}
}
// Get Joint Limits from an URDF model
void getJointLimitsFromModel(const urdf::Model& model,
std::vector<double> &q_min,
std::vector<double> &q_max,
KDL::Chain &kdlArmChain
)
{
boost::shared_ptr<const urdf::Joint> joint;
for(unsigned int i=0; i< kdlArmChain.getNrOfJoints(); ++i)
{
if (kdlArmChain.getSegment(i).getJoint().getType() != KDL::Joint::None)
{
joint = model.getJoint(kdlArmChain.getSegment(i).getJoint().getName());
if ( joint->type != urdf::Joint::CONTINUOUS )
{
q_min.at(i) = joint->limits->lower;
q_max.at(i) = joint->limits->upper;
}
else
{
q_min.at(i) = -3.14/2.0;
q_max.at(i) = 3.14/2.0;
}
}
}
}
ChainKinematics::ChainKinematics(const KDL::Chain chain, const double lambda) :
kinChain_(chain),
Njnt_(chain.getNrOfJoints()),
Nsegment_(chain.getNrOfSegments()) {
initialize(lambda);
}
KDL::Wrenches idynChainGet_usingKDL_aux(iCub::iDyn::iDynChain & idynChain, iCub::iDyn::iDynInvSensor & idynSensor,yarp::sig::Vector & ddp0)
{
yarp::sig::Vector q,dq,ddq;
q = idynChain.getAng();
dq = idynChain.getDAng();
ddq = idynChain.getD2Ang();
KDL::Chain kdlChain;
std::vector<std::string> la_joints;
la_joints.push_back("left_shoulder_pitch");
la_joints.push_back("left_shoulder_roll");
la_joints.push_back("left_arm_ft_sensor");
la_joints.push_back("left_shoulder_yaw");
la_joints.push_back("left_elbow");
la_joints.push_back("left_wrist_prosup");
la_joints.push_back("left_wrist_pitch");
la_joints.push_back("left_wrist_yaw");
idynSensorChain2kdlChain(idynChain,idynSensor,kdlChain,la_joints,la_joints);
std::cout << kdlChain << std::endl;
int nj = idynChain.getN();
//cout << "idynChainGet_usingKDL_aux with sensor" << " nrJoints " << kdlChain.getNrOfJoints() << " nrsegments " << kdlChain.getNrOfSegments() << endl;
assert(nj==kdlChain.getNrOfJoints());
assert(nj+1==kdlChain.getNrOfSegments());
KDL::JntArray jointpositions = KDL::JntArray(nj);
KDL::JntArray jointvel = KDL::JntArray(nj);
KDL::JntArray jointacc = KDL::JntArray(nj);
KDL::JntArray torques = KDL::JntArray(nj);
for(unsigned int i=0;i<nj;i++){
jointpositions(i)=q[i];
jointvel(i) = dq[i];
jointacc(i) = ddq[i];
}
KDL::Wrenches f_ext(nj+1);
KDL::Wrenches f_int(nj+1);
KDL::Vector grav_kdl;
idynVector2kdlVector(idynChain.getH0().submatrix(0,2,0,2).transposed()*ddp0,grav_kdl);
KDL::ChainIdSolver_RNE_IW neSolver = KDL::ChainIdSolver_RNE_IW(kdlChain,-grav_kdl);
int ret = neSolver.CartToJnt_and_internal_wrenches(jointpositions,jointvel,jointacc,f_ext,torques,f_int);
assert(ret >= 0);
return f_int;
}
void getKDLChainInfo(const KDL::Chain &chain,
kinematics_msgs::KinematicSolverInfo &chain_info)
{
int i=0; // segment number
while(i < (int)chain.getNrOfSegments())
{
chain_info.link_names.push_back(chain.getSegment(i).getName());
i++;
}
}
int Kinematics::getKDLSegmentIndex(const std::string &name) {
int i=0;
while (i < (int)chain.getNrOfSegments()) {
if (chain.getSegment(i).getName() == name) {
return i+1;
}
i++;
}
return -1;
}
int ExcavaROBArmKinematics::getKDLSegmentIndex(const std::string &name) {
int i=0;
while (i < (int) arm_chain_.getNrOfSegments()) {
if (arm_chain_.getSegment(i).getName() == name) {
return i + 1;
}
i++;
}
return -1;
}
sensor_msgs::JointState init_message(const KDL::Chain &chain) {
sensor_msgs::JointState msg;
for (unsigned int i=0; i < chain.getNrOfSegments(); ++i) {
KDL::Segment segment = chain.getSegment(i);
KDL::Joint joint = segment.getJoint();
if (joint.getType() == KDL::Joint::JointType::None) continue;
msg.name.push_back(joint.getName());
msg.position.push_back(0);
}
return msg;
}
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());
}
void printKDLchain(std::string s,const KDL::Chain & kdlChain)
{
std::cout << s << std::endl;
for(int p=0; p < (int)kdlChain.getNrOfSegments(); p++)
{
std::cout << "Segment " << p << ":" << std::endl;
KDL::Segment seg = kdlChain.getSegment(p);
KDL::Frame fra = seg.getFrameToTip();
std::cout << seg << std::endl;
std::cout << fra << std::endl;
}
}
bool leatherman::getSegmentIndex(const KDL::Chain &c, std::string name, int &index)
{
for(size_t j = 0; j < c.getNrOfSegments(); ++j)
{
if(c.getSegment(j).getName().compare(name) == 0)
{
index = j;
return true;
}
}
return false;
}
KDL::Chain kukaControl::LWR_knife() {
KDL::Chain chain;
// base
chain.addSegment(
// KDL::Segment(KDL::Joint(KDL::Joint::None),KDL::Frame::DH_Craig1989(0, 0, 0.33989, 0)));
KDL::Segment(KDL::Joint(KDL::Joint::None),
KDL::Frame::DH_Craig1989(0, 0, 0.34, 0)));
// joint 1
chain.addSegment(
KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
KDL::Frame::DH_Craig1989(0, -M_PI_2, 0, 0)));
// joint 2
chain.addSegment(
// KDL::Segment(KDL::Joint(KDL::Joint::RotZ),KDL::Frame::DH_Craig1989(0, M_PI_2, 0.40011, 0)));
KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
KDL::Frame::DH_Craig1989(0, M_PI_2, 0.40, 0)));
// joint 3
chain.addSegment(
KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
KDL::Frame::DH_Craig1989(0, M_PI_2, 0, 0)));
// joint 4
chain.addSegment(
KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
KDL::Frame::DH_Craig1989(0, -M_PI_2, 0.4000, 0)));
// joint 5
chain.addSegment(
KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
KDL::Frame::DH_Craig1989(0, -M_PI_2, 0, 0)));
// joint 6
chain.addSegment(
KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
KDL::Frame::DH_Craig1989(0, M_PI_2, 0, 0)));
// joint 7 (with flange adapter)
/* chain.addSegment(
KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
KDL::Frame::DH_Craig1989(0, 0, 0.12597, 0)));*/
// for HSC
chain.addSegment(
KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
KDL::Frame::DH_Craig1989(0, 0, 0.12597 + 0.41275, 0)));
KDL::Frame ee = KDL::Frame(KDL::Vector(0.0625, -0.0925, -0.07))
* KDL::Frame(
KDL::Rotation::EulerZYX(32.0 * M_PI / 180, 0.0,
54.18 * M_PI / 180));
chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::None), ee));
return chain;
}
IkSolver::IkSolver(const KDL::Chain& chain)
{
const std::string root_name = chain.getSegment(0).getName();
const std::string tip_name = chain.getSegment(chain.getNrOfSegments() - 1).getName();
KDL::Tree tree("root");
tree.addChain(chain, "root");
std::vector<std::string> endpoint_names(1, tip_name);
std::vector<EndpointCoupling> endpoint_couplings(1, Pose);
init(tree, endpoint_names, endpoint_couplings);
}
int getKDLSegmentIndex(const KDL::Chain &chain,
const std::string &name)
{
int i=0; // segment number
while(i < (int)chain.getNrOfSegments())
{
if(chain.getSegment(i).getName() == name)
{
return i+1;
}
i++;
}
return -1;
}
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;
}
void RobotAlgos::Test(void)
{
//Definition of a kinematic chain & add segments to the chain
KDL::Chain chain;
chain.addSegment(Segment(Joint(Joint::RotZ),Frame(Vector(0.0,0.0,1.020))));
chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.480))));
chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.645))));
chain.addSegment(Segment(Joint(Joint::RotZ)));
chain.addSegment(Segment(Joint(Joint::RotX),Frame(Vector(0.0,0.0,0.120))));
chain.addSegment(Segment(Joint(Joint::RotZ)));
// Create solver based on kinematic chain
ChainFkSolverPos_recursive fksolver = ChainFkSolverPos_recursive(chain);
// Create joint array
unsigned int nj = chain.getNrOfJoints();
KDL::JntArray jointpositions = JntArray(nj);
// Assign some values to the joint positions
for(unsigned int i=0;i<nj;i++){
float myinput;
printf ("Enter the position of joint %i: ",i);
scanf ("%e",&myinput);
jointpositions(i)=(double)myinput;
}
// Create the frame that will contain the results
KDL::Frame cartpos;
// Calculate forward position kinematics
int kinematics_status;
kinematics_status = fksolver.JntToCart(jointpositions,cartpos);
if(kinematics_status>=0){
std::cout << cartpos <<std::endl;
printf("%s \n","Succes, thanks KDL!");
}else{
printf("%s \n","Error: could not calculate forward kinematics :(");
}
//Creation of the solvers:
ChainFkSolverPos_recursive fksolver1(chain);//Forward position solver
ChainIkSolverVel_pinv iksolver1v(chain);//Inverse velocity solver
ChainIkSolverPos_NR iksolver1(chain,fksolver1,iksolver1v,100,1e-6);//Maximum 100 iterations, stop at accuracy 1e-6
//Creation of jntarrays:
JntArray result(chain.getNrOfJoints());
JntArray q_init(chain.getNrOfJoints());
//Set destination frame
Frame F_dest=cartpos;
iksolver1.CartToJnt(q_init,F_dest,result);
for(unsigned int i=0;i<nj;i++){
printf ("Axle %i: %f \n",i,result(i));
}
}
int main()
{
KDLCollada kdlCollada;
vector <KDL::Chain> kinematicsModels;
const string filename = "puma.dae"; // loading collada kinematics model and converting it to kdl serial chain
if (!kdlCollada.load(COLLADA_MODELS_PATH + filename, kinematicsModels))
{
cout << "Failed to import " << filename;
return 0;
}
cout << "Imported " << kinematicsModels.size() << " kinematics chains" << endl;
for (unsigned int i = 0; i < kinematicsModels.size(); i++) // parsing output kdl serail chain
{
KDL::Chain chain = kinematicsModels[i];
cout << "Chain " << i << " has " << chain.getNrOfSegments() << " segments" << endl;
for (unsigned int u = 0; u < chain.getNrOfSegments(); u++)
{
KDL::Segment segment = chain.segments[u];
string segmentName = segment.getName();
cout << "Segment " << segmentName << " :" <<endl;
KDL::Frame f_tip = segment.getFrameToTip();
KDL::Vector rotAxis = f_tip.M.GetRot();
double rotAngle = f_tip.M.GetRotAngle(rotAxis);
KDL::Vector trans = f_tip.p;
cout << " frame: rotation " << rotAxis.x() << " " << rotAxis.y() << " " << rotAxis.z() << " " << rotAngle * 180 / M_PI << endl;
cout << " frame: translation " << trans.x() << " " << trans.y() << " " << trans.z() << endl;
KDL::RigidBodyInertia inertia = segment.getInertia();
KDL::Joint joint = segment.getJoint();
string jointName = joint.getName();
string jointType = joint.getTypeName();
KDL::Vector jointAxis = joint.JointAxis();
KDL::Vector jointOrigin = joint.JointOrigin();
cout << " joint name: " << jointName << endl;
cout << " type: " << jointType << endl;
cout << " axis: " << jointAxis.x() << " " <<jointAxis.y() << " " << jointAxis.z() << endl;
cout << " origin: " << jointOrigin.x() << " " << jointOrigin.y() << " " << jointOrigin.z() << endl;
}
}
return 0;
}
TEST_F(PR2IKTest, SingleRowRotation)
{
YAML::Node node = YAML::LoadFile("pr2_left_arm_scope.yaml");
ASSERT_NO_THROW(node.as< giskard_core::ScopeSpec >());
giskard_core::ScopeSpec scope_spec = node.as<giskard_core::ScopeSpec>();
ASSERT_NO_THROW(giskard_core::generate(scope_spec));
giskard_core::Scope scope = giskard_core::generate(scope_spec);
ASSERT_TRUE(scope.has_double_expression("pr2_rot_x"));
KDL::Expression<double>::Ptr exp = scope.find_double_expression("pr2_rot_x");
ASSERT_TRUE(exp.get());
std::string base = "base_link";
std::string tip = "l_wrist_roll_link";
KDL::Chain chain;
ASSERT_TRUE(tree.getChain(base, tip, chain));
ASSERT_EQ(chain.getNrOfJoints(), exp->number_of_derivatives());
boost::shared_ptr<KDL::ChainJntToJacSolver> jac_solver;
jac_solver = boost::shared_ptr<KDL::ChainJntToJacSolver>(
new KDL::ChainJntToJacSolver(chain));
for(int i=-11; i<12; ++i)
{
std::vector<double> exp_in;
KDL::JntArray solver_in(exp->number_of_derivatives());
for(size_t j=0; j<exp->number_of_derivatives(); ++j)
{
double value = 0.1*i;
exp_in.push_back(value);
solver_in(j) = value;
}
exp->setInputValues(exp_in);
exp->value();
KDL::Jacobian solver_jac(chain.getNrOfJoints());
ASSERT_GE(jac_solver->JntToJac(solver_in, solver_jac), 0.0);
for (size_t j=0; j<chain.getNrOfJoints(); ++j)
EXPECT_NEAR(solver_jac(3,j), exp->derivative(j), KDL::epsilon);
}
}
TEST_F(PR2IKTest, SingleExpression)
{
YAML::Node node = YAML::LoadFile("pr2_left_arm_single_expression.yaml");
ASSERT_NO_THROW(node.as<giskard_core::FrameSpecPtr>());
giskard_core::FrameSpecPtr spec = node.as<giskard_core::FrameSpecPtr>();
ASSERT_NO_THROW(spec->get_expression(giskard_core::Scope()));
KDL::Expression<KDL::Frame>::Ptr exp = spec->get_expression(giskard_core::Scope());
ASSERT_TRUE(exp.get());
std::string base = "torso_lift_link";
std::string tip = "l_wrist_roll_link";
KDL::Chain chain;
ASSERT_TRUE(tree.getChain(base, tip, chain));
ASSERT_EQ(chain.getNrOfJoints(), exp->number_of_derivatives());
boost::shared_ptr<KDL::ChainJntToJacSolver> jac_solver;
jac_solver = boost::shared_ptr<KDL::ChainJntToJacSolver>(
new KDL::ChainJntToJacSolver(chain));
for(int i=-11; i<12; ++i)
{
std::vector<double> exp_in;
KDL::JntArray solver_in(exp->number_of_derivatives());
for(size_t j=0; j<exp->number_of_derivatives(); ++j)
{
double value = 0.1*i;
exp_in.push_back(value);
solver_in(j) = value;
}
exp->setInputValues(exp_in);
exp->value();
KDL::Jacobian exp_jac(exp->number_of_derivatives());
for(size_t j=0; j<exp_jac.columns(); ++j)
exp_jac.setColumn(j, exp->derivative(j));
KDL::Jacobian solver_jac(chain.getNrOfJoints());
ASSERT_GE(jac_solver->JntToJac(solver_in, solver_jac), 0.0);
EXPECT_TRUE(KDL::Equal(solver_jac, exp_jac));
}
}
KDL::Chain kukaControl::LWR_HSC_nozzle_RCM_used() {
KDL::Chain chain;
// base
chain.addSegment(
// KDL::Segment(KDL::Joint(KDL::Joint::None),KDL::Frame::DH_Craig1989(0, 0, 0.33989, 0)));
KDL::Segment(KDL::Joint(KDL::Joint::None),
KDL::Frame::DH_Craig1989(0, 0, 0.34, 0)));
//KDL::Frame::DH_Craig1989(0, M_PI_2, 0.36, 0)));
// joint 1
chain.addSegment(
KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
KDL::Frame::DH_Craig1989(0, -M_PI_2, 0, 0)));
// joint 2
chain.addSegment(
// KDL::Segment(KDL::Joint(KDL::Joint::RotZ),KDL::Frame::DH_Craig1989(0, M_PI_2, 0.40011, 0)));
KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
KDL::Frame::DH_Craig1989(0, M_PI_2, 0.40, 0)));
//KDL::Frame::DH_Craig1989(0, M_PI_2, 0.42, 0)));
// joint 3
chain.addSegment(
KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
KDL::Frame::DH_Craig1989(0, M_PI_2, 0, 0)));
// joint 4
chain.addSegment(
KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
KDL::Frame::DH_Craig1989(0, -M_PI_2, 0.4000, 0)));
// joint 5
chain.addSegment(
KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
KDL::Frame::DH_Craig1989(0, -M_PI_2, 0, 0)));
// joint 6
chain.addSegment(
KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
KDL::Frame::DH_Craig1989(0, M_PI_2, 0, 0)));
// joint 7 (with flange adapter)
/* chain.addSegment(
KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
KDL::Frame::DH_Craig1989(0, 0, 0.12597, 0)));*/
// for HSC
chain.addSegment(
KDL::Segment(KDL::Joint(KDL::Joint::RotZ),
KDL::Frame::DH_Craig1989(0, 0,
0.12597 + Dist_from_EndeffectorToRcmPoint, 0)));
return chain;
}
void ArmKinematics::initInternals(const KDL::Chain& chain)
{
dof_ = chain.getNrOfJoints();
q_tmp_.resize(dof_);
deleteInternals();
fk_solver_ = new KDL::ChainFkSolverPos_recursive(chain);
ik_solver_ = new KDL::ChainIkSolverVel_wdls(chain);
}
bool addBaseTransformation(const KDL::Chain & old_chain, KDL::Chain & new_chain, KDL::Frame H_new_old)
{
new_chain = KDL::Chain();
for(unsigned int i=0; i<old_chain.getNrOfSegments(); i++) {
KDL::Segment segm;
segm = old_chain.getSegment(i);
//if is not the first segment add normally the segment
if( i != 0 ) {
new_chain.addSegment(segm);
} else {
//otherwise modify the segment before adding it
KDL::Segment new_segm;
KDL::Joint new_joint, old_joint;
old_joint = segm.getJoint();
KDL::Joint::JointType new_type;
switch(old_joint.getType()) {
case KDL::Joint::RotAxis:
case KDL::Joint::RotX:
case KDL::Joint::RotY:
case KDL::Joint::RotZ:
new_type = KDL::Joint::RotAxis;
break;
case KDL::Joint::TransAxis:
case KDL::Joint::TransX:
case KDL::Joint::TransY:
case KDL::Joint::TransZ:
new_type = KDL::Joint::TransAxis;
break;
case KDL::Joint::None:
default:
new_type = KDL::Joint::None;
}
//check !
new_joint = KDL::Joint(old_joint.getName(),H_new_old*old_joint.JointOrigin(),H_new_old.M*old_joint.JointAxis(),new_type);
new_segm = KDL::Segment(segm.getName(),new_joint,H_new_old*segm.getFrameToTip(),segm.getInertia());
new_chain.addSegment(new_segm);
}
}
return true;
}
bool OrientationConstraint::initialize(const arm_navigation_msgs::OrientationConstraint& constraint,
const KDL::Chain& chain)
{
constraint_ = constraint;
link_id_ = -1;
for (unsigned int i=0; i<chain.getNrOfSegments(); ++i)
{
if (constraint_.link_name == chain.getSegment(i).getName())
{
link_id_ = i;
break;
}
}
if (link_id_ == -1)
{
ROS_ERROR("OrientationConstraint: couldn't find link %s in chain.", constraint_.link_name.c_str());
return false;
}
return true;
}
bool ExcavaROBArmKinematics::getPositionFK(excavaROB_arm_kinematics_msgs::GetPositionFK::Request &request,
excavaROB_arm_kinematics_msgs::GetPositionFK::Response &response) {
KDL::Frame p_out;
KDL::JntArray jnt_pos_in;
geometry_msgs::PoseStamped pose;
tf::Stamped<tf::Pose> tf_pose;
if (num_joints_arm_ != request.fk_link_names.size()) {
ROS_ERROR("The request has not the same dimension of the arm_chain joints.");
return false;
}
jnt_pos_in.resize(num_joints_arm_);
for (unsigned int i = 0; i < num_joints_arm_; i++) {
int jnt_index = getJointIndex(request.joint_state.name[i]);
if (jnt_index >= 0)
jnt_pos_in(jnt_index) = request.joint_state.position[i];
}
response.pose_stamped.resize(num_joints_arm_);
response.fk_link_names.resize(num_joints_arm_);
bool valid = true;
for (unsigned int i = 0; i < num_joints_arm_; i++) {
int segmentIndex = getKDLSegmentIndex(request.fk_link_names[i]);
ROS_DEBUG("End effector index: %d", segmentIndex);
ROS_DEBUG("Chain indices: %d", arm_chain_.getNrOfSegments());
if (fk_solver_->JntToCart(jnt_pos_in, p_out, segmentIndex) >= 0) {
tf_pose.frame_id_ = root_name_;
tf_pose.stamp_ = ros::Time();
tf::PoseKDLToTF(p_out, tf_pose);
try {
tf_listener_.transformPose(request.header.frame_id, tf_pose, tf_pose);
} catch (...) {
ROS_ERROR("ExcavaROB Kinematics: Could not transform FK pose to frame: %s", request.header.frame_id.c_str());
response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
return false;
}
tf::poseStampedTFToMsg(tf_pose, pose);
response.pose_stamped[i] = pose;
response.fk_link_names[i] = request.fk_link_names[i];
response.error_code.val = response.error_code.SUCCESS;
} else {
ROS_ERROR("ExcavaROB Kinematics: Could not compute FK for %s", request.fk_link_names[i].c_str());
response.error_code.val = response.error_code.NO_FK_SOLUTION;
valid = false;
}
}
return true;
}
bool fk_solve_TCP(kinematics_msgs::GetPositionFK::Request &req,
kinematics_msgs::GetPositionFK::Response &res )
{
ROS_INFO("[TESTING]: get_fk_TCP_service has been called!");
unsigned int nj = chain.getNrOfJoints();
ChainFkSolverPos_recursive fksolver1(chain);//Forward position solver
JntArray conf(nj);
geometry_msgs::PoseStamped pose;
tf::Stamped<tf::Pose> tf_pose;
for(int i = 0; i < nj; i++)
conf(i) = req.robot_state.joint_state.position[i];
Frame F_ist;
res.pose_stamped.resize(1);
res.fk_link_names.resize(1);
if(fksolver1.JntToCart(conf, F_ist) >= 0)
{
std::cout << "Calculated Position out of Configuration:\n";
std::cout << F_ist <<"\n";
//TODO: fill out response!!!
tf_pose.frame_id_ = "base_link";//root_name_;
tf_pose.stamp_ = ros::Time();
tf::PoseKDLToTF(F_ist,tf_pose);
try
{
//tf_.transformPose(req.header.frame_id,tf_pose,tf_pose);
}
catch(...)
{
ROS_ERROR("Could not transform FK pose to frame: %s",req.header.frame_id.c_str());
res.error_code.val = res.error_code.FRAME_TRANSFORM_FAILURE;
return false;
}
tf::poseStampedTFToMsg(tf_pose,pose);
res.pose_stamped[0] = pose;
res.fk_link_names[0] = "arm_7_link";
res.error_code.val = res.error_code.SUCCESS;
}
else
{
ROS_ERROR("Could not compute FK for arm_7_link");
res.error_code.val = res.error_code.NO_FK_SOLUTION;
}
return true;
}
VirtualForcePublisher()
{
ros::NodeHandle n_tilde("~");
ros::NodeHandle n;
// subscribe to joint state
joint_state_sub_ = n.subscribe("joint_states", 1, &VirtualForcePublisher::callbackJointState, this);
wrench_stamped_pub_ = n.advertise<geometry_msgs::WrenchStamped>("wrench", 1);
// set publish frequency
double publish_freq;
n_tilde.param("publish_frequency", publish_freq, 50.0);
publish_interval_ = ros::Duration(1.0/std::max(publish_freq,1.0));
//set time constant of low pass filter
n_tilde.param("time_constant", t_const_, 0.3);
// set root and tip
n_tilde.param("root", root, std::string("torso_lift_link"));
n_tilde.param("tip", tip, std::string("l_gripper_tool_frame"));
// load robot model
urdf::Model robot_model;
robot_model.initParam("robot_description");
KDL::Tree kdl_tree;
kdl_parser::treeFromUrdfModel(robot_model, kdl_tree);
kdl_tree.getChain(root, tip, chain_);
jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(chain_));
jnt_pos_.resize(chain_.getNrOfJoints());
jacobian_.resize(chain_.getNrOfJoints());
for (size_t i=0; i<chain_.getNrOfSegments(); i++) {
if (chain_.getSegment(i).getJoint().getType() != KDL::Joint::None) {
std::cerr << "kdl_chain(" << i << ") " << chain_.getSegment(i).getJoint().getName().c_str() << std::endl;
}
}
}
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 PositionConstraint::initialize(const arm_navigation_msgs::PositionConstraint& constraint,
const KDL::Chain& chain)
{
constraint_ = constraint;
link_id_ = -1;
for (unsigned int i=0; i<chain.getNrOfSegments(); ++i)
{
if (constraint_.link_name == chain.getSegment(i).getName())
{
link_id_ = i;
break;
}
}
if (link_id_ == -1)
{
ROS_ERROR("CIK PositionConstraint: couldn't find link %s in chain.", constraint_.link_name.c_str());
return false;
}
rosPointToKdlVector(constraint_.target_point_offset, offset_);
rosPointToKdlVector(constraint_.position, desired_location_);
if (constraint_.constraint_region_shape.type != arm_navigation_msgs::Shape::BOX ||
constraint_.constraint_region_shape.dimensions.size() != 3)
{
ROS_ERROR("CIK PositionConstraint: we only handle 3-d BOX constraints currently.");
return false;
}
KDL::Rotation constraint_rotation;
rosQuaternionToKdlRotation(constraint_.constraint_region_orientation, constraint_rotation);
constraint_rotation_inverse_ = constraint_rotation.Inverse();
kdlRotationToEigenMatrix3d(constraint_rotation_inverse_, constraint_rotation_inverse_eigen_);
return true;
}
void controllerStateCallback(const pr2_controllers_msgs::JointTrajectoryControllerState::ConstPtr& msg)
{
unsigned int nj = chain.getNrOfJoints();
JntArray q(7);
std::cout << "Numbers of joints " << nj << "\n";
for(int i = 0; i < 7; i++)
{
q(i) = msg->actual.positions[i];
}
std::cout << q(0) << " " << q(1) << " " << q(2) << " " << q(3) << " " << q(4) << " " << q(5) << " " << q(6) << "\n";
Frame F_ist;
fksolver1->JntToCart(q, F_ist);
std::cout << F_ist <<"\n";
}
void controllerStateCallback(const sensor_msgs::JointState::ConstPtr& msg)
{
unsigned int nj = chain.getNrOfJoints();
std::vector<std::string> names = msg->name;
std::vector<double> positions = msg->position;
JntArray q = parseJointStates(names,positions);
std::cout << "Joints: " << q(0) << " " << q(1) << " " << q(2) << " " << q(3) << " " << q(4) << " " << q(5) << " " << q(6) << "\n";
JntArray q_out(7);
Frame F_ist;
fksolver1->JntToCart(q, F_ist);
std::cout << F_ist <<"\n";
int ret = iksolver1v->CartToJnt(q, getTwist(F_ist), q_out);
if(ret >= 0)
{
sendVel(q, q_out);
std::cout << q_out(0) << " " << q_out(1) << " " << q_out(2) << " " << q_out(3) << " " << q_out(4) << " " << q_out(5) << " " << q_out(6) << "\n";
}
else
std::cout << "Something went wrong" << "\n";
}
