Skip to content

Commit 79b2322

Browse files
author
Francois Keith
committed
Change the GripperControl entity. Close #27.
The gripper control entity now prevents closing the gripper more if the maximal torque value is reached.
1 parent 45a5610 commit 79b2322

File tree

3 files changed

+118
-141
lines changed

3 files changed

+118
-141
lines changed

‎include/sot/core/gripper-control.hh

Lines changed: 24 additions & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -59,40 +59,45 @@ namespace dynamicgraph { namespace sot {
5959

6060
namespace dg = dynamicgraph;
6161

62+
/*! \brief The goal of this entity is to ensure that the maximal torque will not
63+
* be exceeded during a grasping task.
64+
* If the maximal torque is reached, then the current position of the gripper is
65+
kept
66+
*
67+
*/
6268
class SOTGRIPPERCONTROL_EXPORT GripperControl
6369
{
6470
protected:
6571

6672
double offset;
6773
static const double OFFSET_DEFAULT;
74+
//! \brief The multiplication
6875
ml::Vector factor;
6976

7077
public:
7178
GripperControl( void );
7279

80+
//! \brief Computes the
81+
// if the torque limit is reached, the normalized position is reduced by
82+
// (offset)
7383
void computeIncrement( const ml::Vector& torques,
74-
const ml::Vector& torqueLimits,
75-
const ml::Vector& currentNormPos );
76-
77-
static void computeNormalizedPosition( const ml::Vector& currentPos,
78-
const ml::Vector& upperLim,
79-
const ml::Vector& lowerLim,
80-
ml::Vector& currentNormPos );
81-
static void computeDenormalizedPosition( const ml::Vector& currentNormPos,
82-
const ml::Vector& upperLim,
83-
const ml::Vector& lowerLim,
84-
ml::Vector& currentPos );
84+
const ml::Vector& torqueLimits,
85+
const ml::Vector& currentNormVel );
8586

87+
//! \brief
8688
ml::Vector& computeDesiredPosition( const ml::Vector& currentPos,
87-
const ml::Vector& torques,
88-
const ml::Vector& upperLim,
89-
const ml::Vector& lowerLim,
90-
const ml::Vector& torqueLimits,
91-
ml::Vector& desPos );
89+
const ml::Vector& desiredPos,
90+
const ml::Vector& torques,
91+
const ml::Vector& torqueLimits,
92+
ml::Vector& referencePos );
93+
94+
/*! \brief select only some of the values of the vector fullsize,
95+
* based on the Flags vector.
96+
*/
9297

9398
static ml::Vector& selector( const ml::Vector& fullsize,
94-
const Flags& selec,
95-
ml::Vector& desPos );
99+
const Flags& selec,
100+
ml::Vector& desPos );
96101
};
97102

98103
/* --------------------------------------------------------------------- */
@@ -119,19 +124,14 @@ class SOTGRIPPERCONTROL_EXPORT GripperControlPlugin
119124

120125
/* --- INPUTS --- */
121126
dg::SignalPtr<ml::Vector,int> positionSIN;
122-
dg::SignalPtr<ml::Vector,int> upperLimitSIN;
123-
dg::SignalPtr<ml::Vector,int> lowerLimitSIN;
127+
dg::SignalPtr<ml::Vector,int> positionDesSIN;
124128
dg::SignalPtr<ml::Vector,int> torqueSIN;
125129
dg::SignalPtr<ml::Vector,int> torqueLimitSIN;
126130
dg::SignalPtr<Flags,int> selectionSIN;
127131

128132
/* --- INTERMEDIARY --- */
129133
dg::SignalPtr<ml::Vector,int> positionFullSizeSIN;
130134
dg::SignalTimeDependent<ml::Vector,int> positionReduceSOUT;
131-
dg::SignalPtr<ml::Vector,int> upperLimitFullSizeSIN;
132-
dg::SignalTimeDependent<ml::Vector,int> upperLimitReduceSOUT;
133-
dg::SignalPtr<ml::Vector,int> lowerLimitFullSizeSIN;
134-
dg::SignalTimeDependent<ml::Vector,int> lowerLimitReduceSOUT;
135135
dg::SignalPtr<ml::Vector,int> torqueFullSizeSIN;
136136
dg::SignalTimeDependent<ml::Vector,int> torqueReduceSOUT;
137137
dg::SignalPtr<ml::Vector,int> torqueLimitFullSizeSIN;

‎include/sot/core/macros-signal.hh

Lines changed: 18 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -24,7 +24,7 @@
2424

2525
#define SOT_CALL_SIG(sotName,sotType) \
2626
boost::bind(&Signal<sotType,int>::access, \
27-
&sotName,_2)
27+
&sotName,_2)
2828

