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;
}
//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);
}
Eigen::VectorXd toEigen(const KDL::Twist & v, const KDL::JntArray & dq)
{
VectorXd ret(6+dq.rows());
ret.segment(0,6) = toEigen(v);
for(int i=0; i < (int)dq.rows(); i++ ) { ret(6+i) = dq(i); }
return ret;
}
Eigen::VectorXd toEigen(const KDL::Wrench & f, const KDL::JntArray & tau)
{
VectorXd ret(6+tau.rows());
ret.segment(0,6) = toEigen(f);
for(int i=0; i < (int)tau.rows(); i++ ) { ret(6+i) = tau(i); }
return ret;
}
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;
}
void KDLChainWrapper::jointArrayToRealJointArray(const KDL::JntArray& joint_array, KDL::JntArray& real_joint_array)
{
ROS_ASSERT(int(joint_array.rows()) == num_joints_);
ROS_ASSERT(int(real_joint_array.rows()) == num_real_joints_);
for (int i=0; i<num_real_joints_; ++i)
{
real_joint_array(i) = mimic_joints_[i].offset + mimic_joints_[i].multiplier *
joint_array(mimic_joints_[i].mimic_joint);
}
}
bool ArmAnalyticalInverseKinematics::isSolutionValid(const KDL::JntArray &solution) const
{
bool valid = true;
if (solution.rows() != 5) return false;
for (unsigned int i = 0; i < solution.rows(); i++) {
if ((solution(i) < _min_angles[i]) || (solution(i) > _max_angles[i])) {
valid = false;
}
}
return valid;
}
void Arm_Cartesian_Control::checkLimits(
double dt, KDL::JntArray& joint_positions,
KDL::JntArray& jntVel) {
if (upper_joint_limits.size() < arm_chain->getNrOfJoints()
|| lower_joint_limits.size() < arm_chain->getNrOfJoints()) {
std::cout << "No Joint limits defined";
return;
}
double limit_range = 0.1;
jointLimitsReached = false;
for (int i=0; i<jntVel.data.rows(); i++) {
double pos = joint_positions.data(i);
double vel = jntVel.data(i);
double fpos = pos + vel * dt * 2;
double upper_limit = this->upper_joint_limits[i];
double lower_limit = this->lower_joint_limits[i];
double upper_clearance = upper_limit - fpos;
double lower_clearance = lower_limit - fpos;
double limit_vel = vel;
if (fabs(upper_clearance) < limit_range*2 && vel > 0) {
limit_vel *= upper_clearance;
}
if (fabs(lower_clearance) < limit_range*2 && vel < 0) {
limit_vel *= lower_clearance;
}
if (fpos > upper_limit - limit_range && vel > 0) {
limit_vel = 0.0;
std::cout << "Upper limit reached for joint " << i << std::endl;
jointLimitsReached = true;
} else if (fpos < lower_limit + limit_range && vel < 0) {
limit_vel = 0.0;
std::cout << "Lower limit reached for joint " << i << std::endl;
jointLimitsReached = true;
}
jntVel.data(i) = limit_vel;
}
}
void KdlTreeFk::getPoses(const KDL::JntArray& joints, std::map<std::string, KDL::Frame>& frames)
{
if (joints.rows() != tree.getNrOfJoints())
{
std::stringstream err;
err << "getPoses() joints not properly sized";
//RCS::Logger::log("gov.nasa.controllers.KdlTreeFk", log4cpp::Priority::ERROR, err.str());
throw std::runtime_error(err.str());
return;
}
KDL::Frame baseFrame;
if (frames.empty())
{
recursiveGetPoses(true, baseFrame, tree.getRootSegment(), joints, frames);
}
else
{
/// make sure all of the listed frames are available
// if non existant frames are requested, this is the only way to find out
for (std::map<std::string, KDL::Frame>::const_iterator it = frames.begin(); it != frames.end(); ++it)
{
if (tree.getSegment(it->first) == tree.getSegments().end())
{
std::stringstream err;
err << "KdlTreeFk::getPoses() requested an unavailable pose";
//RCS::Logger::log("gov.nasa.controllers.KdlTreeFk", log4cpp::Priority::ERROR, err.str());
throw std::runtime_error(err.str());
return;
}
}
recursiveGetPoses(false, baseFrame, tree.getRootSegment(), joints, frames);
}
}
bool FKSolver::solve(const KDL::JntArray& q_in, std::vector<KDL::Vector>& joint_pos, std::vector<KDL::Vector>& joint_axis, std::vector<KDL::Frame>& segment_frames) const
{
Frame p_out = Frame::Identity();
if (q_in.rows() != num_joints_)
return false;
joint_pos.resize(num_joints_);
joint_axis.resize(num_joints_);
segment_frames.resize(num_segments_);
int j = 0;
for (unsigned int i = 0; i < num_segments_; i++)
{
double joint_value = 0.0;
if (chain_.getSegment(i).getJoint().getType() != Joint::None)
{
joint_value = q_in(j);
joint_pos[j] = p_out * chain_.getSegment(i).getJoint().JointOrigin();
joint_axis[j] = p_out.M * chain_.getSegment(i).getJoint().JointAxis();
j++;
}
p_out = p_out * chain_.getSegment(i).pose(joint_value);
segment_frames[i] = p_out;
}
return true;
}
int SNS_IK::CartToJnt(const KDL::JntArray &q_init, const KDL::Frame &p_in,
const KDL::JntArray& q_bias,
const std::vector<std::string>& biasNames,
KDL::JntArray &q_out, const KDL::Twist& bounds) {
if (!m_initialized) {
ROS_ERROR("SNS_IK was not properly initialized with a valid chain or limits.");
return -1;
}
// The position solver uses a barrier function instead of the hard position limits
m_ik_vel_solver->usePositionLimits(false);
int result;
if (q_bias.rows()) {
MatrixD ns_jacobian;
std::vector<int> indicies;
if (!nullspaceBiasTask(q_bias, biasNames, &ns_jacobian, &indicies)) {
ROS_ERROR("Could not create nullspace bias task");
result = -1;
} else {
result = m_ik_pos_solver->CartToJnt(q_init, p_in, q_bias, ns_jacobian, indicies,
m_nullspaceGain, &q_out, bounds);
}
} else {
result = m_ik_pos_solver->CartToJnt(q_init, p_in, &q_out, bounds);
}
m_ik_vel_solver->usePositionLimits(true);
return result;
}
void PostureControl::initialize(const KDL::JntArray& q_min, const KDL::JntArray& q_max, const std::vector<double>& q0, const std::vector<double>& gain, const std::map<std::string, unsigned int>& joint_name_to_index) {
num_joints_ = q_min.rows();
q0_.resize(num_joints_);
q0_default_.resize(num_joints_);
q_min_.resize(num_joints_);
q_max_.resize(num_joints_);
K_.resize(num_joints_);
joint_name_to_index_ = joint_name_to_index;
for (uint i = 0; i < num_joints_; i++) {
q0_[i] = q0[i];
q0_default_[i] = q0[i];
q_min_[i] = q_min(i);
q_max_[i] = q_max(i);
}
ROS_INFO("Length joint array = %i",num_joints_);
for (uint i = 0; i < num_joints_; i++) {
K_[i] = gain[i] / ((q_max(i) - q_min(i))*(q_max(i) - q_min(i)));
//ROS_INFO("qmin = %f, qmax = %f, q0 = %f, K = %f, gain = %f",q_min(i),q_max(i),q0_[i], K_[i], gain[i]);
}
//ROS_INFO("Length joint array = %i",num_joints_);
current_cost_ = 0;
}
double InverseKinematicsSolver::computeEuclideanDistance(
const KDL::JntArray &array_1,
const KDL::JntArray &array_2)
{
double distance = 0.0;
for (unsigned int i = 0; i < array_1.rows(); i++) {
distance += (array_1(i) - array_2(i)) * (array_1(i) - array_2(i));
}
return sqrt(distance);
}
KDL::Frame ArmKinematics::get_pos_fk(const KDL::JntArray& q)
{
assert(q.rows() == dof_);
assert(fk_solver_);
KDL::Frame pose;
fk_solver_->JntToCart(q, pose);
return pose;
}
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;
}
// Fill in the plot message for publishing data to be plotted in the /VS_errors topic
void makePlotMSG(std_msgs::Float64MultiArray &plotMsg,
vpColVector &taskError,
vpColVector &qdot,
vpColVector &q1dot,
vpColVector &q2dot,
vpMatrix &fJe,
vpVelocityTwistMatrix &eVf,
KDL::JntArray &kdlArmPosition
)
{
// add qdot - total joint velocity
for(unsigned int i=0; i<qdot.getRows(); i++)
{
plotMsg.data.push_back(qdot[i]);
}
// add q1dot - main task
for(unsigned int i=0; i<q1dot.getRows(); i++)
{
plotMsg.data.push_back(q1dot[i]);
}
// add q2dot - main task
for(unsigned int i=0; i<q2dot.getRows(); i++)
{
plotMsg.data.push_back(q2dot[i]);
}
// add the end-effector velocity
vpColVector vel;
vel = eVf*fJe*qdot;
for(unsigned int i=0; i<vel.getRows(); i++)
{
plotMsg.data.push_back(vel[i]);
}
// add the joint positions
for(unsigned int i=0; i<kdlArmPosition.rows(); i++)
{
plotMsg.data.push_back(kdlArmPosition.data[i]);
}
// add the error norm
plotMsg.data.push_back(taskError.sumSquare());
// add the errors
for(unsigned int i=0; i<taskError.getRows(); i++)
{
ROS_INFO_STREAM("=> makePlotMSG: adding taskError at position " << plotMsg.data.size());
plotMsg.data.push_back(taskError[i]);
}
}
bool RobotStatus::reachedPosition(KDL::JntArray reference)
{
if( reference.rows() != m_n_joints_monitoring )
{
ERROR_STREAM("reachedPosition check for " << reference.rows() << " joints while robot status contains " << m_n_joints_monitoring << " joints");
return false;
}
if( m_pos_reached_threshold < 0.0)
{
ERROR_STREAM("Check for reachedPosition check while threshold was not set!");
return false;
}
for( int i = 0; i < m_n_joints_monitoring; ++i)
{
if( fabs( reference(i) - m_joints_to_monitor(i)) > m_pos_reached_threshold )
return false;
}
return true;
}
///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 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();
}
bool SNS_IK::nullspaceBiasTask(const KDL::JntArray& q_bias,
const std::vector<std::string>& biasNames,
MatrixD* jacobian,
std::vector<int>* indicies)
{
ROS_ASSERT_MSG(q_bias.rows() == biasNames.size(), "SNS_IK: Number of joint bias and names differ");
Task task2;
*jacobian = MatrixD::Zero(q_bias.rows(), m_jointNames.size());
indicies->resize(q_bias.rows(), 0);
std::vector<std::string>::iterator it;
for (size_t ii = 0; ii < q_bias.rows(); ++ii) {
it = std::find(m_jointNames.begin(), m_jointNames.end(), biasNames[ii]);
if (it == m_jointNames.end())
{
std::cout << "Could not find bias joint name: " << biasNames[ii] << std::endl;
return false;
}
int indx = std::distance(m_jointNames.begin(), it);
(*jacobian)(ii, indx) = 1;
indicies->at(ii) = indx;
}
return true;
}
///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 simpleLeggedOdometry::setJointsState(const KDL::JntArray& qj,
const KDL::JntArray& dqj,
const KDL::JntArray& ddqj)
{
if( qj.rows() != odometry_model->getNrOfDOFs() ||
dqj.rows() != odometry_model->getNrOfDOFs() ||
ddqj.rows() != odometry_model->getNrOfDOFs() )
{
return false;
}
bool ok = true ;
ok = ok && odometry_model->setAngKDL(qj);
ok = ok && odometry_model->setDAngKDL(dqj);
ok = ok && odometry_model->setD2AngKDL(ddqj);
//Update also the floating base position, given this new joint positions
KDL::Frame world_H_base = this->getWorldFrameTransform(odometry_model->getFloatingBaseLink());
ok = ok && odometry_model->setWorldBasePoseKDL(world_H_base);
return ok;
}
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);
}
int TreeIdSolver_RNE::CartToJnt(const KDL::JntArray &q, const KDL::JntArray &q_dot, const KDL::JntArray &q_dotdot, const Twist& base_velocity, const Twist& base_acceleration, const Wrenches& f_ext,JntArray &torques, Wrench& base_force)
{
if( q.rows() != undirected_tree.getNrOfDOFs() ||
q_dot.rows() != q.rows() ||
q_dotdot.rows() != q.rows() ||
f_ext.size() != undirected_tree.getNrOfLinks() ||
torques.rows() != q.rows() ) return -1;
base_force = Wrench::Zero();
rneaKinematicLoop(undirected_tree,q,q_dot,q_dotdot,traversal,base_velocity,base_acceleration,v,a);
rneaDynamicLoop(undirected_tree,q,traversal,v,a,f_ext,f,torques,base_force);
return 0;
}
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;
}
}
}
int ChainIdSolver_RNE_FB::CartToJnt(const KDL::JntArray &q, const KDL::JntArray &q_dot, const KDL::JntArray &q_dotdot, const Twist& base_velocity, const Twist& base_acceleration, const Wrenches& f_ext, KDL::JntArray &torques, Wrench& base_force)
{
//Check sizes when in debug mode
if(q.rows()!=nj || q_dot.rows()!=nj || q_dotdot.rows()!=nj || torques.rows()!=nj || f_ext.size()!=ns)
return -1;
unsigned int j=0;
//Sweep from root to leaf
for(unsigned int i=0;i<ns;i++){
double q_,qdot_,qdotdot_;
Segment segm;
segm = chain.getSegment(i);
if(segm.getJoint().getType()!=Joint::None){
q_=q(j);
qdot_=q_dot(j);
qdotdot_=q_dotdot(j);
j++;
}else
q_=qdot_=qdotdot_=0.0;
//Calculate segment properties: X,S,vj,cj
X[i]=segm.pose(q_);//Remark this is the inverse of the
//frame for transformations from
//the parent to the current coord frame
//Transform velocity and unit velocity to segment frame
Twist vj=X[i].M.Inverse(segm.twist(q_,qdot_));
S[i]=X[i].M.Inverse(segm.twist(q_,1.0));
//We can take cj=0, see remark section 3.5, page 55 since the unit velocity vector S of our joints is always time constant
//calculate velocity and acceleration of the segment (in segment coordinates)
if(i==0){
v[i]=X[i].Inverse(base_velocity)+vj;
a[i]=X[i].Inverse(base_acceleration)+S[i]*qdotdot_+v[i]*vj;
}else{
v[i]=X[i].Inverse(v[i-1])+vj;
a[i]=X[i].Inverse(a[i-1])+S[i]*qdotdot_+v[i]*vj;
}
//Calculate the force for the joint
//Collect RigidBodyInertia and external forces
RigidBodyInertia Ii=segm.getInertia();
f[i]=Ii*a[i]+v[i]*(Ii*v[i])-f_ext[i];
//std::cout << "aLink " << segm.getName() << "\na= " << a[i] << "\nv= " << v[i] << "\nf= " << f[i] << "\nf_ext= " << f_ext[i] << std::endl;
//std::cout << "a["<<i<<"]=" << a[i] << "\n f["<<i<<"]=" << f[i] << "\n S["<<i<<"]=" << S[i] << std::endl;
}
//Sweep from leaf to root
j=nj-1;
for(int i=ns-1;i>=0;i--){
Segment segm;
segm = chain.getSegment(i);
if(segm.getJoint().getType()!=Joint::None)
torques(j--)=dot(S[i],f[i]);
if(i!=0)
f[i-1]=f[i-1]+X[i]*f[i];
}
base_force = X[0]*f[0];
//debug
for(int i=0; i < (int)ns; i++) {
Segment segm;
segm = chain.getSegment(i);
//std::cout << "bLink " << segm.getName() << " a= " << a[i] << " f= " << f[i] << std::endl;
}
return 0;
}