//NEVER call this without setting the container chain!!
void kinematics_utilities::initialize_solvers(chain_and_solvers* container, KDL::JntArray& joints_value,KDL::JntArray& q_max, KDL::JntArray& q_min, int index)
{
for (KDL::Segment& segment: container->chain.segments)
{
if (segment.getJoint().getType()==KDL::Joint::None) continue;
//std::cout<<segment.getJoint().getName()<<std::endl;
container->joint_names.push_back(segment.getJoint().getName());
}
assert(container->joint_names.size()==container->chain.getNrOfJoints());
joints_value.resize(container->chain.getNrOfJoints());
SetToZero(joints_value);
q_max.resize(container->chain.getNrOfJoints());
q_min.resize(container->chain.getNrOfJoints());
container->average_joints.resize(container->chain.getNrOfJoints());
container->fksolver=new KDL::ChainFkSolverPos_recursive(container->chain);
container->ikvelsolver = new KDL::ChainIkSolverVel_pinv(container->chain);
container->index=index;
int j=0;
for (auto joint_name:container->joint_names)
{
#if IGNORE_JOINT_LIMITS
q_max(j)=M_PI/3.0;
q_min(j)=-M_PI/3.0;
#else
q_max(j)=urdf_model.joints_[joint_name]->limits->upper;
q_min(j)=urdf_model.joints_[joint_name]->limits->lower;
#endif
container->average_joints(j)=(q_max(j)+q_min(j))/2.0;
j++;
}
container->iksolver= new KDL::ChainIkSolverPos_NR_JL(container->chain,q_min,q_max,*container->fksolver,*container->ikvelsolver);
}
bool Kinematics::readJoints(urdf::Model &robot_model) {
num_joints = 0;
// get joint maxs and mins
boost::shared_ptr<const urdf::Link> link = robot_model.getLink(tip_name);
boost::shared_ptr<const urdf::Joint> joint;
while (link && link->name != root_name) {
joint = robot_model.getJoint(link->parent_joint->name);
if (!joint) {
ROS_ERROR("Could not find joint: %s",link->parent_joint->name.c_str());
return false;
}
if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) {
ROS_INFO( "adding joint: [%s]", joint->name.c_str() );
num_joints++;
}
link = robot_model.getLink(link->getParent()->name);
}
joint_min.resize(num_joints);
joint_max.resize(num_joints);
info.joint_names.resize(num_joints);
info.link_names.resize(num_joints);
info.limits.resize(num_joints);
link = robot_model.getLink(tip_name);
unsigned int i = 0;
while (link && i < num_joints) {
joint = robot_model.getJoint(link->parent_joint->name);
if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) {
ROS_INFO( "getting bounds for joint: [%s]", joint->name.c_str() );
float lower, upper;
int hasLimits;
if ( joint->type != urdf::Joint::CONTINUOUS ) {
lower = joint->limits->lower;
upper = joint->limits->upper;
hasLimits = 1;
} else {
lower = -M_PI;
upper = M_PI;
hasLimits = 0;
}
int index = num_joints - i -1;
joint_min.data[index] = lower;
joint_max.data[index] = upper;
info.joint_names[index] = joint->name;
info.link_names[index] = link->name;
info.limits[index].joint_name = joint->name;
info.limits[index].has_position_limits = hasLimits;
info.limits[index].min_position = lower;
info.limits[index].max_position = upper;
i++;
}
link = robot_model.getLink(link->getParent()->name);
}
return true;
}
int PR2ArmIKSolver::CartToJnt(const KDL::JntArray& q_init,
const KDL::Frame& p_in,
std::vector<KDL::JntArray> &q_out)
{
Eigen::Matrix4f b = KDLToEigenMatrix(p_in);
std::vector<std::vector<double> > solution_ik;
KDL::JntArray q;
if(free_angle_ == 0)
{
pr2_arm_ik_.computeIKShoulderPan(b, q_init(0), solution_ik);
}
else
{
pr2_arm_ik_.computeIKShoulderRoll(b, q_init(2), solution_ik);
}
if(solution_ik.empty())
return -1;
q.resize(7);
q_out.clear();
for(int i=0; i< (int) solution_ik.size(); ++i)
{
for(int j=0; j < 7; j++)
{
q(j) = solution_ik[i][j];
}
q_out.push_back(q);
}
return 1;
}
int PR2ArmIKSolver::CartToJnt(const KDL::JntArray& q_init,
const KDL::Frame& p_in,
KDL::JntArray &q_out)
{
Eigen::Matrix4f b = KDLToEigenMatrix(p_in);
std::vector<std::vector<double> > solution_ik;
if(free_angle_ == 0)
{
ROS_DEBUG("Solving with %f",q_init(0));
pr2_arm_ik_.computeIKShoulderPan(b,q_init(0),solution_ik);
}
else
{
pr2_arm_ik_.computeIKShoulderRoll(b,q_init(2),solution_ik);
}
if(solution_ik.empty())
return -1;
double min_distance = 1e6;
int min_index = -1;
for(int i=0; i< (int) solution_ik.size(); i++)
{
ROS_DEBUG("Solution : %d",(int)solution_ik.size());
for(int j=0; j < (int)solution_ik[i].size(); j++)
{
ROS_DEBUG("%d: %f",j,solution_ik[i][j]);
}
ROS_DEBUG(" ");
ROS_DEBUG(" ");
double tmp_distance = computeEuclideanDistance(solution_ik[i],q_init);
if(tmp_distance < min_distance)
{
min_distance = tmp_distance;
min_index = i;
}
}
if(min_index > -1)
{
q_out.resize((int)solution_ik[min_index].size());
for(int i=0; i < (int)solution_ik[min_index].size(); i++)
{
q_out(i) = solution_ik[min_index][i];
}
return 1;
}
else
return -1;
}
bool jointPosLimitsFromUrdfModel(const urdf::ModelInterface& robot_model,
std::vector<std::string> & joint_names,
KDL::JntArray & min,
KDL::JntArray & max)
{
int nrOfJointsWithLimits=0;
for (urdf::JointPtrMap::const_iterator it=robot_model.joints_.begin(); it!=robot_model.joints_.end(); ++it)
{
if( it->second->type == urdf::Joint::REVOLUTE ||
it->second->type == urdf::Joint::PRISMATIC )
{
nrOfJointsWithLimits++;
}
}
joint_names.resize(nrOfJointsWithLimits);
min.resize(nrOfJointsWithLimits);
max.resize(nrOfJointsWithLimits);
int index =0;
for (urdf::JointPtrMap::const_iterator it=robot_model.joints_.begin(); it!=robot_model.joints_.end(); ++it)
{
if( it->second->type == urdf::Joint::REVOLUTE ||
it->second->type == urdf::Joint::PRISMATIC )
{
joint_names[index] = (it->first);
min(index) = it->second->limits->lower;
max(index) = it->second->limits->upper;
index++;
}
}
if( index != nrOfJointsWithLimits )
{
std::cerr << "[ERR] kdl_format_io error in jointPosLimitsFromUrdfModel function" << std::endl;
return false;
}
return true;
}
///reads Position of all manipulator joints
///@param data returns the Position per joint
void YouBotKDLInterface::getJointPosition(KDL::JntArray& data)
{
data.resize(ARMJOINTS);
std::vector<youbot::JointSensedAngle> jointSensedAngle;
jointSensedAngle.resize(ARMJOINTS);
youBotManipulator->getJointData(jointSensedAngle);
data(0) = (double) jointSensedAngle[0].angle.value();
data(1) = (double) jointSensedAngle[1].angle.value();
data(2) = (double) jointSensedAngle[2].angle.value();
data(3) = (double) jointSensedAngle[3].angle.value();
data(4) = (double) jointSensedAngle[4].angle.value();
}
///reads Velocity of all manipulator joints
///@param data returns the Velocity per joint
void YouBotKDLInterface::getJointVelocity(KDL::JntArray& data)
{
data.resize(ARMJOINTS);
std::vector<youbot::JointSensedVelocity> jointSensedVelocity;
jointSensedVelocity.resize(ARMJOINTS);
youBotManipulator->getJointData(jointSensedVelocity);
data(0) = (double) jointSensedVelocity[0].angularVelocity.value();
data(1) = (double) jointSensedVelocity[1].angularVelocity.value();
data(2) = (double) jointSensedVelocity[2].angularVelocity.value();
data(3) = (double) jointSensedVelocity[3].angularVelocity.value();
data(4) = (double) jointSensedVelocity[4].angularVelocity.value();
}
///reads Current of all manipulator joints
///@param data returns the current per joint
void YouBotKDLInterface::getJointCurrent(KDL::JntArray& data)
{
data.resize(ARMJOINTS);
std::vector<youbot::JointSensedCurrent> jointSensedCurrent;
jointSensedCurrent.resize(ARMJOINTS);
youBotManipulator->getJointData(jointSensedCurrent);
data(0) = (double) jointSensedCurrent[0].current.value();
data(1) = (double) jointSensedCurrent[1].current.value();
data(2) = (double) jointSensedCurrent[2].current.value();
data(3) = (double) jointSensedCurrent[3].current.value();
data(4) = (double) jointSensedCurrent[4].current.value();
}
bool FkLookup::ChainFK::parseJointState(const sensor_msgs::JointState &state_in, KDL::JntArray &array_out) const{
unsigned int num = state_in.name.size();
int joints_needed = joints.size();
array_out.resize(joints_needed);
if(num == state_in.position.size()){
for(unsigned i = 0; i < num; ++i){
std::map<std::string, unsigned int>::const_iterator it = joints.find(state_in.name[i]);
if( it != joints.end()){
array_out(it->second) = state_in.position[i];
--joints_needed;
}
}
}
return joints_needed == 0;
}
bool FkLookup::ChainFK::parseJointState(const sensor_msgs::JointState &state_in, KDL::JntArray &array_out, std::map<std::string, unsigned int> &missing) const{
// seed state > robot_state > planning_scene
if(missing.size() == 0){
array_out.resize(joints.size());
missing = joints;
}
int joints_needed = missing.size();
unsigned int num = state_in.name.size();
if(state_in.name.size() == state_in.position.size()){
for(unsigned i = 0; i < num && joints_needed > 0; ++i){
std::map<std::string, unsigned int>::iterator it = missing.find(state_in.name[i]);
if( it != missing.end()){
array_out(it->second) = state_in.position[i];
--joints_needed;
}
}
}
return joints_needed == 0;
}
int InverseKinematicsSolver::CartToJnt(const KDL::JntArray& q_init,
const KDL::Frame& p_in,
KDL::JntArray &q_out)
{
ROS_DEBUG("InverseKinematicsSolver::CartToJnt\n");
std::vector<KDL::JntArray> solution_ik;
_ik.CartToJnt(q_init, p_in, solution_ik);
if (solution_ik.empty()) return -1;
double min_distance = 1e6;
int min_index = -1;
for (unsigned int i = 0; i < solution_ik.size(); i++) {
ROS_DEBUG("Solution : %ud", i);
for (unsigned int j = 0; j < solution_ik[i].rows(); j++) {
ROS_DEBUG("%d: %f", j, solution_ik[i](j));
}
ROS_DEBUG(" ");
ROS_DEBUG(" ");
double tmp_distance = computeEuclideanDistance(solution_ik[i], q_init);
if (tmp_distance < min_distance) {
min_distance = tmp_distance;
min_index = i;
}
}
if (min_index > -1) {
q_out.resize(solution_ik[min_index].rows());
for (unsigned int i = 0; i < solution_ik[min_index].rows(); i++) {
q_out(i) = solution_ik[min_index](i);
}
return 1;
} else {
return -1;
}
}
void ITRCartesianImpedanceController::multiplyJacobian(const KDL::Jacobian& jac, const KDL::Wrench& src, KDL::JntArray& dest)
{
Eigen::Matrix<double,6,1> w;
w(0) = src.force(0);
w(1) = src.force(1);
w(2) = src.force(2);
w(3) = src.torque(0);
w(4) = src.torque(1);
w(5) = src.torque(2);
Eigen::MatrixXd j(jac.rows(), jac.columns());
j = jac.data;
j.transposeInPlace();
Eigen::VectorXd t(jac.columns());
t = j*w;
dest.resize(jac.columns());
for (unsigned i=0; i<jac.columns(); i++)
dest(i) = t(i);
}
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;
}
}
}
void callbackJointState(const JointStateConstPtr& state)
{
std::map<std::string, double> joint_name_position;
if (state->name.size() != state->position.size()) {
ROS_ERROR("Robot state publisher received an invalid joint state vector");
return;
}
// determine least recently published joint
ros::Time last_published = ros::Time::now();
for (unsigned int i=0; i<state->name.size(); i++) {
ros::Time t = last_publish_time_[state->name[i]];
last_published = (t < last_published) ? t : last_published;
}
if (state->header.stamp >= last_published + publish_interval_) {
// get joint positions from state message
std::map<std::string, double> joint_positions;
std::map<std::string, double> joint_efforts;
for (unsigned int i=0; i<state->name.size(); i++) {
joint_positions.insert(make_pair(state->name[i], state->position[i]));
joint_efforts.insert(make_pair(state->name[i], state->effort[i]));
}
KDL::JntArray tau;
KDL::Wrench F;
Eigen::Matrix<double,Eigen::Dynamic,6> jac_t;
Eigen::Matrix<double,6,Eigen::Dynamic> jac_t_pseudo_inv;
tf::StampedTransform transform;
KDL::Wrench F_pub;
tf::Vector3 tf_force;
tf::Vector3 tf_torque;
tau.resize(chain_.getNrOfJoints());
//getPositions;
for (size_t i=0, j=0; i<chain_.getNrOfSegments(); i++) {
if (chain_.getSegment(i).getJoint().getType() == KDL::Joint::None)
continue;
// check
std::string joint_name = chain_.getSegment(i).getJoint().getName();
if ( joint_positions.find(joint_name) == joint_positions.end() ) {
ROS_ERROR("Joint '%s' is not found in joint state vector", joint_name.c_str());
}
// set position
jnt_pos_(j) = joint_positions[joint_name];
tau(j) = joint_efforts[joint_name];
j++;
}
jnt_to_jac_solver_->JntToJac(jnt_pos_, jacobian_);
jac_t = jacobian_.data.transpose();
if ( jacobian_.columns() >= jacobian_.rows() ) {
jac_t_pseudo_inv =(jac_t.transpose() * jac_t).inverse() * jac_t.transpose();
} else {
jac_t_pseudo_inv =jac_t.transpose() * ( jac_t * jac_t.transpose() ).inverse();
}
#if 1
{
ROS_INFO("jac_t# jac_t : ");
Eigen::Matrix<double,6,6> mat_i = mat_i = jac_t_pseudo_inv * jac_t;
for (unsigned int i = 0; i < 6; i++) {
std::stringstream ss;
for (unsigned int j=0; j<6; j++)
ss << std::fixed << std::setw(8) << std::setprecision(4) << mat_i(j,i) << " ";
ROS_INFO_STREAM(ss.str());
}
}
#endif
// f = - inv(jt) * effort
for (unsigned int j=0; j<6; j++)
{
F(j) = 0;
for (unsigned int i = 0; i < jnt_pos_.rows(); i++)
{
F(j) += -1 * jac_t_pseudo_inv(j, i) * tau(i);
}
}
//low pass filter
F += (F_pre_ - F) * exp(-1.0 / t_const_);
for (unsigned int j=0; j<6; j++) {
F_pre_(j) = 0;
}
F_pre_ += F;
//tf transformation
tf::vectorKDLToTF(F.force, tf_force);
tf::vectorKDLToTF(F.torque, tf_torque);
try {
listener_.waitForTransform( tip, root, state->header.stamp, ros::Duration(1.0));
listener_.lookupTransform( tip, root, state->header.stamp , transform);
}
catch (tf::TransformException ex) {
ROS_ERROR("%s",ex.what());
ros::Duration(1.0).sleep();
}
for (unsigned int j=0; j<3; j++) {
F_pub.force[j] = transform.getBasis()[j].dot(tf_force);
F_pub.torque[j] = transform.getBasis()[j].dot(tf_torque);
}
geometry_msgs::WrenchStamped msg;
msg.header.stamp = state->header.stamp;
msg.header.frame_id = tip;
// http://code.ros.org/lurker/message/20110107.151127.7177af06.nl.html
//tf::WrenchKDLToMsg(F,msg.wrench);
msg.wrench.force.x = F_pub.force[0];
msg.wrench.force.y = F_pub.force[1];
msg.wrench.force.z = F_pub.force[2];
msg.wrench.torque.x = F_pub.torque[0];
msg.wrench.torque.y = F_pub.torque[1];
msg.wrench.torque.z = F_pub.torque[2];
wrench_stamped_pub_.publish(msg);
{
ROS_INFO("jacobian : ");
for (unsigned int i = 0; i < jnt_pos_.rows(); i++) {
std::stringstream ss;
for (unsigned int j=0; j<6; j++)
ss << std::fixed << std::setw(8) << std::setprecision(4) << jacobian_(j,i) << " ";
ROS_INFO_STREAM(ss.str());
}
ROS_INFO("effort : ");
std::stringstream sstau;
for (unsigned int i = 0; i < tau.rows(); i++) {
sstau << std::fixed << std::setw(8) << std::setprecision(4) << tau(i) << " ";
}
ROS_INFO_STREAM(sstau.str());
ROS_INFO("force : ");
std::stringstream ssf;
for (unsigned int j = 0; j < 6; j++) {
ssf << std::fixed << std::setw(8) << std::setprecision(4) << F(j) << " ";
}
ROS_INFO_STREAM(ssf.str());
}
// store publish time in joint map
for (unsigned int i=0; i<state->name.size(); i++)
last_publish_time_[state->name[i]] = state->header.stamp;
}
}
bool TreeKinematics::readJoints(urdf::Model &robot_model,
KDL::Tree &kdl_tree,
std::string &tree_root_name,
unsigned int &nr_of_jnts,
KDL::JntArray &joint_min,
KDL::JntArray &joint_max,
KDL::JntArray &joint_vel_max)
{
KDL::SegmentMap segmentMap;
segmentMap = kdl_tree.getSegments();
tree_root_name = kdl_tree.getRootSegment()->second.segment.getName();
nr_of_jnts = kdl_tree.getNrOfJoints();
ROS_DEBUG( "the tree's number of joints: [%d]", nr_of_jnts );
joint_min.resize(nr_of_jnts);
joint_max.resize(nr_of_jnts);
joint_vel_max.resize(nr_of_jnts);
info_.joint_names.resize(nr_of_jnts);
info_.limits.resize(nr_of_jnts);
// The following walks through all tree segments, extracts their joints except joints of KDL::None and extracts
// the information about min/max position and max velocity of the joints not of type urdf::Joint::UNKNOWN or
// urdf::Joint::FIXED
ROS_DEBUG("Extracting all joints from the tree, which are not of type KDL::Joint::None.");
boost::shared_ptr<const urdf::Joint> joint;
for (KDL::SegmentMap::const_iterator seg_it = segmentMap.begin(); seg_it != segmentMap.end(); ++seg_it)
{
if (seg_it->second.segment.getJoint().getType() != KDL::Joint::None)
{
// check, if joint can be found in the URDF model of the robot
joint = robot_model.getJoint(seg_it->second.segment.getJoint().getName().c_str());
if (!joint)
{
ROS_FATAL("Joint '%s' has not been found in the URDF robot model!", joint->name.c_str());
return false;
}
// extract joint information
if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
{
ROS_DEBUG( "getting information about joint: [%s]", joint->name.c_str() );
double lower = 0.0, upper = 0.0, vel_limit = 0.0;
unsigned int has_pos_limits = 0, has_vel_limits = 0;
if ( joint->type != urdf::Joint::CONTINUOUS )
{
ROS_DEBUG("joint is not continuous.");
lower = joint->limits->lower;
upper = joint->limits->upper;
has_pos_limits = 1;
if (joint->limits->velocity)
{
has_vel_limits = 1;
vel_limit = joint->limits->velocity;
ROS_DEBUG("joint has following velocity limit: %f", vel_limit);
}
else
{
has_vel_limits = 0;
vel_limit = 0.0;
ROS_DEBUG("joint has no velocity limit.");
}
}
else
{
ROS_DEBUG("joint is continuous.");
lower = -M_PI;
upper = M_PI;
has_pos_limits = 0;
if(joint->limits && joint->limits->velocity)
{
has_vel_limits = 1;
vel_limit = joint->limits->velocity;
ROS_DEBUG("joint has following velocity limit: %f", vel_limit);
}
else
{
has_vel_limits = 0;
vel_limit = 0.0;
ROS_DEBUG("joint has no velocity limit.");
}
}
joint_min(seg_it->second.q_nr) = lower;
joint_max(seg_it->second.q_nr) = upper;
joint_vel_max(seg_it->second.q_nr) = vel_limit;
ROS_DEBUG("pos_min = %f, pos_max = %f, vel_max = %f", lower, upper, vel_limit);
info_.joint_names[seg_it->second.q_nr] = joint->name;
info_.limits[seg_it->second.q_nr].joint_name = joint->name;
info_.limits[seg_it->second.q_nr].has_position_limits = has_pos_limits;
info_.limits[seg_it->second.q_nr].min_position = lower;
info_.limits[seg_it->second.q_nr].max_position = upper;
info_.limits[seg_it->second.q_nr].has_velocity_limits = has_vel_limits;
info_.limits[seg_it->second.q_nr].max_velocity = vel_limit;
}
}
}
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);
}
}
bool ChainParser::parse(Tree& tree,
std::map<std::string,unsigned int>& joint_name_to_index,
std::vector<std::string>& index_to_joint_name,
KDL::JntArray& q_min,
KDL::JntArray& q_max)
{
ros::NodeHandle n("~");
/* * * * * * * * PARSE JOINT TOPICS * * * * * * * * */
/* * * * * * * * PARSE URDF * * * * * * * * */
std::string urdf_xml, full_urdf_xml;
n.param("urdf_xml", urdf_xml,std::string("/amigo/robot_description"));
n.searchParam(urdf_xml, full_urdf_xml);
ROS_INFO("Reading xml file from parameter server");
std::string result_string;
if (!n.getParam(full_urdf_xml, result_string)) {
ROS_FATAL("Could not load the xml from parameter server: %s", urdf_xml.c_str());
//error = true;
return false;
}
urdf::Model robot_model;
// Is this necessary?
if (!robot_model.initString(result_string)) {
ROS_FATAL("Could not initialize robot model");
//error = true;
return -1;
}
ROS_INFO("Robot model initialized");
if (!kdl_parser::treeFromString(result_string, tree.kdl_tree_)) {
ROS_ERROR("Could not initialize tree object");
//error = true;
return false;
}
ROS_INFO("Tree object initialized");
/* * * * * * * * PARSE CHAINS * * * * * * * * */
XmlRpc::XmlRpcValue chain_params;
if (!n.getParam("chain_description", chain_params)) {
ROS_ERROR("No chain description given: %s", "chain_description");
return false;
}
if (chain_params.getType() != XmlRpc::XmlRpcValue::TypeArray) {
ROS_ERROR("Chain description list should be an array. (namespace: %s)", n.getNamespace().c_str());
return false;
}
std::vector<double> q_min_vec;
std::vector<double> q_max_vec;
for(int i = 0; i < chain_params.size(); ++i) {
ROS_INFO("Going to parse chain ...");
//Chain* chain = parseChain(chain_params[i], tree, robot_model, joint_name_to_index, index_to_joint_name, q_min_vec, q_max_vec);
parseChain(chain_params[i], tree, robot_model, joint_name_to_index, index_to_joint_name, q_min_vec, q_max_vec);
ROS_INFO("Parse chain ready");
//if (chain) {
// chains.push_back(chain);
//}
}
q_min.resize(joint_name_to_index.size());
q_max.resize(joint_name_to_index.size());
for(unsigned int i = 0; i < joint_name_to_index.size(); ++i) {
q_min(i) = q_min_vec[i];
q_max(i) = q_max_vec[i];
}
ROS_INFO("Parsing done");
return true;
}