2929
/* --- STATIC MACROS -------------------------------------------------------- */
3030
/* --- STATIC MACROS -------------------------------------------------------- */
@@ -33,7 +33,7 @@
3333
#define SOT_INIT_SIGNAL_1(sotFunction, \
3434
sotArg1,sotArg1Type) \
3535
boost::bind(&sotFunction, \
36-
SOT_CALL_SIG(sotArg1,sotArg1Type),_1), \
36+
SOT_CALL_SIG(sotArg1,sotArg1Type),_1), \
3737
sotArg1
3838

3939
#define SOT_INIT_SIGNAL_2(sotFunction, \
@@ -121,7 +121,7 @@ boost::bind(&sotFunction, \
121121
#define SOT_MEMBER_SIGNAL_1(sotFunction, \
122122
sotArg1,sotArg1Type) \
123123
boost::bind(&sotFunction,this, \
124-
SOT_CALL_SIG(sotArg1,sotArg1Type),_1), \
124+
SOT_CALL_SIG(sotArg1,sotArg1Type),_1), \
125125
sotArg1
126126

127127
#define SOT_MEMBER_SIGNAL_2(sotFunction, \
@@ -131,6 +131,21 @@ boost::bind(&sotFunction,this, \
131131
SOT_CALL_SIG(sotArg1,sotArg1Type), \
132132
SOT_CALL_SIG(sotArg2,sotArg2Type),_1), \
133133
sotArg1<<sotArg2
134+
135+
#define SOT_MEMBER_SIGNAL_4(sotFunction, \
136+
sotArg1,sotArg1Type, \
137+
sotArg2,sotArg2Type, \
138+
sotArg3,sotArg3Type, \
139+
sotArg4,sotArg4Type) \
140+
boost::bind(&sotFunction,this, \
141+
SOT_CALL_SIG(sotArg1,sotArg1Type), \
142+
SOT_CALL_SIG(sotArg2,sotArg2Type), \
143+
SOT_CALL_SIG(sotArg3,sotArg3Type), \
144+
SOT_CALL_SIG(sotArg4,sotArg4Type),_1), \
145+
sotArg1<<sotArg2<<sotArg3<<sotArg4
146+
147+
148+
134149
#define SOT_MEMBER_SIGNAL_5(sotFunction, \
135150
sotArg1,sotArg1Type, \
136151
sotArg2,sotArg2Type, \

‎src/tools/gripper-control.cpp

Lines changed: 76 additions & 114 deletions
Original file line numberDiff line numberDiff line change
@@ -37,19 +37,22 @@ DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(GripperControlPlugin,"GripperControl");
3737
/* --- PLUGIN --------------------------------------------------------------- */
3838

3939

40-
#define SOT_FULL_TO_REDUCED( sotName ) \
41-
sotName##FullSizeSIN(NULL,"GripperControl("+name+")::input(vector)::" \
42-
+#sotName+"FullIN") \
43-
,sotName##ReduceSOUT( SOT_INIT_SIGNAL_2( GripperControlPlugin::selector, \
44-
sotName##FullSizeSIN,ml::Vector, \
45-
selectionSIN,Flags ), \
46-
"GripperControl("+name+")::input(vector)::" \
47-
+#sotName+"ReducedOUT")
40+
#define SOT_FULL_TO_REDUCED( sotName ) \
41+
sotName##FullSizeSIN(NULL,"GripperControl("+name+")::input(vector)::" \
42+
+#sotName+"FullIN") \
43+
,sotName##ReduceSOUT( SOT_INIT_SIGNAL_2( GripperControlPlugin::selector, \
44+
sotName##FullSizeSIN,ml::Vector, \
45+
selectionSIN,Flags ), \
46+
"GripperControl("+name+")::input(vector)::" \
47+
+#sotName+"ReducedOUT")
4848

4949

5050

5151
const double GripperControl::OFFSET_DEFAULT = 0.9;
5252

