DHParam.h

Go to the documentation of this file.

#ifndef URDF2GRASPIT_DHPARAM_H
#define URDF2GRASPIT_DHPARAM_H
// Copyright Jennifer Buehler

#include <urdf/model.h>
#include <urdf_traverser/Types.h>
#include <Eigen/Core>
#include <Eigen/Geometry>

namespace urdf2graspit
{

class DHParam
{
public:
    typedef urdf_traverser::EigenTransform EigenTransform;
    typedef urdf_traverser::JointConstPtr JointConstPtr;
    typedef urdf_traverser::LinkConstPtr LinkConstPtr;

    DHParam():
        joint(JointConstPtr()),
        dof_index(-1),
        d(0),
        r(0),
        theta(0),
        alpha(0) { }
    DHParam(const DHParam& p):
        joint(p.joint),
        childLink(p.childLink),
        dof_index(p.dof_index),
        d(p.d),
        r(p.r),
        theta(p.theta),
        alpha(p.alpha) { }

    // URDF joint which is located at reference frame (i).
    // The DH parameters transform to frame (i+1).
    JointConstPtr joint;
    LinkConstPtr childLink;
    int dof_index;   // < index of this dh-parameter in the list of DOF specifications of graspit
    double d;  // < distance along previous z
    double r;  // < orthogonal distance from previous z axis to current z
    double theta;  // < rotation of previous x around previous z to current x
    double alpha;  // < rotation of previous z to current z

    DHParam& operator=(const DHParam& p)
    {
        if (&p == this) return *this;
        joint = p.joint;
        childLink = p.childLink;
        dof_index = p.dof_index;
        d = p.d;
        r = p.r;
        theta = p.theta;
        alpha = p.alpha;
        return *this;
    }


    friend std::ostream& operator<<(std::ostream& o, const DHParam& p)
    {
        o << p.joint->name << ": d=" << p.d << ", r=" << p.r << ", theta=" << p.theta
          << ", alpha=" << p.alpha << ", dof_idx=" << p.dof_index;
        if (p.joint->type == urdf::Joint::REVOLUTE)
         o << " (revolute)";
        else if (p.joint->type == urdf::Joint::CONTINUOUS)
         o << " (continuous)";
        else if (p.joint->type == urdf::Joint::PRISMATIC)
         o << " (prismatic)";
        else
         o << " (other type = " << p.joint->type << ")";
        return o;
    }

    static bool toDenavitHartenberg(DHParam& param,
                                    const Eigen::Vector3d& zi_1,
                                    const Eigen::Vector3d& xi_1,
                                    const Eigen::Vector3d& pi_1,
                                    const Eigen::Vector3d& zi,
                                    const Eigen::Vector3d& pi,
                                    Eigen::Vector3d& xi);

    static EigenTransform getTransform(const DHParam& p);


    static bool getTransforms(const std::vector<DHParam>& dh, const bool dh2urdf, std::map<std::string,EigenTransform>& transforms);

private:
/*
     * \param r the distance between them along the common normal \e xi.
     static bool getRAndAlpha(const Eigen::Vector3d& zi_1, const Eigen::Vector3d& zi,
                             const Eigen::Vector3d& pi_1, const Eigen::Vector3d& pi, double& r, double& alpha,
                             Eigen::Vector3d& commonNormal, Eigen::Vector3d& nOriginOnZi_1);*/
      static bool getAlpha(const Eigen::Vector3d& zi_1, const Eigen::Vector3d& zi,
                           const Eigen::Vector3d& pi_1, const Eigen::Vector3d& pi,
                           const Eigen::Vector3d& xi,
                           double& alpha);



    static bool getDAndTheta(const Eigen::Vector3d& zi_1, const Eigen::Vector3d& xi_1, const Eigen::Vector3d& pi_1,
                             const Eigen::Vector3d& xi, const Eigen::Vector3d& normOriginOnZi_1,
                             double& d, double& theta);


    static bool getCommonNormal(const Eigen::Vector3d& zi_1, const Eigen::Vector3d& zi, const Eigen::Vector3d& pi_1,
                                const Eigen::Vector3d& pi, Eigen::Vector3d& commonNormal,
                                Eigen::Vector3d& nOriginOnZi_1, double& shortestDistance, bool& parallel);


    static EigenTransform getTransform(const urdf::Pose& p);

    // Get joint transform to parent
    static EigenTransform getTransform(const DHParam::JointConstPtr& joint);

};

}  //  namespace urdf2graspit
#endif  //  URDF2GRASPIT_DHPARAM_H