← Back to branch summary

~hrg/hrg-packaging/roki

« back to all changes in this revision

Viewing changes to include/roki/rk_fd.h

  • Committer: Yosuke Matsusaka
  • Date: 2016-01-12 05:00:12 UTC
  • Revision ID: yosuke.matsusaka@gmail.com-20160112050012-6z0c4z5dr1u2rp64
initial import

Show diffs side-by-side

added added

removed removed

Lines of Context:
include/roki/rk_fd.h
 
1
/* RoKi - Robot Kinetics library
 
2
 * Copyright (C) 1998 Tomomichi Sugihara (Zhidao)
 
3
 *
 
4
 * rk_fd - forward dynamics computation
 
5
 * contributer: 2014-2015 Naoki Wakisaka
 
6
 */
 
7
 
 
8
#ifndef __RK_FD_H__
 
9
#define __RK_FD_H__
 
10
 
 
11
#include <roki/rk_chain.h>
 
12
#include <roki/rk_abi.h>
 
13
#include <roki/rk_cd.h>
 
14
 
 
15
__BEGIN_DECLS
 
16
 
 
17
/* ********************************************************** */
 
18
/* CLASS: rkFD
 
19
   forward dynamics class
 
20
 * ********************************************************** */
 
21
typedef struct{
 
22
  rkChain chain;   /* chain */
 
23
  int _offset;     /* offset */
 
24
  zVecStruct _dis; /* displacement */
 
25
  zVecStruct _vel; /* velocity */
 
26
  zVecStruct _acc; /* acceleration */
 
27
} rkFDCellDat;
 
28
 
 
29
zListClass( rkFDCellList, rkFDCell, rkFDCellDat );
 
30
 
 
31
typedef struct _rkFD{
 
32
  rkFDCellList list;    /* chain list */
 
33
  rkContactInfoPool ci; /* contact information */
 
34
  rkCD cd;              /* contact maneger */
 
35
  zVec dis, vel;        /* total joint state */
 
36
  zVec acc;
 
37
  int size;             /* total joint size */
 
38
  double t, dt;
 
39
 
 
40
  /* working space */
 
41
  zODE2 _ode;    /* ode solver */
 
42
  int _ode_step;
 
43
 
 
44
  double _comp_k;
 
45
  double _comp_l;
 
46
 
 
47
  /* solver */
 
48
  struct _rkFD *(*_solve)(struct _rkFD*);
 
49
  void (*_updateinit)(struct _rkFD*);
 
50
  struct _rkFD *(*_update)(struct _rkFD*);
 
51
  void (*_updatedestroy)(struct _rkFD*);
 
52
} rkFD;
 
53
 
 
54
#define rkFDTime(f) (f)->t
 
55
#define rkFDDT(f)   (f)->dt
 
56
 
 
57
__EXPORT rkFD *rkFDCreate(rkFD *fd);
 
58
__EXPORT void rkFDDestroy(rkFD *fd);
 
59
 
 
60
__EXPORT rkFDCell *rkFDChainReg(rkFD *fd, rkChain *chain);
 
61
__EXPORT rkFDCell *rkFDChainRegFile(rkFD *fd, char filename[]);
 
62
__EXPORT bool rkFDChainUnreg(rkFD *fd, rkFDCell *cell);
 
63
 
 
64
__EXPORT void rkFDChainFK(rkFD *fd, zVec dis);
 
65
__EXPORT void rkFDChainUpdateRate(rkFD *fd, zVec vel, zVec acc);
 
66
__EXPORT void rkFDChainUpdateFKRate(rkFD *fd);
 
67
 
 
68
__EXPORT void rkFDChainSetDis(rkFDCell *lc, zVec dis);
 
69
__EXPORT bool rkFDContactInfoReadFile(rkFD *fd, char filename[]);
 
70
 
 
71
/* for a fake-crawler */
 
72
 
 
73
__EXPORT void rkFDCDCellSetSlideMode(rkCDCell *cell, bool mode);
 
74
__EXPORT void rkFDCDCellSetSlideVel(rkCDCell *cell, double vel);
 
75
__EXPORT void rkFDCDCellSetSlideAxis(rkCDCell *cell, zVec3D *axis);
 
76
__EXPORT rkCDCell *rkFDShape3DGetCDCell(rkFD *fd, zShape3D *shape);
 
77
__EXPORT rkCDCell *rkFDShape3DSetSlideMode(rkFD *fd, zShape3D *shape, bool mode);
 
78
__EXPORT rkCDCell *rkFDShape3DSetSlideVel(rkFD *fd, zShape3D *shape, double vel);
 
79
__EXPORT rkCDCell *rkFDShape3DSetSlideAxis(rkFD *fd, zShape3D *shape, zVec3D *axis);
 
80
 
 
81
/* ODE updater */
 
82
 
 
83
__EXPORT zVec rkFDODECatDefault(zVec x, double k, zVec v, zVec xnew, void *util);
 
84
__EXPORT zVec rkFDODESubDefault(zVec x1, zVec x2, zVec dx, void *util);
 
85
#define rkFDODE2Assign(f,t)        zODE2Assign( &(f)->_ode, t, rkFDODECatDefault, NULL, rkFDODESubDefault, NULL )
 
86
#define rkFDODE2AssignRegular(f,t) zODE2AssignRegular( &(f)->_ode, t )
 
87
 
 
88
#define rkFDSetDT(f,t)  ( (f)->dt = (t) )
 
89
#define rkFDSetJointFricPrp(f,k,l) do{\
 
90
  (f)->_comp_k = (k);\
 
91
  (f)->_comp_l = (l);\
 
92
} while(0)
 
93
 
 
94
#define rkFDSetSolver(f,type) do{\
 
95
  (f)->_solve = rkFDSolve_##type;\
 
96
  (f)->_updateinit = rkFDUpdateInit_##type;\
 
97
  (f)->_update = rkFDUpdate_##type;\
 
98
  (f)->_updatedestroy = rkFDUpdateDestroy_##type;\
 
99
} while(0)
 
100
 
 
101
#define rkFDSolve(f)         (f)->_solve(f)
 
102
#define rkFDUpdateInit(f)    (f)->_updateinit(f)
 
103
#define rkFDUpdate(f)        (f)->_update(f)
 
104
#define rkFDUpdateDestroy(f) (f)->_updatedestroy(f)
 
105
 
 
106
/* solver */
 
107
__EXPORT rkFD *rkFDSolve_Vert(rkFD *fd);
 
108
__EXPORT void rkFDUpdateInit_Vert(rkFD *fd);
 
109
__EXPORT rkFD *rkFDUpdate_Vert(rkFD *fd);
 
110
__EXPORT void rkFDUpdateDestroy_Vert(rkFD *fd);
 
111
 
 
112
__EXPORT rkFD *rkFDSolve_Volume(rkFD *fd);
 
113
__EXPORT void rkFDUpdateInit_Volume(rkFD *fd);
 
114
__EXPORT rkFD *rkFDUpdate_Volume(rkFD *fd);
 
115
__EXPORT void rkFDUpdateDestroy_Volume(rkFD *fd);
 
116
 
 
117
__EXPORT void rkFDWrite(rkFD *fd);
 
118
 
 
119
__END_DECLS
 
120
 
 
121
#endif /* __RK_FD_H__ */