Joint.h

/****************************************************************************
 * Copyright 2014 Evan Drumwright
 * This library is distributed under the terms of the Apache V2.0 license 
 ****************************************************************************/

#ifndef JOINT
#error This class is not to be included by the user directly. Use Jointd.h or Jointf.h instead.
#endif

class ARTICULATED_BODY;
class RIGIDBODY;

class JOINT : public virtual_enable_shared_from_this<JOINT>
{
  public:
    enum ConstraintType { eUnknown, eExplicit, eImplicit };
    enum DOFs { DOF_1=0, DOF_2=1, DOF_3=2, DOF_4=3, DOF_5=4, DOF_6=5 };

    JOINT();
    virtual const std::vector<SVELOCITY>& get_spatial_axes();
    void add_force(const VECTORN& force);
    void reset_force(); 
    ConstraintType get_constraint_type() const { return _constraint_type; }

    std::string joint_id;

    void set_constraint_type(ConstraintType type) { _constraint_type = type; }


    boost::shared_ptr<ARTICULATED_BODY> get_articulated_body() { return (_abody.expired()) ? boost::shared_ptr<ARTICULATED_BODY>() : boost::shared_ptr<ARTICULATED_BODY>(_abody); }


    void set_articulated_body(boost::shared_ptr<ARTICULATED_BODY> abody) { _abody = abody; }

 
    virtual void set_inboard_link(boost::shared_ptr<RIGIDBODY> link, bool update_pose);
    virtual void set_outboard_link(boost::shared_ptr<RIGIDBODY> link, bool update_pose);
    void set_location(const VECTOR3& p, boost::shared_ptr<RIGIDBODY> inboard, boost::shared_ptr<RIGIDBODY> outboard);
    VECTOR3 get_location(bool use_outboard = false) const;
    virtual void set_inboard_pose(boost::shared_ptr<const POSE3> inboard_pose, bool update_joint_pose);
    virtual void set_outboard_pose(boost::shared_ptr<POSE3> outboard_pose, bool update_joint_pose);
    virtual void update_spatial_axes();
    virtual void evaluate_constraints_dot(REAL C[]);
    virtual void set_q_tare(const VECTORN& tare) { _q_tare = tare; }
    virtual const VECTORN& get_q_tare() const { return _q_tare; }  

    boost::shared_ptr<RIGIDBODY> get_inboard_link() const { return (_inboard_link.expired()) ? boost::shared_ptr<RIGIDBODY>() : boost::shared_ptr<RIGIDBODY>(_inboard_link); }

    boost::shared_ptr<RIGIDBODY> get_outboard_link() const { return (_outboard_link.expired()) ? boost::shared_ptr<RIGIDBODY>() : boost::shared_ptr<RIGIDBODY>(_outboard_link); }

    VECTORN qdd;

    VECTORN force;

    VECTORN lambda;

    unsigned get_index() const { return _joint_idx; }


    void set_index(unsigned index) { _joint_idx = index; }

    unsigned get_constraint_index() const { return _constraint_idx; }

    void set_constraint_index(unsigned idx) { _constraint_idx = idx; } 


    void set_coord_index(unsigned index) { _coord_idx = index; }

    unsigned get_coord_index() const { return _coord_idx; }

    boost::shared_ptr<const POSE3> get_pose() const { return _F; };

    boost::shared_ptr<const POSE3> get_pose_from_outboard() const { return _Fb; };


    virtual bool is_singular_config() const = 0;

    virtual unsigned num_constraint_eqns() const { return 6 - num_dof(); }


    virtual void evaluate_constraints(REAL C[]) = 0;


    virtual const std::vector<SVELOCITY>& get_spatial_axes_dot() = 0;


    virtual boost::shared_ptr<const POSE3> get_induced_pose() = 0;

    virtual void determine_q(VECTORN& q) = 0;

    virtual unsigned num_dof() const = 0;
  
    VECTORN q;

    VECTORN qd;


    virtual void calc_constraint_jacobian(bool inboard, MATRIXN& Cq) = 0;
 

    virtual void calc_constraint_jacobian_dot(bool inboard, MATRIXN& Cq) = 0;

  protected:
    void calc_constraint_jacobian_numeric(bool inboard, MATRIXN& Cq);
    bool transform_jacobian(MATRIXN& J, bool use_inboard, MATRIXN& output);
    void invalidate_pose_vectors() { get_outboard_link()->invalidate_pose_vectors(); }
    boost::shared_ptr<const POSE3> get_inboard_pose() { if (_inboard_link.expired()) return boost::shared_ptr<const POSE3>(); return get_inboard_link()->get_pose(); }
    boost::shared_ptr<const POSE3> get_outboard_pose() { if (_outboard_link.expired()) return boost::shared_ptr<const POSE3>(); return get_outboard_link()->get_pose(); }

    boost::shared_ptr<POSE3> _Fprime;

    boost::shared_ptr<POSE3> _F;

    boost::shared_ptr<POSE3> _Fb;


    virtual void init_data();


    std::vector<SVELOCITY> _s;


    VECTORN _q_tare;

    boost::weak_ptr<RIGIDBODY> _inboard_link;
    boost::weak_ptr<RIGIDBODY> _outboard_link;
    boost::weak_ptr<ARTICULATED_BODY> _abody;
    ConstraintType _constraint_type;
    unsigned _joint_idx;
    unsigned _coord_idx;
    unsigned _constraint_idx;
}; // end class