kernel/DH_Chain.h

Go to the documentation of this file.

#ifndef DH_CHAIN_H
#define DH_CHAIN_H

#include <iostream>
#include <string>
#include <vector>
#include <stdlib.h>
#include <stdio.h>

#include "DH_Link.h"

template <class DataType, int dim>
class SceneBase;

template <class DataType, int dim>
class Scene;

template <class DataType, int dim>
class DH_Chain {
public:
  friend class Joint<DataType,dim>;
  friend class DH_Link<DataType,dim>;
  friend class SceneBase<DataType,dim>;
  friend class Scene<DataType,dim>;
  typedef Joint<DataType,dim> JointType;
  typedef DH_Link<DataType,dim> LinkType;
  typedef Scene<DataType,dim> SceneType;

  // Constructors
  DH_Chain() {
    id = -1;
    joints.clear();
    links.clear();
    natives.clear();
    terNlinks.clear();
    mobile = 0;
    tipDHMat.resize(dim+1,dim+1);
    tipDHMat.assign(imatrix(dim+1));
    parentChainId = -1;
  }

  DH_Chain(const DH_Chain& v) {
    id = v.id;
    joints = v.joints;
    links = v.links;
    natives = v.natives;
    terNlinks = v.terNlinks;
    mobile = v.mobile;
    tipDHMat = v.tipDHMat;
    parentChainId = v.parentChainId;
  }

  // Destructor
  ~DH_Chain() {}

  virtual void AddJoint( JointType * j) {
    j->id = joints.size();
    joints.push_back(j);
  }
  virtual void AddJoint( JointType& j) { AddJoint(&j); }

  void addLink( LinkType * l ) {
    l->id = links.size();
    links.push_back(l);
  }
  void addLink( LinkType& l ) { addLink(&l); }

  void addNativeLink( LinkType * l ) {
    l->id = natives.size();
    natives.push_back(l);
  }
  void addNativeLink( LinkType& l ) { addNativeLink(&l); }
  int GetNumNativeLinks() const {
    return ((int) natives.size());
  }
  LinkType * GetNthNative(int i) {
    return natives[i];
  }

  void addTernLink(int link, int nativeLink) {
    boost::array<int,2> tmp;
    tmp[0] = link;
    tmp[1] = nativeLink;
    terNlinks.push_back(tmp);
  }
  int GetNumTernLinks() const {
    return ((int) terNlinks.size());
  }

  int GetNumLinks() const {
    return ((int) links.size());
  }

  LinkType * GetNthLink(int i) {
    return links[i];
  }

  void calcDHMat( int baseLink, int targetLink) {
    LinkType * tlink;
    MType mtmp(4,4);
    tlink = links[targetLink];
    std::vector<int> idTag = tlink->child->CFIdTag();

    for (register int i = baseLink; i <= targetLink; i ++) {
      mtmp = prod( mtmp, links[i]->DHMat );
    }
    if (idTag[idTag.size()-1] < 0) {
      SceneType::GetPart(idTag)->setDHMat(mtmp);
    }
    else {
      SceneType::GetAssembly(idTag)->setDHMat(mtmp);
    }
  }

  void calcNativeDHMat(SceneType& TheScene, LinkType * native, int baseLink, int targetLink) {
    MType mtmp(4,4);
    std::vector<int> idTag = native->child->CFIdTag();

    for (register int i = baseLink; i <= targetLink; i ++) {
      mtmp = prod( mtmp, links[i]->DHMat );
    }

    if (idTag[idTag.size()-1] < 0) {
      TheScene.GetPart(idTag)->setDHMat( prod(mtmp, native->DHMat) );
    }
    else {
      TheScene.GetAssembly(idTag)->setDHMat( prod(mtmp, native->DHMat) );
    }
  }

  void updateLinks() {
    for (register int i = 0; i < links.size(); i ++) {
      links[i]->update();
    }
    for (register int i = 0; i < natives.size(); i ++) {
      natives[i]->update();
    }
  }

  void setParentChain(int val) { parentChainId = val; }
  int getParentChain() { return parentChainId; }

  void print(Scene<DataType,dim>& thisScene, int level) {
    for (register int i = 0; i < GetNumLinks(); i++) {
      links[i]->print(thisScene, level+1);
    }
    for (register int i = 0; i < GetNumNativeLinks(); i++) {
      ( mklog() << std::string(2, '\t') << "Native\n" ).flush();
      natives[i]->print(thisScene, level+2);
    }
    level++;
    for (register int i = 0; i < GetNumTernLinks(); i++) {
      ( mklog() << std::string(level, '\t') << "terNlinks["<<i<<"]" << terNlinks[i][0]
                << " " << terNlinks[i][1] << std::endl ).flush();
    }
  }

