RigidBody.h

/****************************************************************************
 * Copyright 2015 Evan Drumwright
 * This library is distributed under the terms of the Apache V2.0
 * License (obtainable from http://www.apache.org/licenses/LICENSE-2.0).
 ****************************************************************************/

#ifndef RIGIDBODY 
#error This class is not to be included by the user directly. Use RigidBodyd.h or RigidBodyf.h instead.
#endif

class JOINT;
class ARTICULATED_BODY;


class RIGIDBODY : public virtual SINGLE_BODY 
{
  friend class ARTICULATED_BODY;
  friend class RC_ARTICULATED_BODY;
  friend class MCARTICULATED_BODY;
  friend class JOINT;

  public:
    enum Compliance { eRigid, eCompliant};
    RIGIDBODY();
    virtual ~RIGIDBODY() {}
    void add_force(const SFORCE& w);
    void set_pose(const POSE3& pose);
    void apply_impulse(const SMOMENTUM& w);
    virtual void rotate(const QUAT& q);
    virtual void translate(const ORIGIN3& o);
    virtual void calc_fwd_dyn();
    const SPATIAL_RB_INERTIA& get_inertia();
    void set_inertia(const SPATIAL_RB_INERTIA& J);

    bool is_child_link(boost::shared_ptr<const RIGIDBODY> query) const;
    bool is_descendant_link(boost::shared_ptr<const RIGIDBODY> query) const;
    const SVELOCITY& get_velocity();
    void set_velocity(const SVELOCITY& xd);
    void set_accel(const SACCEL& xdd);
    virtual const SACCEL& get_accel();
    void set_velocity(const SACCEL& xdd);
    virtual void set_generalized_forces(const SHAREDVECTORN& gf);
    virtual void set_generalized_forces(const VECTORN& gf) { DYNAMIC_BODY::set_generalized_forces(gf); }
    virtual SHAREDMATRIXN& get_generalized_inertia(SHAREDMATRIXN& M);
    virtual MATRIXN& get_generalized_inertia(MATRIXN& M) { return DYNAMIC_BODY::get_generalized_inertia(M); }
    virtual SHAREDVECTORN& get_generalized_coordinates_euler(SHAREDVECTORN& gc);
    virtual VECTORN& get_generalized_coordinates_euler(VECTORN& gc) { return DYNAMIC_BODY::get_generalized_coordinates_euler(gc); }
    virtual SHAREDVECTORN& get_generalized_velocity(DYNAMIC_BODY::GeneralizedCoordinateType gctype, SHAREDVECTORN& gv);
    virtual VECTORN& get_generalized_velocity(DYNAMIC_BODY::GeneralizedCoordinateType gctype, VECTORN& gv) { return DYNAMIC_BODY::get_generalized_velocity(gctype, gv); }
    virtual SHAREDVECTORN& get_generalized_acceleration(SHAREDVECTORN& ga);
    virtual void add_generalized_force(const SHAREDVECTORN& gf);
    virtual void add_generalized_force(const VECTORN& gf) { DYNAMIC_BODY::add_generalized_force(gf); }
    virtual void apply_generalized_impulse(const SHAREDVECTORN& gf);
    virtual void apply_generalized_impulse(const VECTORN& gj) { DYNAMIC_BODY::apply_generalized_impulse(gj); }
    virtual void set_generalized_coordinates_euler(const SHAREDVECTORN& gc);
    virtual void set_generalized_coordinates_euler(const VECTORN& gc) { DYNAMIC_BODY::set_generalized_coordinates_euler(gc); }
    virtual void set_generalized_acceleration(const SHAREDVECTORN& ga);
    virtual void set_generalized_velocity(DYNAMIC_BODY::GeneralizedCoordinateType gctype, const SHAREDVECTORN& gv);
    virtual void set_generalized_velocity(DYNAMIC_BODY::GeneralizedCoordinateType gctype, const VECTORN& gv) { DYNAMIC_BODY::set_generalized_velocity(gctype, gv); }
    virtual SHAREDVECTORN& get_generalized_forces(SHAREDVECTORN& f);
    virtual VECTORN& get_generalized_forces(VECTORN& f) { return DYNAMIC_BODY::get_generalized_forces(f); }
    virtual SHAREDVECTORN& convert_to_generalized_force(boost::shared_ptr<SINGLE_BODY> body, const SFORCE& w, SHAREDVECTORN& gf);
    virtual VECTORN& convert_to_generalized_force(boost::shared_ptr<SINGLE_BODY> body, const SFORCE& w, VECTORN& gf) { return DYNAMIC_BODY::convert_to_generalized_force(body, w, gf); }
    virtual unsigned num_generalized_coordinates(DYNAMIC_BODY::GeneralizedCoordinateType gctype) const;
    virtual SHAREDMATRIXN& transpose_solve_generalized_inertia(const SHAREDMATRIXN& B, SHAREDMATRIXN& X);
    SHAREDMATRIXN& transpose_solve_generalized_inertia_single(const SHAREDMATRIXN& B, SHAREDMATRIXN& X);
    virtual SHAREDMATRIXN& solve_generalized_inertia(const SHAREDMATRIXN& B, SHAREDMATRIXN& X);
    virtual SHAREDVECTORN& solve_generalized_inertia(const SHAREDVECTORN& b, SHAREDVECTORN& x);
    boost::shared_ptr<RIGIDBODY> get_parent_link() const;
    boost::shared_ptr<JOINT> get_inner_joint_explicit() const;
    void add_inner_joint(boost::shared_ptr<JOINT> j);
    void add_outer_joint(boost::shared_ptr<JOINT> j);
    void remove_inner_joint(boost::shared_ptr<JOINT> joint);
    void remove_outer_joint(boost::shared_ptr<JOINT> joint);
    virtual REAL calc_kinetic_energy(boost::shared_ptr<const POSE3> P = boost::shared_ptr<const POSE3>());
    virtual VECTOR3 calc_point_vel(const VECTOR3& p) const;
    bool is_base() const;
    bool is_ground() const;
    virtual boost::shared_ptr<const POSE3> get_computation_frame() const;
    virtual void set_computation_frame_type(ReferenceFrameType rftype);
    virtual MATRIXN& calc_jacobian(boost::shared_ptr<const POSE3> source_pose, boost::shared_ptr<const POSE3> target_pose, boost::shared_ptr<DYNAMIC_BODY> body, MATRIXN& J);
    virtual MATRIXN& calc_jacobian_dot(boost::shared_ptr<const POSE3> source_pose, boost::shared_ptr<const POSE3> target_pose, boost::shared_ptr<DYNAMIC_BODY> body, MATRIXN& J);
    const SFORCE& sum_forces();
    void reset_accumulators();
    SFORCE calc_euler_torques();
    void set_enabled(bool flag);
    void invalidate_pose_vectors();