53+
// TODO: hard coded
54+
const double DT = 0.005;
55+
5356
GripperControl::
5457
GripperControl(void)
5558
:offset( GripperControl::OFFSET_DEFAULT )
@@ -60,45 +63,34 @@ GripperControlPlugin::
6063
GripperControlPlugin( const std::string & name )
6164
:Entity(name)
6265
,calibrationStarted(false)
63-
6466
,positionSIN(NULL,"GripperControl("+name+")::input(vector)::position")
65-
,upperLimitSIN(NULL,"GripperControl("+name+")::input(vector)::upperLimit")
66-
,lowerLimitSIN(NULL,"GripperControl("+name+")::input(vector)::lowerLimit")
67+
,positionDesSIN(NULL,"GripperControl("+name+")::input(vector)::positionDes")
6768
,torqueSIN(NULL,"GripperControl("+name+")::input(vector)::torque")
6869
,torqueLimitSIN(NULL,"GripperControl("+name+")::input(vector)::torqueLimit")
6970
,selectionSIN(NULL,"GripperControl("+name+")::input(vector)::selec")
70-
71-
,SOT_FULL_TO_REDUCED( position )
72-
,SOT_FULL_TO_REDUCED( upperLimit )
73-
,SOT_FULL_TO_REDUCED( lowerLimit )
74-
,SOT_FULL_TO_REDUCED( torque )
75-
,SOT_FULL_TO_REDUCED( torqueLimit )
76-
77-
,desiredPositionSOUT( SOT_MEMBER_SIGNAL_5( GripperControl::computeDesiredPosition,
78-
positionSIN,ml::Vector,
79-
torqueSIN,ml::Vector,
80-
upperLimitSIN,ml::Vector,
81-
lowerLimitSIN,ml::Vector,
82-
torqueLimitSIN,ml::Vector ),
83-
"GripperControl("+name+")::output(vector)::reference" )
84-
71+
72+
,SOT_FULL_TO_REDUCED( position )
73+
,SOT_FULL_TO_REDUCED( torque )
74+
,SOT_FULL_TO_REDUCED( torqueLimit )
75+
,desiredPositionSOUT( SOT_MEMBER_SIGNAL_4( GripperControl::computeDesiredPosition,
76+
positionSIN,ml::Vector,
77+
positionDesSIN,ml::Vector,
78+
torqueSIN,ml::Vector,
79+
torqueLimitSIN,ml::Vector ),
80+
"GripperControl("+name+")::output(vector)::reference" )
8581
{
8682
sotDEBUGIN(5);
87-
83+
8884
positionSIN.plug( &positionReduceSOUT );
89-
upperLimitSIN.plug( &upperLimitReduceSOUT );
90-
lowerLimitSIN.plug( &lowerLimitReduceSOUT );
9185
torqueSIN.plug( &torqueReduceSOUT );
9286
torqueLimitSIN.plug( &torqueLimitReduceSOUT );
9387

94-
signalRegistration( positionSIN << upperLimitSIN << lowerLimitSIN
88+
signalRegistration( positionSIN << positionDesSIN
9589
<< torqueSIN << torqueLimitSIN << selectionSIN
9690
<< desiredPositionSOUT
97-
<< positionFullSizeSIN << positionReduceSOUT
98-
<< upperLimitFullSizeSIN << upperLimitReduceSOUT
99-
<< lowerLimitFullSizeSIN << lowerLimitReduceSOUT
100-
<< torqueFullSizeSIN << torqueReduceSOUT
101-
<< torqueLimitFullSizeSIN << torqueLimitReduceSOUT );
91+
<< positionFullSizeSIN
92+
<< torqueFullSizeSIN
93+
<< torqueLimitFullSizeSIN);
10294
sotDEBUGOUT(5);
10395

10496
initCommands();
@@ -107,17 +99,12 @@ GripperControlPlugin( const std::string & name )
10799

108100
GripperControlPlugin::
109101
~GripperControlPlugin( void )
110-
{
111-
return;
112-
}
102+
{}
113103

114104
std::string GripperControlPlugin::
115105
getDocString () const
116106
{
117-
std::string docstring =
118-
"\n"
119-
"\n Control of HRP2 gripper."
120-
"\n";
107+
std::string docstring ="Control of gripper.";
121108
return docstring;
122109
}
123110

@@ -128,89 +115,64 @@ getDocString () const
128115
void GripperControl::
129116
computeIncrement( const ml::Vector& torques,
130117
const ml::Vector& torqueLimits,
131-
const ml::Vector& currentNormPos )
118+
const ml::Vector& currentNormVel )
132119
{
133-
const unsigned int SIZE = torques.size();
134-
if( (SIZE==torqueLimits.size())||(SIZE==currentNormPos.size()) )
135-
{ /* ERROR ... */ }
136-
137-
if( factor.size()!=SIZE ) { factor.resize(SIZE); factor.fill(1.); }
138-
for( unsigned int i=0;i<SIZE;++i )
139-
{
140-
if( (torques(i)>torqueLimits(i))&&(currentNormPos(i)>0) )
141-
{ factor(i)*=offset; }
142-
else if( (torques(i)<-torqueLimits(i))&&(currentNormPos(i)<0) )
143-
{ factor(i)*=offset; }
144-
else { factor(i)/=offset; }
145-
if( factor(i)>1 ) factor(i)=1;
146-
if( factor(i)<0 ) factor(i)=0;
147-
}
148-
}
120+
const unsigned int SIZE = currentNormVel.size();
149121

122+
// initialize factor, if needed.
123+
if( factor.size()!=SIZE ) { factor.resize(SIZE); factor.fill(1.); }
150124

