RCArticulatedBody.h

/****************************************************************************
 * Copyright 2005 Evan Drumwright
 * This library is distributed under the terms of the GNU General Public 
 * License (found in COPYING).
 ****************************************************************************/

#ifndef _RC_ARTICULATED_BODY_H
#define _RC_ARTICULATED_BODY_H

#include <pthread.h>
#include <map>
#include <list>
#include <vector>
#include <ext/hash_map>
#include <boost/shared_ptr.hpp>
#include <Physsim/Constants.h>
#include <Physsim/HashClasses.h>
#include <Physsim/SVector6.h>
#include <Physsim/Vector3.h>
#include <Physsim/Matrix3.h>
#include <Physsim/ArticulatedBody.h>
#include <Physsim/RigidBody.h>

namespace Physsim {

class FSABAlgorithm;
class Joint;
class RCArticulatedBodyFwdDynAlgo;


class RCArticulatedBody : public ArticulatedBody
{
        public:
                RCArticulatedBody();
                RCArticulatedBody(const RCArticulatedBody& source);
                virtual ~RCArticulatedBody() {}
                virtual void clone(BasePtr& cloned, bimap<BasePtr, BasePtr>* obj_map = NULL) const;
                virtual bool is_struct_identical(BaseConstPtr object) const;
                virtual void load_state(BaseConstPtr object, bimap<BasePtr, BasePtr>* correspondence = NULL);
                virtual void save_state(BasePtr object, bimap<BasePtr, BasePtr>* correspondence = NULL) const;
                void set_links(const std::vector<RigidBodyPtr>& links);
                RigidBodyPtr find_link(const std::string& linkname) const;
                JointPtr find_joint(const std::string& jointname) const;
                void add_link(RigidBodyPtr link);
                virtual VectorN get_state_deriv();
                virtual VectorN get_state();
                virtual void set_state(const VectorN& state);
                MatrixN calc_jacobian_column(JointPtr joint, const Vector3& point);
                MatrixNN calc_jacobian_floating_base(const VectorN& point);
                virtual std::list<RigidBodyPtr> get_rigid_bodies() const;
                virtual void reset_accumulators();
                virtual std::list<sorted_pair<RigidBodyPtr> > get_adjacent_links() const;
                void add_joint(JointPtr joint);
                void set_joints(const std::vector<JointPtr > joints);
                void update_link_transforms() const;            
                void update_link_velocities();
                void apply_impulse(const Vector3& j, const Vector3& k, const Vector3& p, RigidBodyPtr link);
                virtual void compile();
                virtual void calc_fwd_dyn();
                virtual void update_visualization();
                virtual void load_from_xml(XMLTreeConstPtr node, std::map<std::string, BasePtr>& id_map);
                virtual void save_to_xml(XMLTreePtr node, std::list<BaseConstPtr>& shared_objects) const;
                RCArticulatedBodyPtr get_this() { return boost::dynamic_pointer_cast<RCArticulatedBody>(shared_from_this()); }
                RCArticulatedBodyConstPtr get_this() const { return boost::dynamic_pointer_cast<const RCArticulatedBody>(shared_from_this()); }

                // NOTE: the following three methods are currently disabled, pending
                // my decision on whether to keep them...               
                virtual bool joint_limit_collision(Real collision_tolerance) const { return false; }
                virtual void correct_joint_limit_collisions(Real collision_tolerance) {}
                virtual void correct_joint_limit_violations() {}

                RigidBodyPtr get_base_link() const { return _links.front(); }

                const std::vector<RigidBodyPtr>& get_links() const { return _links; }

                const std::vector<JointPtr>& get_joints() const { return _joints; }

                bool is_floating_base() const { return _floating_base; }

                void set_floating_base(bool flag) { _floating_base = flag; }

                RigidBodyPtr get_base() const { return _links.front(); }


                void set_computation_ref_frame(ReferenceFrameType type) { _computation_frame_type = type; }
                
                ReferenceFrameType get_computation_ref_frame() const { return _computation_frame_type; }

                boost::shared_ptr<RCArticulatedBodyFwdDynAlgo> get_forward_dynamics_algorithm() const { return _fdyn_algorithm; }       
                
                void set_forward_dynamics_algorithm(boost::shared_ptr<RCArticulatedBodyFwdDynAlgo> algorithm) { _fdyn_algorithm = algorithm; }  

        private:

                VectorN _state_deriv;

                std::vector<JointPtr> _joints;

                std::vector<RigidBodyPtr> _links;
        
                std::map<RigidBodyConstPtr, unsigned> _link_idx_map;

                bool _floating_base;
                
                boost::shared_ptr<RCArticulatedBodyFwdDynAlgo> _fdyn_algorithm;

                ReferenceFrameType _computation_frame_type;
}; // end class

} // end namespace
#endif

---