    template <class OutputIterator>
    OutputIterator get_parent_links(OutputIterator begin) const;

    template <class OutputIterator>
    OutputIterator get_child_links(OutputIterator begin) const;

    boost::shared_ptr<RIGIDBODY> get_this() { return boost::dynamic_pointer_cast<RIGIDBODY>(shared_from_this()); }

    boost::shared_ptr<const RIGIDBODY> get_this() const { return boost::dynamic_pointer_cast<const RIGIDBODY>(shared_from_this()); }

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

    // Gets the pose used in generalized coordinates calculations
    boost::shared_ptr<const POSE3> get_gc_pose() const { return _F2; }

    // Gets the "mixed" pose of this body (pose origin at the body's reference point but pose aligned with global frame)
    boost::shared_ptr<const POSE3> get_mixed_pose() const { return _F2; }

    REAL calc_mass() const { return _Ji.m; }

    virtual REAL get_mass() const { return _Ji.m; }

    virtual bool is_enabled() const { return _enabled; }


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


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

    unsigned num_child_links() const { return _outer_joints.size(); }

    bool is_end_effector() const { assert (!_abody.expired()); return !is_base() && _outer_joints.empty(); }

    void clear_inner_joints() { _inner_joints.clear(); }

    void clear_outer_joints() { _outer_joints.clear(); }

