Joint.h

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

#ifndef _JOINT_H
#define _JOINT_H

#include <boost/weak_ptr.hpp>
#include <boost/shared_ptr.hpp>
#include <Physsim/Base.h>
#include <Physsim/Vector3.h>
#include <Physsim/SMatrixN.h>
#include <Physsim/Visualizable.h>

namespace Physsim {

class VisualizationData;


class Joint : public Visualizable
{
        public:
                enum DOFs { DOF_1=0, DOF_2=1, DOF_3=2, DOF_4=3, DOF_5=4, DOF_6=5 };

                Joint();
                Joint(boost::weak_ptr<RigidBody> inboard, boost::weak_ptr<RigidBody> outboard);
                virtual ~Joint() {}
                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_q(const VectorN& q);
                void set_qd(const VectorN& qd);
                void add_force(const VectorN& force);
                void reset_force();             
                virtual const SMatrixN& get_spatial_axes(ReferenceFrameType type);
                void reset_spatial_axis();
                Vector3 get_position_global() const;
                VectorN get_scaled_force();
                VectorN compute_joint_friction_forces() const;
                virtual void save_to_xml(XMLTreePtr node, std::list<BaseConstPtr>& shared_objects) const;
                virtual void load_from_xml(XMLTreeConstPtr node, std::map<std::string, BasePtr>& id_map);
                virtual void set_inboard_link(RigidBodyPtr link);
                virtual void set_outboard_link(RigidBodyPtr link);

                virtual bool is_singular_config() const = 0;

                JointPtr get_this() { return boost::dynamic_pointer_cast<Joint>(shared_from_this()); }

                JointConstPtr get_this() const { return boost::dynamic_pointer_cast<const Joint>(shared_from_this()); }


//              virtual unsigned num_constraint_eqns() const = 0;


//              virtual MatrixN get_constraint_jacobian() = 0;


//              virtual MatrixN get_constraint_jacobian_dot() = 0;


//              virtual double eval_constraint_eqn(unsigned i) const = 0;


//              virtual double eval_constraint_dot_eqn(unsigned i) const = 0;

                virtual const SMatrixN& get_spatial_axes_deriv(ReferenceFrameType type) = 0;

                virtual const Matrix4& get_transform() = 0;

                virtual void update_spatial_axes() = 0;

                virtual void determine_Q() = 0;

                virtual unsigned get_num_dof() const = 0;
        
                const VectorN& get_max_force() const { return _maxforce; }

                const VectorN& get_q() const { return _q; }

                const VectorN& get_qd() const { return _qd; }

                const VectorN& get_qdd() const { return _qdd; }


                const VectorN& get_low_limit() const { return _lolimit; }


                const VectorN& get_high_limit() const { return _hilimit; }


                Real get_joint_limit_rcoeff() const { return _joint_limit_rcoeff; }
                

                Real get_joint_limit_kp() const { return _joint_limit_kp; }
                

                Real get_joint_limit_kv() const { return _joint_limit_kv; }
        

                const VectorN& get_joint_viscous_mu() const { return _fv; }


                const VectorN& get_joint_coulomb_mu() const { return _fc; }
        
                boost::weak_ptr<RigidBody> get_inboard_link() const { return _inboard_link; }

                boost::weak_ptr<RigidBody> get_outboard_link() const { return _outboard_link;}

                const VectorN& get_force() const { return _force; }


                void set_joint_limit_rcoeff(Real joint_limit_rcoeff) { assert(joint_limit_rcoeff >= 0 && joint_limit_rcoeff <= 1); _joint_limit_rcoeff = joint_limit_rcoeff; } 
                

                void set_joint_limit_kp(Real kp) { assert(kp >= 0); _joint_limit_kp = kp; }
                

                void set_joint_limit_kv(Real kv) { assert(kv >= 0); _joint_limit_kv = kv; }
                
                void set_max_force(const VectorN& maxforce) { assert(maxforce.size() == get_num_dof()); _maxforce = maxforce; }

                void set_qdd(const VectorN& qdd) { assert(qdd.size() == get_num_dof()); _qdd = qdd; }


                void set_joint_viscous_mu(const VectorN& mu) { assert(mu.size() == get_num_dof()); assert(VectorN::dot(mu, VectorN::one(get_num_dof())) >= 0); _fv = mu; }


                void set_joint_coulomb_mu(const VectorN& mu) { assert(mu.size() == get_num_dof()); assert(VectorN::dot(mu, VectorN::one(get_num_dof())) >= 0); _fc = mu; }


                void set_low_limit(const VectorN& lolimit) { assert(lolimit.size() == get_num_dof()); _lolimit = lolimit; }


                void set_high_limit(const VectorN& hilimit) { assert(hilimit.size() == get_num_dof()); _hilimit = hilimit; }


                boost::weak_ptr<ArticulatedBody> get_articulated_body() { return _abody; }


                void set_articulated_body(ArticulatedBodyPtr abody) { _abody = abody; }

        protected:


                void init_data();

                SMatrixN _si;
                SMatrixNPtr _s0;
                
        private:

                VectorN _lolimit, _hilimit;
                VectorN _maxforce;
                VectorN _q;
                VectorN _qd;
                VectorN _qdd;
                VectorN _fc;
                VectorN _fv;
                boost::weak_ptr<RigidBody> _inboard_link;
                boost::weak_ptr<RigidBody> _outboard_link;
                boost::weak_ptr<ArticulatedBody> _abody;
                VectorN _force;
                Real _joint_limit_rcoeff;
                Real _joint_limit_kp;
                Real _joint_limit_kv;
}; // end class
} // end namespace

#endif

---