sdcontact.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
3  * All rights reserved. This program is made available under the terms of the
4  * Eclipse Public License v1.0 which accompanies this distribution, and is
5  * available at http://www.eclipse.org/legal/epl-v10.html
6  * Contributors:
7  * The University of Tokyo
8  */
14 #ifndef __SDCONTACT_H__
15 #define __SDCONTACT_H__
16 
17 //#include <colinfo.h>
18 #include "chain.h"
19 #include <vector>
20 
21 #define SD_NONLINEAR
22 
24 {
25 public:
26  SDContactPair(Joint* _jnt1, Joint* _jnt2,
27  double _spring = 1e5, double _damper = 10.0,
28  double _static_fric = 0.0, double _slip_fric = 0.0,
29  double _slip_p = 2000.0, double _slip_d = 700.0,
30  double _slip_func_coef_base = 0.1) {
31  joints[0] = _jnt1;
32  joints[1] = _jnt2;
33  spring = _spring;
34  damper = _damper;
35  static_fric = _static_fric;
36  slip_fric = _slip_fric;
37  slip_p = _slip_p;
38  slip_d = _slip_d;
39  slip_func_coef_base = _slip_func_coef_base;
40  in_slip = false;
41  }
43  }
44 
45  void SetSpring(double _spring) {
46  spring = _spring;
47  }
48  void SetDamper(double _damper) {
49  damper = _damper;
50  }
51  void SetStaticFric(double _static_fric) {
52  static_fric = _static_fric;
53  }
54  void SetSlipFric(double _slip_fric) {
55  slip_fric = _slip_fric;
56  }
57  double StaticFric() {
58  return static_fric;
59  }
60  double SlipFric() {
61  return slip_fric;
62  }
63 
64  Joint* GetJoint(int index) {
65  return joints[index];
66  }
67 
68  void Clear() {
69  coords.clear();
70  normals.clear();
71  depths.clear();
72  }
73 
74  void AddPoint(double* coord, double* normal, double depth) {
75  coords.push_back(fVec3(coord[0], coord[1], coord[2]));
76  normals.push_back(fVec3(normal[0], normal[1], normal[2]));
77  depths.push_back(depth);
78  }
79 
80  int NumPoints() {
81  return coords.size();
82  }
83  const fVec3& Coord(int index) {
84  return coords[index];
85  }
86  const fVec3& Normal(int index) {
87  return normals[index];
88  }
89  double Depth(int index) {
90  return depths[index];
91  }
92 
93  // compute external forces of links in contact
94  int Update(double timestep, int n_contact, double** coords, double** normals, double* depths);
95 
96 protected:
97  int set_init();
98  int update(double timestep, int n_contact, double* coord, double* normal, double depth);
99 
100  // pair
102 
103  std::vector<fVec3> coords;
104  std::vector<fVec3> normals;
105  std::vector<double> depths;
106 
107  // spring-damper model parameters
108  double spring;
109  double damper;
110  // friction parameters
111  double static_fric;
112  double slip_fric;
113  double slip_p;
114  double slip_d;
116  // initial position/orientation of joint2 in joint1 frame
117  int init_set;
120  // slipping?
121  int in_slip;
122 };
123 
124 #endif
int update(double timestep, int n_contact, double *coord, double *normal, double depth)
Definition: sdcontact.cpp:62
3x3 matrix class.
Definition: fMatrix3.h:29
double slip_fric
Definition: sdcontact.h:112
SDContactPair(Joint *_jnt1, Joint *_jnt2, double _spring=1e5, double _damper=10.0, double _static_fric=0.0, double _slip_fric=0.0, double _slip_p=2000.0, double _slip_d=700.0, double _slip_func_coef_base=0.1)
Definition: sdcontact.h:26
const fVec3 & Coord(int index)
Definition: sdcontact.h:83
double damper
Definition: sdcontact.h:109
void SetDamper(double _damper)
Definition: sdcontact.h:48
void AddPoint(double *coord, double *normal, double depth)
Definition: sdcontact.h:74
double SlipFric()
Definition: sdcontact.h:60
double spring
Definition: sdcontact.h:108
void SetSpring(double _spring)
Definition: sdcontact.h:45
const fVec3 & Normal(int index)
Definition: sdcontact.h:86
double slip_func_coef_base
Definition: sdcontact.h:115
int NumPoints()
Definition: sdcontact.h:80
Joint * joints[2]
Definition: sdcontact.h:101
Classes for defining open/closed kinematic chains.
double slip_d
Definition: sdcontact.h:114
void SetSlipFric(double _slip_fric)
Definition: sdcontact.h:54
double StaticFric()
Definition: sdcontact.h:57
void SetStaticFric(double _static_fric)
Definition: sdcontact.h:51
double static_fric
Definition: sdcontact.h:111
void Clear()
Definition: sdcontact.h:68
int set_init()
Definition: sdcontact.cpp:42
fVec3 init_pos
Definition: sdcontact.h:118
fMat33 init_att
Definition: sdcontact.h:119
std::vector< double > depths
Definition: sdcontact.h:105
3-element vector class.
Definition: fMatrix3.h:206
Joint * GetJoint(int index)
Definition: sdcontact.h:64
The class for representing a joint.
Definition: chain.h:538
int Update(double timestep, int n_contact, double **coords, double **normals, double *depths)
Definition: sdcontact.cpp:22
double Depth(int index)
Definition: sdcontact.h:89
std::vector< fVec3 > coords
Definition: sdcontact.h:103
std::vector< fVec3 > normals
Definition: sdcontact.h:104
double slip_p
Definition: sdcontact.h:113


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sat Apr 13 2019 02:14:25