taoJoint.h

/* Copyright (c) 2005 Arachi, Inc. and Stanford University. All rights reserved.
 *
 * Permission is hereby granted, free of charge, to any person obtaining
 * a copy of this software and associated documentation files (the
 * "Software"), to deal in the Software without restriction, including
 * without limitation the rights to use, copy, modify, merge, publish,
 * distribute, sublicense, and/or sell copies of the Software, and to
 * permit persons to whom the Software is furnished to do so, subject
 * to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included
 * in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
 * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
 * IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY
 * CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
 * TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE
 * SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */

#ifndef _taoJoint_h
#define _taoJoint_h

#include "taoDJoint.h"
#include "taoABJoint.h"
#include "taoVar.h"
#include "taoTypes.h"
#include <tao/matrix/TaoDeMath.h>

#include <cassert>
#include <iostream>

class taoJoint : public taoDJoint
{
public:
  taoJoint() : _type(scl::JOINT_TYPE_NOTASSIGNED), _clamp_dQ(0), _max_dQ(5), _damping(0), _inertia(0), _var(NULL), _next(NULL) {}
  virtual ~taoJoint();

  virtual deInt getDOF() = 0;
  virtual void reset() = 0;

  virtual void setDVar(taoDVar* var) { _var = var; }
  virtual taoDVar* getDVar() { return _var; }
  virtual taoDVar const * getDVar() const { return _var; }

  void setType(scl::EJointType t) { _type = t; }
  scl::EJointType getType() const { return _type; }

  virtual void setABJoint(taoABJoint* joint) = 0;
  virtual taoABJoint* getABJoint() = 0;
  virtual taoABJoint const * getABJoint() const = 0;

  void setDQclamp(deInt b) { _clamp_dQ = b; }
  deInt getDQclamp() { return _clamp_dQ; }
  
  void setNext(taoJoint* joint) { _next = joint; }
  taoJoint* getNext() { return _next; }

  virtual void setDQmax(deFloat dq) { _max_dQ = dq; }
  virtual deFloat getDQmax() { return _max_dQ; }

  virtual void setDamping(deFloat d) { _damping = d; }
  virtual deFloat getDamping() { return _damping; }

  virtual void setInertia(deFloat i) { _inertia = i; }
  virtual deFloat getInertia() { return _inertia; }

  virtual void clampDQ() = 0;
  virtual void integrate(const deFloat dt) = 0;
  virtual void updateFrameLocal(deFrame* local) = 0;
  
  virtual void setTau(const deFloat* v) = 0;
        virtual void zeroTau() = 0;
  
  virtual void setDDQ(const deFloat* v) = 0;
  virtual void zeroDDQ() = 0;
  
  virtual void setDQ(const deFloat* v) = 0;
  virtual void zeroDQ() = 0;
  
  virtual void setQ(const deFloat* v) = 0;
  virtual void zeroQ() = 0;
  
  virtual void getTau(deFloat* v) const = 0;
  
  virtual void getDDQ(deFloat* v) const = 0;
  
  virtual void getDQ(deFloat* v) const = 0;
  
  virtual void getQ(deFloat* v) const = 0;
  
  virtual void getJgColumns(deVector6 * Jg_columns) const = 0;
  
private:
  scl::EJointType _type;

  deInt _clamp_dQ;
  deFloat _max_dQ;

  deFloat _damping;
  deFloat _inertia;

  taoDVar* _var;
  taoJoint* _next;
};

class taoJointSpherical : public taoJoint
{
public:
  taoJointSpherical();

  virtual deInt getDOF() { return 3; }
  virtual void reset();

  virtual taoVarSpherical* getVarSpherical() { return (taoVarSpherical*)getDVar(); }
  virtual taoVarSpherical const * getVarSpherical() const { return (taoVarSpherical*)getDVar(); }

  virtual void setABJoint(taoABJoint* joint)
  {
    taoABJointSpherical * tmp = dynamic_cast<taoABJointSpherical *>(joint);
    if(NULL == tmp)
    {
      std::cout <<"\ntaoJointPrismatic::setABJoint(): Incorrect joint type passed. Can't set ABJoint";
      assert(false);
    }
    if(NULL== _abJoint) { delete _abJoint; }
    _abJoint = tmp;
  }

  virtual taoABJoint* getABJoint(){ return _abJoint; }
  virtual taoABJoint const * getABJoint() const { return _abJoint; }

  virtual void addQdelta();
  virtual void addDQdelta();
  virtual void zeroTau() { getVarSpherical()->force_gc_.zero(); }
  virtual void zeroDDQ() { getVarSpherical()->ddq_.zero(); }
  virtual void zeroDQ() { getVarSpherical()->dq_.zero(); getVarSpherical()->dq_rotated_.zero(); }
  virtual void zeroQ() { getVarSpherical()->q_quat_.identity(); }
  virtual void setTau(const deFloat* v) { getVarSpherical()->force_gc_.set(v); }
  virtual void setDDQ(const deFloat* v) { getVarSpherical()->ddq_.set(v); }
  virtual void setDQ(const deFloat* v) { getVarSpherical()->dq_.set(v); }
  virtual void setQ(const deFloat* v) { getVarSpherical()->q_quat_.set(v); }
  virtual void getTau(deFloat* v) const { getVarSpherical()->force_gc_.get(v); }
  virtual void getDDQ(deFloat* v) const { getVarSpherical()->ddq_.get(v); }
  virtual void getDQ(deFloat* v) const { getVarSpherical()->dq_.get(v); }
  virtual void getQ(deFloat* v) const { deQuaternion const & q = getVarSpherical()->q_quat_; v[0] = q[0]; v[1] = q[1]; v[2] = q[2]; v[3] = q[3]; }

