ArticulatedBody.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 ARTICULATED_BODY 
#error This class is not to be included by the user directly. Use ArticulatedBodyd.h or ArticulatedBodyf.h instead.
#endif

class RIGIDBODY;
class JOINT;

class ARTICULATED_BODY : public virtual DYNAMIC_BODY
{
  public:
    ARTICULATED_BODY();
    virtual ~ARTICULATED_BODY() {}
    virtual bool is_floating_base() const = 0;
    virtual boost::shared_ptr<RIGIDBODY> get_base_link() const = 0;
    unsigned num_constraint_eqns_explicit() const;
    unsigned num_constraint_eqns_implicit() const;
    virtual void rotate(const QUAT& q);
    virtual void translate(const ORIGIN3& o);
    virtual REAL calc_kinetic_energy(boost::shared_ptr<const POSE3> P = boost::shared_ptr<const POSE3>());
    boost::shared_ptr<RIGIDBODY> find_link(const std::string& id) const; 
    boost::shared_ptr<JOINT> find_joint(const std::string& id) const; 
    void get_adjacent_links(std::list<sorted_pair<boost::shared_ptr<RIGIDBODY> > >& links) const;
    virtual void set_links_and_joints(const std::vector<boost::shared_ptr<RIGIDBODY> >& links, const std::vector<boost::shared_ptr<JOINT> >& joints);
    virtual unsigned num_joint_dof() const;
    void find_loops(std::vector<unsigned>& loop_indices, std::vector<std::vector<unsigned> >& loop_links) const;
    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);
    virtual MATRIXN& calc_jacobian(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> target_pose, boost::shared_ptr<DYNAMIC_BODY> body, MATRIXN& J);

    virtual bool is_enabled() const { return true; }

    virtual unsigned num_joint_dof_explicit() const = 0;

    virtual unsigned num_joint_dof_implicit() const = 0;

    virtual const std::vector<boost::shared_ptr<RIGIDBODY> >& get_links() const { return _links; }

    virtual const std::vector<boost::shared_ptr<JOINT> >& get_joints() const { return _joints; }

    virtual const std::vector<boost::shared_ptr<JOINT> >& get_explicit_joints() const = 0; 

    virtual const std::vector<boost::shared_ptr<JOINT> >& get_implicit_joints() const = 0; 

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

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


    virtual void apply_impulse(const SMOMENTUM& w, boost::shared_ptr<RIGIDBODY> link) = 0;
      
    virtual void reset_accumulators() = 0;

  protected:
    std::vector<unsigned> _processed;


    virtual void compile();

    std::vector<boost::shared_ptr<RIGIDBODY> > _links;

    std::vector<boost::shared_ptr<JOINT> > _joints;

  private:
    ARTICULATED_BODY(const ARTICULATED_BODY& ab) {}

    // joint constraint violation
    std::vector<REAL> _cvio;

    // joint velocity tolerances (for joints at constraints)
    std::vector<REAL> _cvel_vio;

    // temporary variables
    VECTORN _dq;

}; // end class