///reads manipulator joints
///@param Inputs the joint angles
///@return The position of the endfactor
void YouBotKDLInterface::getEndFactorPose(KDL::JntArray& iData, KDL::Frame& oPose)
{
bool kinematic_status;
KDL::ChainFkSolverPos_recursive fksolver = KDL::ChainFkSolverPos_recursive(chain) ;
kinematic_status = fksolver.JntToCart(iData, oPose);
}
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;
}
int main()
{
Vector Fend(3); Fend.zero();
Vector Muend(Fend);
Vector w0(3); Vector dw0(3); Vector ddp0(3);
w0 = dw0=ddp0=0.0; ddp0[1]=g;
int N = 7;
Vector q(N);
Vector dq(N);
Vector ddq(N);
Random rng;
rng.seed(yarp::os::Time::now());
for(int i=0;i<N-1;i++)
{
q[i] = 0*CTRL_DEG2RAD*rng.uniform();
dq[i] = 0*CTRL_DEG2RAD*rng.uniform();
ddq[i] = 0*CTRL_DEG2RAD*rng.uniform();
}
for(int i=N-1;i<N;i++)
{
q[i] = 0;
dq[i] = 0;
ddq[i] = 1;
}
L5PMDyn threeChain;
iDynInvSensorL5PM threeChainSensor(threeChain.asChain(),"papare",DYNAMIC,iCub::skinDynLib::VERBOSE);
int sensor_link = threeChainSensor.getSensorLink();
q = threeChain.setAng(q);
dq = threeChain.setDAng(dq);
ddq = threeChain.setD2Ang(ddq);
int nj=N;
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];
}
threeChain.prepareNewtonEuler(DYNAMIC);
threeChain.computeNewtonEuler(w0,dw0,ddp0,Fend,Muend);
threeChainSensor.computeSensorForceMoment();
// then we can retrieve our results...
// forces moments and torques
Matrix F = threeChain.getForces();
Matrix Mu = threeChain.getMoments();
Vector Tau = threeChain.getTorques();
Matrix F_KDL = idynChainGetForces_usingKDL(threeChain,ddp0);
Matrix Mu_KDL = idynChainGetMoments_usingKDL(threeChain,ddp0);
Vector Tau_KDL = idynChainGetTorques_usingKDL(threeChain,ddp0);
Matrix F_KDL_sens = idynChainGetForces_usingKDL(threeChain,threeChainSensor,ddp0);
Matrix Mu_KDL_sens = idynChainGetMoments_usingKDL(threeChain,threeChainSensor,ddp0);
Vector Tau_KDL_sens = idynChainGetTorques_usingKDL(threeChain,threeChainSensor,ddp0);
Matrix p_idyn(3,N),p_kdl_no_sens(3,N),p_kdl_sens(3,N+1);
for(int l=0;l<N;l++)
{
p_idyn.setCol(l,threeChain.getH(l).subcol(0,3,3));
}
KDL::Chain threeChainKDL;
std::vector<std::string> dummy;
std::string dummy_str = "";
for(int l=0;l<N;l++) {
Vector p_kdl;
idynChain2kdlChain(*(threeChain.asChain()),threeChainKDL,dummy,dummy,dummy_str,dummy_str,l+1);
KDL::ChainFkSolverPos_recursive posSolver = KDL::ChainFkSolverPos_recursive(threeChainKDL);
KDL::Frame cartpos;
KDL::JntArray jpos;
jpos.resize(l+1);
for(int p=0;p<jpos.rows();p++) jpos(p) = jointpositions(p);
posSolver.JntToCart(jpos,cartpos);
to_iDyn(cartpos.p,p_kdl);
p_kdl_no_sens.setCol(l,p_kdl);
}
KDL::Chain threeChainKDLsens;
for(int l=0;l<N+1;l++) {
Vector p_kdl;
idynSensorChain2kdlChain(*(threeChain.asChain()),threeChainSensor,threeChainKDLsens,dummy,dummy,dummy_str,dummy_str,l+1);
KDL::ChainFkSolverPos_recursive posSolver = KDL::ChainFkSolverPos_recursive(threeChainKDLsens);
KDL::Frame cartpos;
KDL::JntArray jpos;
if( l <= sensor_link ) {
jpos.resize(l+1);
for(int p=0;p<jpos.rows();p++) jpos(p) = jointpositions(p);
}
//if( l >= sensor_link )
else {
jpos.resize(l);
for(int p=0;p<jpos.rows();p++) jpos(p) = jointpositions(p);
}
cout << "l " << l << " nrJoints " << threeChainKDLsens.getNrOfJoints() << " nrsegments " << threeChainKDLsens.getNrOfSegments() <<" jpos dim " << jpos.rows() << endl;
assert(posSolver.JntToCart(jpos,cartpos) >= 0);
to_iDyn(cartpos.p,p_kdl);
p_kdl_sens.setCol(l,p_kdl);
}
printMatrix("p_idyn",p_idyn);
printMatrix("p_kdl_no_sens",p_kdl_no_sens);
printMatrix("p_kdl_sens",p_kdl_sens);
cout << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~`KDL Chain~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~``" << endl << threeChainKDL << endl;
cout << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~KDL Chain sens~~~~~~~~~~~~~~~~~~~~~~~~~~~~`" << endl << threeChainKDLsens << endl;
//printKDLchain("threeChainKDLsens",threeChainKDLsens);
printMatrix("F",F);
printMatrix("F_KDL",F_KDL);
printMatrix("F_KDL_sens",F_KDL_sens);
printMatrix("Mu",Mu);
printMatrix("Mu_KDL",Mu_KDL);
printMatrix("Mu_KDL_sens",Mu_KDL_sens);
printVector("Tau",Tau);
printVector("Tau_KDL",Tau_KDL);
printVector("Tau_KDL_sens",Tau_KDL_sens);
for(int l=0;l<F_KDL.cols();l++)
{
YARP_ASSERT(EQUALISH(F_KDL.getCol(l),F.getCol(l)));
YARP_ASSERT(EQUALISH(F_KDL_sens.getCol(l),F.getCol(l)));
YARP_ASSERT(EQUALISH(Mu_KDL.getCol(l),Mu.getCol(l)));
YARP_ASSERT(EQUALISH(Mu_KDL_sens.getCol(l),Mu.getCol(l)));
}
for(int l=0;l<Tau.size();l++) {
YARP_ASSERT(abs(Tau(l)-Tau_KDL(l))<delta);
YARP_ASSERT(abs(Tau(l)-Tau_KDL_sens(l))<delta);
}
}
int main(int argc, char *argv[]) {
std::string tip_frame;
ros::init (argc, argv, "run_cbf");
parse_arguments(argc, argv, tip_frame);
ros::NodeHandle nh;
rdf_loader::RDFLoader rdf("robot_description");
boost::shared_ptr<srdf::Model> srdf = rdf.getSRDF();
if (!srdf) srdf.reset(new srdf::Model());
const boost::shared_ptr<urdf::ModelInterface>& urdf = rdf.getURDF();
if (!urdf) {
ROS_ERROR("couldn't load URDF model");
return EXIT_FAILURE;
}
robot_model::RobotModelPtr robot_model(new robot_model::RobotModel(urdf, srdf));
// fetch KDL tree
KDL::Tree kdl_tree;
if (!kdl_parser::treeFromUrdfModel(*urdf, kdl_tree)) {
ROS_ERROR("Could not initialize KDL tree");
return EXIT_FAILURE;
}
KDL::Chain kdl_chain;
if (!kdl_tree.getChain(kdl_tree.getRootSegment()->first, tip_frame, kdl_chain)) {
ROS_ERROR_STREAM("Could not find chain to " << tip_frame);
return EXIT_FAILURE;
}
// create controller
auto controller = createController(boost::make_shared<KDL::Chain>(kdl_chain));
CBF::DummyResourcePtr joints = boost::dynamic_pointer_cast<CBF::DummyResource>(controller->resource());
CBF::DummyReferencePtr target = boost::dynamic_pointer_cast<CBF::DummyReference>(controller->reference());
CBF::FloatVector target_vector(target->dim());
// joint state publisher
auto jsp = nh.advertise<sensor_msgs::JointState>("joint_states", 1);
// init joint_state message
auto js_msg = init_message(kdl_chain);
server.reset( new interactive_markers::InteractiveMarkerServer("cbf_control","",false) );
// initialize marker with end-effector pose from forward kinematics
KDL::ChainFkSolverPos_recursive fk = KDL::ChainFkSolverPos_recursive(kdl_chain);
KDL::JntArray kdl_joints = KDL::JntArray(kdl_chain.getNrOfJoints());
KDL::Frame kdl_pose;
fk.JntToCart(kdl_joints, kdl_pose);
tf::Pose tf_pose;
tf::poseKDLToTF(kdl_pose, tf_pose);
geometry_msgs::PoseStamped stamped;
stamped.header.frame_id = kdl_tree.getRootSegment()->first;
tf::poseTFToMsg(tf_pose, stamped.pose);
bool bStalled = false;
make6DofMarker(stamped, !bStalled);
// set initial pose
marker_feedback.pose = stamped.pose;
// run controller
ros::Rate rate(50); // 50 hz update rate
while (ros::ok()) {
// set target from marker feedback
assign(target_vector.head(3), marker_feedback.pose.position);
assign(target_vector.tail(3), marker_feedback.pose.orientation);
target->set_reference(target_vector);
// perform controller step
unsigned int iMaxIter = 10;
while (iMaxIter && controller->step() == false) {
if (bStalled != controller->stalled()) {
bStalled = controller->stalled();
stamped.pose = marker_feedback.pose;
make6DofMarker(stamped, !bStalled);
if (bStalled) break;
}
--iMaxIter;
}
// fill + publish ros joint_state message
update_message(js_msg, joints);
jsp.publish(js_msg);
// process ros messages
ros::spinOnce();
rate.sleep();
}
server.reset();
return EXIT_SUCCESS;
}
int main(int argc, char** argv){
ros::init(argc, argv, "my_parser");
if (argc != 2){
ROS_ERROR("Need a urdf file as argument");
return -1;
}
std::string urdf_file = argv[1];
KDL::Tree hermes_tree;
urdf::Model hermes_model;
if (!hermes_model.initFile(urdf_file)){
ROS_ERROR("Failed to parse urdf file");
return -1;
}
if (!kdl_parser::treeFromUrdfModel(hermes_model,hermes_tree)){
ROS_ERROR("Failed to construct kdl tree");
return -1;
}
ROS_INFO("Successfully parsed urdf file to KDL");
ROS_INFO("Try to solve forward kinematics for zero joint position ...");
// Get Chain from the Tree
bool exit_value;
KDL::Chain hermes_chain;
exit_value = hermes_tree.getChain("base","link7",hermes_chain);
// Create solver based on kinematic chain
KDL::ChainFkSolverPos_recursive fksolver = ChainFkSolverPos_recursive(hermes_chain);
// Create joint array
KDL::JntArray q(hermes_chain.getNrOfJoints());
// Assign values to the joints
/*for(unsigned int i=0;i<hermes_chain.getNrOfJoints();i++){
q(i)=0.0;
}*/
q(0) = -0.1;
q(1) = 1.2;
q(2) = -0.3;
q(3) = 0.6;
q(4) = 0.4;
q(5) = 0.75;
q(6) = -0.1;
// Create the frame that will contain the results
KDL::Frame cartpos;
// Calculate forward position kinematics
bool kinematics_status;
kinematics_status = fksolver.JntToCart(q,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 :(");
}
return 0;
}