  virtual void clampDQ();
  virtual void integrate(const deFloat dt);
  virtual void updateFrameLocal(deFrame* local) { local->rotation() = getVarSpherical()->q_quat_; }

  virtual deMatrix3* getJg();
  
  virtual void getJgColumns(deVector6 * Jg_columns) const;
  
protected:
  taoABJointSpherical * _abJoint;

};

class taoJointDOF1 : public taoJoint
{
public:
  taoJointDOF1(taoJointType axis) : _axis(axis) {}

  taoJointType getJointType() const { return _axis; }
  virtual deInt getDOF() { return 1; }

  virtual deVector6& getS();

  virtual void reset() 
  {
    getVarDOF1()->q_ = 0;
    getVarDOF1()->dq_ = 0;
    getVarDOF1()->ddq_ = 0;
    getVarDOF1()->tau_ = 0;
  }

  virtual taoVarDOF1* getVarDOF1() { return (taoVarDOF1*)getDVar(); }
  virtual taoVarDOF1 const * getVarDOF1() const { return (taoVarDOF1*)getDVar(); }

  virtual void addQdelta() { getVarDOF1()->q_ += getVarDOF1()->ddq_; }
  virtual void addDQdelta() { getVarDOF1()->dq_ += getVarDOF1()->ddq_; if (getDQclamp()) clampDQ(); }
  virtual void zeroTau() { getVarDOF1()->tau_ = 0; }
  virtual void zeroDDQ() { getVarDOF1()->ddq_ = 0; }
  virtual void zeroDQ() { getVarDOF1()->dq_ = 0; }
  virtual void zeroQ() { getVarDOF1()->q_ = 0; }

  virtual void setTau(const deFloat* v) { getVarDOF1()->tau_ = *v; }
  virtual void setDDQ(const deFloat* v) { getVarDOF1()->ddq_ = *v; }
  virtual void setDQ(const deFloat* v) { getVarDOF1()->dq_ = *v; }
  virtual void setQ(const deFloat* v) { getVarDOF1()->q_ = *v; }
  virtual void getTau(deFloat* v) const { *v = getVarDOF1()->tau_; }
  virtual void getDDQ(deFloat* v) const { *v = getVarDOF1()->ddq_; }
  virtual void getDQ(deFloat* v) const { *v = getVarDOF1()->dq_; }
  virtual void getQ(deFloat* v) const { *v = getVarDOF1()->q_; }

  virtual void clampDQ();
  virtual void integrate(const deFloat dt);

  virtual deVector6& getJg() const;
  
  virtual void getJgColumns(deVector6 * Jg_columns) const;
  
private:
  taoJointType _axis;
};

class taoJointPrismatic : public taoJointDOF1
{
public:
  taoJointPrismatic(taoJointType axis);
  virtual void updateFrameLocal(deFrame* local) 
  { local->translation()[getJointType()] = getVarDOF1()->q_; }

  virtual void setABJoint(taoABJoint* joint)
  {
    taoABJointPrismatic * tmp = dynamic_cast<taoABJointPrismatic *>(joint);
    if(NULL == tmp)
    {
      std::cout <<"\ntaoJointPrismatic::setABJoint(): Incorrect joint type passed. Can't set ABJoint";
      assert(false);
    }
    if(NULL== _abJoint) { delete _abJoint; }
    _abJoint = tmp;
  }

  virtual taoABJoint* getABJoint(){ return _abJoint; }
  virtual taoABJoint const * getABJoint() const { return _abJoint; }

protected:
  taoABJointPrismatic *_abJoint;
};

class taoJointRevolute : public taoJointDOF1
{
public:
  taoJointRevolute(taoJointType axis);
  virtual void updateFrameLocal(deFrame* local)
  {
    local->rotation().set(getJointType(), getVarDOF1()->q_);
  }

  virtual void setABJoint(taoABJoint* joint)
  {
    taoABJointRevolute * tmp = dynamic_cast<taoABJointRevolute *>(joint);
    if(NULL == tmp)
    {
      std::cout <<"\ntaoJointPrismatic::setABJoint(): Incorrect joint type passed. Can't set ABJoint";
      assert(false);
    }
    if(NULL== _abJoint) { delete _abJoint; }
    _abJoint = tmp;
  }

  virtual taoABJoint* getABJoint(){ return _abJoint; }
  virtual taoABJoint const * getABJoint() const { return _abJoint; }

protected:
  taoABJointRevolute *_abJoint;
};

class taoJointUser : public taoJointDOF1
{
public:
  taoJointUser();
  //virtual void updateFrameLocal(deFrame* local) = 0;


  virtual void setABJoint(taoABJoint* joint)
  {
    if(NULL== _abJoint) { delete _abJoint; }
    _abJoint = joint;
  }

  virtual taoABJoint* getABJoint(){ return _abJoint; }
  virtual taoABJoint const * getABJoint() const { return _abJoint; }

protected:
  taoABJoint* _abJoint;
};

#endif // _taoJoint_h