Пример #1
0
 void convertKDLJacToVispMat(const KDL::Jacobian &kdlJac,
                             vpMatrix &vispMat)
 {
   for(unsigned int i=0; i<kdlJac.rows(); ++i)
   {
     for(unsigned int j=0; j<kdlJac.columns(); ++j)
     {
       vispMat[i][j] = kdlJac.data(i,j);
     }
   }
 }
Пример #2
0
  vpMatrix kdlJacobianToVispJacobian(const KDL::Jacobian& kdlJacobian,
                                     const vpHomogeneousMatrix& bMe)
  {
    vpMatrix vpJacobian(kdlJacobian.rows(), kdlJacobian.columns());

    KDL::Jacobian kdlJacobianCopy = kdlJacobian;

    // Fix reference point so that it is in the base frame
    vpTranslationVector bOe; //origin of end frame in base frame
    bMe.extract(bOe);
    kdlJacobianCopy.changeRefPoint(KDL::Vector(-bOe[0], -bOe[1], -bOe[2]));
    pal::convertKDLJacToVispMat(kdlJacobianCopy, vpJacobian);// KDL data --> Visp data
    return vpJacobian;
  }
    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);
    }
    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;
        }
    }
Пример #5
0
 vpMatrix convertKDLJacToVispMat(const KDL::Jacobian& kdlJac)
 {
   vpMatrix vispMat(kdlJac.rows(), kdlJac.columns());
   convertKDLJacToVispMat(kdlJac, vispMat);
   return vispMat;
 }