151-
void GripperControl::
152-
computeNormalizedPosition( const ml::Vector& currentPos,
153-
const ml::Vector& upperLim,
154-
const ml::Vector& lowerLim,
155-
ml::Vector& currentNormPos )
156-
{
157-
sotDEBUG(25) << "UJL = " << upperLim;
158-
sotDEBUG(25) << "LJL = " << lowerLim;
159-
160-
const unsigned int SIZE = currentPos.size();
161-
if( (SIZE==upperLim.size())||(SIZE==lowerLim.size()) )
162-
{ /* ERROR ... */ }
163-
currentNormPos.resize(SIZE);
164-
for( unsigned int i=0;i<SIZE;++i )
165-
{
166-
currentNormPos(i) = ( -1+2*( currentPos(i)-lowerLim(i) )
167-
/( upperLim(i)-lowerLim(i) ));
168-
}
169-
}
125+
// Torque not provided?
126+
if (torques.size() == 0)
127+
{
128+
std::cerr << "torque is not provided " << std::endl;
129+
return;
130+
}
170131

171-
void GripperControl::
172-
computeDenormalizedPosition( const ml::Vector& currentNormPos,
173-
const ml::Vector& upperLim,
174-
const ml::Vector& lowerLim,
175-
ml::Vector& currentPos )
176-
{
177-
const unsigned int SIZE = currentNormPos.size();
178-
if( (SIZE==upperLim.size())||(SIZE==lowerLim.size()) )
179-
{ /* ERROR ... */ }
180-
currentPos.resize(SIZE);
181-
for( unsigned int i=0;i<SIZE;++i )
182-
{
183-
currentPos(i) = ( (currentNormPos(i)+1)*( upperLim(i)-lowerLim(i) )/2
184-
+lowerLim(i) );
185-
}
132+
for( unsigned int i=0;i<SIZE;++i )
133+
{
134+
// apply a reduction factor if the torque limits are exceeded
135+
// and the velocity goes in the same way
136+
if( (torques(i)>torqueLimits(i))&&(currentNormVel(i)>0) )
137+
{ factor(i)*=offset; }
138+
else if( (torques(i)< -torqueLimits(i))&&(currentNormVel(i)<0) )
139+
{ factor(i)*=offset; }
140+
// otherwise, release smoothly the reduction if possible/needed
141+
else { factor(i)/=offset; }
142+
143+
// ensure factor is in )0,1(
144+
factor(i) = std::min(1., std::max(factor(i),0.));
145+
}
186146
}
187147

188148
ml::Vector& GripperControl::
189149
computeDesiredPosition( const ml::Vector& currentPos,
150+
const ml::Vector& desiredPos,
190151
const ml::Vector& torques,
191-
const ml::Vector& upperLim,
192-
const ml::Vector& lowerLim,
193152
const ml::Vector& torqueLimits,
194-
ml::Vector& desPos )
153+
ml::Vector& referencePos )
195154
{
196155
const unsigned int SIZE = currentPos.size();
197-
if( (SIZE==torques.size()) )
198-
{ /* ERROR ... */ }
199-
desPos.resize(SIZE);
200-
201-
ml::Vector normPos;
202-
computeNormalizedPosition( currentPos,upperLim,lowerLim,normPos );
203-
sotDEBUG(25) << "Norm pos = " << normPos;
204-
205-
computeIncrement( torques,torqueLimits,normPos );
206-
sotDEBUG(25) << "Factor = " << factor;
207-
208-
ml::Vector desNormPos(SIZE);
209-
normPos.multiply(factor,desNormPos);
210-
211-
computeDenormalizedPosition( desNormPos,upperLim,lowerLim,desPos );
212-
213-
return desPos;
156+
// if( (SIZE==torques.size()) )
157+
// { /* ERROR ... */ }
158+
159+
// compute the desired velocity
160+
ml::Vector velocity = (desiredPos - currentPos)* (1. / DT);
161+
162+
computeIncrement(torques, torqueLimits, velocity);
163+
164+
sotDEBUG(25) << " velocity " << velocity << std::endl;
165+
sotDEBUG(25) << " factor " << factor << std::endl;
166+
167+
// multiply the velocity elmt per elmt
168+
ml::Vector weightedVel(SIZE);
169+
velocity.multiply(factor, weightedVel);
170+
sotDEBUG(25) << " weightedVel " << weightedVel << std::endl;
171+
172+
// integrate the desired velocity
173+
referencePos.resize(SIZE);
174+
referencePos = currentPos + weightedVel * DT;
175+
return referencePos;
214176
}
215177

216178

@@ -241,7 +203,7 @@ void GripperControlPlugin::initCommands()
241203
namespace dc = ::dynamicgraph::command;
242204
addCommand("offset",
243205
dc::makeCommandVoid1(*this,&GripperControlPlugin::setOffset,
244-
"set the offset (should be in )0, 1(."));
206+
"set the offset (should be in )0, 1( )."));
245207
}
246208

247209

0 commit comments

Comments
 (0)