sdcontact.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * The University of Tokyo
00008  */
00014 #ifndef __SDCONTACT_H__
00015 #define __SDCONTACT_H__
00016 
00017 //#include <colinfo.h>
00018 #include "chain.h"
00019 #include <vector>
00020 
00021 #define SD_NONLINEAR
00022 
00023 class SDContactPair
00024 {
00025 public:
00026         SDContactPair(Joint* _jnt1, Joint* _jnt2,
00027                                   double _spring = 1e5, double _damper = 10.0,
00028                                   double _static_fric = 0.0, double _slip_fric = 0.0,
00029                                   double _slip_p = 2000.0, double _slip_d = 700.0,
00030                                   double _slip_func_coef_base = 0.1) {
00031                 joints[0] = _jnt1;
00032                 joints[1] = _jnt2;
00033                 spring = _spring;
00034                 damper = _damper;
00035                 static_fric = _static_fric;
00036                 slip_fric = _slip_fric;
00037                 slip_p = _slip_p;
00038                 slip_d = _slip_d;
00039                 slip_func_coef_base = _slip_func_coef_base;
00040                 in_slip = false;
00041         }
00042         ~SDContactPair() {
00043         }
00044 
00045         void SetSpring(double _spring) {
00046                 spring = _spring;
00047         }
00048         void SetDamper(double _damper) {
00049                 damper = _damper;
00050         }
00051         void SetStaticFric(double _static_fric) {
00052                 static_fric = _static_fric;
00053         }
00054         void SetSlipFric(double _slip_fric) {
00055                 slip_fric = _slip_fric;
00056         }
00057         double StaticFric() {
00058                 return static_fric;
00059         }
00060         double SlipFric() {
00061                 return slip_fric;
00062         }
00063 
00064         Joint* GetJoint(int index) {
00065                 return joints[index];
00066         }
00067 
00068         void Clear() {
00069                 coords.clear();
00070                 normals.clear();
00071                 depths.clear();
00072         }
00073 
00074         void AddPoint(double* coord, double* normal, double depth) {
00075                 coords.push_back(fVec3(coord[0], coord[1], coord[2]));
00076                 normals.push_back(fVec3(normal[0], normal[1], normal[2]));
00077                 depths.push_back(depth);
00078         }
00079 
00080         int NumPoints() {
00081                 return coords.size();
00082         }
00083         const fVec3& Coord(int index) {
00084                 return coords[index];
00085         }
00086         const fVec3& Normal(int index) {
00087                 return normals[index];
00088         }
00089         double Depth(int index) {
00090                 return depths[index];
00091         }
00092         
00093         // compute external forces of links in contact
00094         int Update(double timestep, int n_contact, double** coords, double** normals, double* depths);
00095 
00096 protected:
00097         int set_init();
00098         int update(double timestep, int n_contact, double* coord, double* normal, double depth);
00099 
00100         // pair
00101         Joint* joints[2];
00102 
00103         std::vector<fVec3> coords;
00104         std::vector<fVec3> normals;
00105         std::vector<double> depths;
00106 
00107         // spring-damper model parameters
00108         double spring;
00109         double damper;
00110         // friction parameters
00111         double static_fric;
00112         double slip_fric;
00113         double slip_p;
00114         double slip_d;
00115         double slip_func_coef_base;
00116         // initial position/orientation of joint2 in joint1 frame
00117         int init_set;
00118         fVec3 init_pos;
00119         fMat33 init_att;
00120         // slipping?
00121         int in_slip;
00122 };
00123 
00124 #endif


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Apr 11 2019 03:30:19