Experiments - 1.2.3

Utilities Configuration GA
NNFW WorldSim Total 99% Experiments
musclepair.cpp
1 /********************************************************************************
2  * FARSA Experimentes Library *
3  * Copyright (C) 2007-2012 *
4  * Gianluca Massera <emmegian@yahoo.it> *
5  * Stefano Nolfi <stefano.nolfi@istc.cnr.it> *
6  * Tomassino Ferrauto <tomassino.ferrauto@istc.cnr.it> *
7  * Onofrio Gigliotta <onofrio.gigliotta@istc.cnr.it> *
8  * *
9  * This program is free software; you can redistribute it and/or modify *
10  * it under the terms of the GNU General Public License as published by *
11  * the Free Software Foundation; either version 2 of the License, or *
12  * (at your option) any later version. *
13  * *
14  * This program is distributed in the hope that it will be useful, *
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
17  * GNU General Public License for more details. *
18  * *
19  * You should have received a copy of the GNU General Public License *
20  * along with this program; if not, write to the Free Software *
21  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA *
22  ********************************************************************************/
23 
24 #include "musclepair.h"
25 #include "phyjoint.h"
26 
27 /*
28 dopo aver inizializzato con i valori di Gianluca
29 1)setActivation()
30 2)aplly ritorna la velocità da applicare al giunto
31 
32 */
33 
34 namespace farsa {
35 
36 MusclePair::MusclePair( PhyDOF* dof, float ksh, float tmax, float rl, float lmax ) {
37  //vedere parametri di Gianluca
38  this->dof = dof;
39  this->ksh = ksh;
40  reduceKsh = 1.0;
41  this->tmax = tmax;
42  this->rl = rl;
43  this->lmax = lmax;
44  lmin = 2.0f*rl - lmax;
45  this->ash = (rl*rl) / ( (lmax-rl)*(lmax-rl) );
46  this->b = 1.0f;
47  getExtension( last_x1, last_x2 );
48  this->dx1 = 0.0f;
49  this->dx2 = 0.0f;
50 }
51 
52 void MusclePair::setRestLength( float rl ) {
53  this->rl = rl;
54  lmin = 2.0f*rl - lmax;
55  ash = (rl*rl) / ( (lmax-rl)*(lmax-rl) );
56 }
57 
58 float MusclePair::getRestLength() {
59  return rl;
60 }
61 
62 void MusclePair::setMaxTension( float tmax ) {
63  this->tmax = tmax;
64 }
65 
66 float MusclePair::getMaxTension( ) {
67  return tmax;
68 }
69 
70 void MusclePair::setMaxLength( float lmax ) {
71  this->lmax = lmax;
72  lmin = 2.0f*rl - lmax;
73  ash = (rl*rl) / ( (lmax-rl)*(lmax-rl) );
74 }
75 
76 float MusclePair::getMaxLength() {
77  return lmax;
78 }
79 
80 float MusclePair::getMinLength() {
81  return lmin;
82 }
83 
84 float MusclePair::getAsh( ) {
85  return ash;
86 }
87 
88 void MusclePair::setAsh( float newash ) {
89  ash = newash;
90 }
91 
92 void MusclePair::setKsh( float ksh, float reduceFactor ) {
93  this->ksh = ksh;
94  reduceKsh = reduceFactor;
95 }
96 
97 float MusclePair::getKsh( ) {
98  return ksh;
99 }
100 
101 void MusclePair::setViscosity( float b ) {
102  this->b = b;
103 }
104 
105 float MusclePair::getViscosity( ) {
106  return b;
107 }
108 
109 void MusclePair::setActivation( float act1, float act2 ) {
110  //da fare prima di apply
111  //act1 e act2 devono andare da 0 a 1
112  this->act1 = act1;
113  this->act2 = act2;
114 }
115 
116 void MusclePair::getActivation( float& act1, float& act2 ) {
117  act1 = this->act1;
118  act2 = this->act2;
119 }
120 
121 void MusclePair::getExtension( float& l1, float& l2 ) {
122  float angle = dof->position();
123  real lowLim, higLim;
124  dof->limits( lowLim, higLim );
125  l1 = linearMap( angle, lowLim, higLim, lmin, lmax );
126  l2 = invLinearMap( angle, lowLim, higLim, lmin, lmax );
127 }
128 
129 float MusclePair::apply() {
130  float x1, x2;
131  getExtension( x1, x2 );
132  // Calculate the current velocity of shortening/lengthing of muscles
133  float timestep = dof->joint()->world()->timeStep();
134  dx1 = ( x1 - last_x1 ) / timestep;
135  dx2 = ( x2 - last_x2 ) / timestep;
136 
137  float ta1 = act1 * ( -ash*tmax*(x1-rl)*(x1-rl)/(rl*rl) + tmax );
138  float tp1 = reduceKsh*0.5*tmax * ( exp( ksh*(x1-rl)/(lmax-rl) ) - 1.0f ) / ( exp( ksh ) - 1.0f );
139  float tv1 = b * dx1;
140 
141  float ta2 = act2 * ( -ash*tmax*(x2-rl)*(x2-rl)/(rl*rl) + tmax );
142  float tp2 = reduceKsh*0.5*tmax * ( exp( ksh*(x2-rl)/(lmax-rl) ) - 1.0f ) / ( exp( ksh ) - 1.0f );
143  float tv2 = b * dx2;
144 
145  tal1 = ta1;
146  tal2 = ta2;
147 
148  last_x1 = x1;
149  last_x2 = x2;
150 
151  return -(ta1+tp1+tv1) + (ta2+tp2+tv2);
152 }
153 
154 } // end namespace farsa