  virtual void Restart(std::ifstream& ifs, int& pos, double& t, double& dt) {
#ifdef CHECKRESTART_DEBUG_ON
    ( mklog() << "\nRestarting chain " << id << std::endl ).flush();
#endif
    ifs.seekg(pos);
    ifs.read((char*)&num_joints,sizeof(int));
    ifs.read((char*)&num_links,sizeof(int));
    ifs.read((char*)&num_natives,sizeof(int));
    ifs.read((char*)&num_terNlinks,sizeof(int));
#ifdef CHECKRESTART_DEBUG_ON
    ( mklog() << " num_joints " << num_joints << " num_links " << num_links
              << " num_natives " << num_natives << " num_terNlinks " << num_terNlinks
              << std::endl ).flush();
#endif
    ifs.read((char*)&tipDHMat(0,0),sizeof(DataType));
    ifs.read((char*)&tipDHMat(0,1),sizeof(DataType));
    ifs.read((char*)&tipDHMat(0,2),sizeof(DataType));
    ifs.read((char*)&tipDHMat(0,3),sizeof(DataType));
    ifs.read((char*)&tipDHMat(1,0),sizeof(DataType));
    ifs.read((char*)&tipDHMat(1,1),sizeof(DataType));
    ifs.read((char*)&tipDHMat(1,2),sizeof(DataType));
    ifs.read((char*)&tipDHMat(1,3),sizeof(DataType));
    ifs.read((char*)&tipDHMat(2,0),sizeof(DataType));
    ifs.read((char*)&tipDHMat(2,1),sizeof(DataType));
    ifs.read((char*)&tipDHMat(2,2),sizeof(DataType));
    ifs.read((char*)&tipDHMat(2,3),sizeof(DataType));
    ifs.read((char*)&tipDHMat(3,0),sizeof(DataType));
    ifs.read((char*)&tipDHMat(3,1),sizeof(DataType));
    ifs.read((char*)&tipDHMat(3,2),sizeof(DataType));
    ifs.read((char*)&tipDHMat(3,3),sizeof(DataType));
    pos = ifs.tellg();

    for (register int i=0; i<num_joints; i++) {
      joints[i]->Restart(ifs,pos,t,dt);
      pos = ifs.tellg();
    }
    pos = ifs.tellg();
    for (register int i=0; i<num_links; i++) {
      links[i]->Restart(ifs,pos,t,dt);
      pos = ifs.tellg();
    }
    pos = ifs.tellg();
    for (register int i=0; i<num_natives; i++) {
      natives[i]->Restart(ifs,pos,t,dt);
      pos = ifs.tellg();
    }
    pos = ifs.tellg();
    for (register int i=0; i<num_terNlinks; i++) {
      ifs.read((char*)&terNlinks[i][0],sizeof(int));
      ifs.read((char*)&terNlinks[i][1],sizeof(int));
    }
    pos = ifs.tellg();
  }

  virtual void Checkpointing(std::ofstream& ofs) {
#ifdef CHECKRESTART_DEBUG_ON
    ( mklog() << "\nCHECKPOINTING chain " << id << std::endl ).flush();
#endif
    num_joints = joints.size();
    num_links = links.size();
    num_natives = natives.size();
    num_terNlinks = terNlinks.size();
    ofs.write((char*)&num_joints,sizeof(int));
    ofs.write((char*)&num_links,sizeof(int));
    ofs.write((char*)&num_natives,sizeof(int));
    ofs.write((char*)&num_terNlinks,sizeof(int));
#ifdef CHECKRESTART_DEBUG_ON
    ( mklog() << " num_joints " << num_joints << " num_links " << num_links
              << " num_natives " << num_natives << " num_terNlinks " << num_terNlinks
              << std::endl ).flush();
#endif
    ofs.write((char*)&tipDHMat(0,0),sizeof(DataType));
    ofs.write((char*)&tipDHMat(0,1),sizeof(DataType));
    ofs.write((char*)&tipDHMat(0,2),sizeof(DataType));
    ofs.write((char*)&tipDHMat(0,3),sizeof(DataType));
    ofs.write((char*)&tipDHMat(1,0),sizeof(DataType));
    ofs.write((char*)&tipDHMat(1,1),sizeof(DataType));
    ofs.write((char*)&tipDHMat(1,2),sizeof(DataType));
    ofs.write((char*)&tipDHMat(1,3),sizeof(DataType));
    ofs.write((char*)&tipDHMat(2,0),sizeof(DataType));
    ofs.write((char*)&tipDHMat(2,1),sizeof(DataType));
    ofs.write((char*)&tipDHMat(2,2),sizeof(DataType));
    ofs.write((char*)&tipDHMat(2,3),sizeof(DataType));
    ofs.write((char*)&tipDHMat(3,0),sizeof(DataType));
    ofs.write((char*)&tipDHMat(3,1),sizeof(DataType));
    ofs.write((char*)&tipDHMat(3,2),sizeof(DataType));
    ofs.write((char*)&tipDHMat(3,3),sizeof(DataType));

    for (register int i=0; i<num_joints; i++) {
      joints[i]->Checkpointing(ofs);
    }

    for (register int i=0; i<num_links; i++) {
      links[i]->Checkpointing(ofs);
    }

    for (register int i=0; i<num_natives; i++) {
      natives[i]->Checkpointing(ofs);
    }

    for (register int i=0; i<num_terNlinks; i++) {
      ofs.write((char*)&terNlinks[i][0],sizeof(int));
      ofs.write((char*)&terNlinks[i][1],sizeof(int));
    }
  }

  void setId(int val) { id = val; }

protected:
  int id; 
  std::vector<JointType *> joints;
  std::vector<LinkType *> links, natives;
  std::vector<boost::array<int, 2> > terNlinks;   
  bool mobile;
  int parentChainId;
  MType tipDHMat;
  int num_terNlinks, num_joints, num_links, num_natives;
};

#endif // DH_CHAIN_H

---