    unsigned get_index() const { return _link_idx; }


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

    const std::set<boost::shared_ptr<JOINT> >& get_inner_joints() const { return _inner_joints; }

    const std::set<boost::shared_ptr<JOINT> >& get_outer_joints() const { return _outer_joints; }

    Compliance compliance;

  private:
    template <class V>
    void get_generalized_coordinates_euler_generic(V& gc);

    template <class V>
    void get_generalized_velocity_generic(DYNAMIC_BODY::GeneralizedCoordinateType gctype, V& gv);

    template <class V>
    void get_generalized_acceleration_generic(V& ga);

    template <class V>
    void set_generalized_coordinates_euler_generic(V& gc);

    template <class V>
    void set_generalized_velocity_generic(DYNAMIC_BODY::GeneralizedCoordinateType gctype, V& gv);

    template <class V>
    void set_generalized_acceleration_generic(V& ga);

    void set_force(const SFORCE& w);
    void apply_generalized_impulse_single(const SHAREDVECTORN& gf);
    SHAREDMATRIXN& get_generalized_inertia_single(SHAREDMATRIXN& M);
    virtual SHAREDMATRIXN& get_generalized_inertia_inverse(SHAREDMATRIXN& M) const;
    SHAREDVECTORN& get_generalized_forces_single(SHAREDVECTORN& f);
    SHAREDVECTORN& convert_to_generalized_force_single(boost::shared_ptr<SINGLE_BODY> body, const SFORCE& w, SHAREDVECTORN& gf);
    unsigned num_generalized_coordinates_single(DYNAMIC_BODY::GeneralizedCoordinateType gctype) const;
    SHAREDMATRIXN& solve_generalized_inertia_single(const SHAREDMATRIXN& B, SHAREDMATRIXN& X);
    SHAREDVECTORN& solve_generalized_inertia_single(const SHAREDVECTORN& b, SHAREDVECTORN& x);
    boost::shared_ptr<RIGIDBODY> get_parent_link(boost::shared_ptr<JOINT> j) const;
    boost::shared_ptr<RIGIDBODY> get_child_link(boost::shared_ptr<JOINT> j) const;

    bool _xdi_valid;

    bool _xdj_valid;

    bool _xd0_valid;

    bool _xddi_valid;

    bool _xdd0_valid;

    bool _forcei_valid;

    bool _forcej_valid;

    bool _force0_valid;

    bool _J0_valid;

    bool _Jcom_valid;

    bool _Jj_valid;

    SPATIAL_RB_INERTIA _J0;

    SVELOCITY _xd0;

    SACCEL _xdd0;

    SFORCE _force0;

    SPATIAL_RB_INERTIA _Ji;

    SVELOCITY _xdi;

    SACCEL _xddi;

    SFORCE _forcei;

    SPATIAL_RB_INERTIA _Jcom;

    SVELOCITY _xdcom;

    SACCEL _xddcom;

    SFORCE _forcecom;

    unsigned _link_idx;

    bool _enabled;

  protected:
    void update_mixed_pose();

    bool _xddj_valid;

    SPATIAL_RB_INERTIA _Jj;

    SVELOCITY _xdj;

    SFORCE _forcej;

    SACCEL _xddj;

    boost::shared_ptr<POSE3> _F;

    boost::shared_ptr<POSE3> _F2;

    boost::weak_ptr<ARTICULATED_BODY> _abody;

    std::set<boost::shared_ptr<JOINT> > _inner_joints;

    std::set<boost::shared_ptr<JOINT> > _outer_joints;

}; // end class

// incline inline functions
#include "RigidBody.inl"