← Back to branch summary

~shadowrobot/sr-ros-interface/stable

« back to all changes in this revision

Viewing changes to sr_friction_compensation/src/sr_friction_compensation.py

  • Committer: Ugo Cupcic
  • Date: 2012-08-08 10:06:43 UTC
  • mfrom: (1.1.380 trunk)
  • Revision ID: ugo@shadowrobot.com-20120808100643-9ovn6lpwf5wi5wu4
new organisation: split into different stacks for maintenance to be easier, also moved the models to their own sr_description package, ...

Show diffs side-by-side

added added

removed removed

Lines of Context:
sr_friction_compensation/src/sr_friction_compensation.py
1
 
#!/usr/bin/env python
2
 
#
3
 
# Copyright 2011 Shadow Robot Company Ltd.
4
 
#
5
 
# This program is free software: you can redistribute it and/or modify it
6
 
# under the terms of the GNU General Public License as published by the Free
7
 
# Software Foundation, either version 2 of the License, or (at your option)
8
 
# any later version.
9
 
#
10
 
# This program is distributed in the hope that it will be useful, but WITHOUT
11
 
# ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
12
 
# FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
13
 
# more details.
14
 
#
15
 
# You should have received a copy of the GNU General Public License along
16
 
# with this program.  If not, see <http://www.gnu.org/licenses/>.
17
 
#
18
 
 
19
 
import roslib; roslib.load_manifest('sr_friction_compensation')
20
 
import rospy
21
 
 
22
 
from etherCAT_hand_lib import EtherCAT_Hand_Lib
23
 
#from sensor_msgs import JointState
24
 
from std_msgs.msg import Float64
25
 
from pylab import plot, savefig
26
 
 
27
 
from scipy.optimize import leastsq
28
 
#from numpy import interp
29
 
import numpy
30
 
import time
31
 
 
32
 
epsilon = 0.02
33
 
 
34
 
class SrFrictionCompensation(object):
35
 
    """
36
 
    """
37
 
 
38
 
    def __init__(self):
39
 
        """
40
 
        """
41
 
        self.lib = EtherCAT_Hand_Lib()
42
 
 
43
 
        self.joint_name = "FFJ3"
44
 
        if rospy.has_param("~joint_name"):
45
 
            self.joint_name = rospy.get_param("~joint_name")
46
 
 
47
 
        self.min = 0.0
48
 
        if rospy.has_param("~min"):
49
 
            self.min = rospy.get_param("~min")
50
 
 
51
 
        self.max = 1.55
52
 
        if rospy.has_param("~max"):
53
 
            self.max = rospy.get_param("~max")
54
 
 
55
 
        self.nb_repetition = 2
56
 
        if rospy.has_param("~nb_repetition"):
57
 
            self.nb_repetition = rospy.get_param("~nb_repetition")
58
 
 
59
 
        print " nb repetition: ", self.nb_repetition
60
 
 
61
 
        self.sign = -1
62
 
        if rospy.has_param("~sign"):
63
 
            self.sign = rospy.get_param("~sign")
64
 
        rospy.loginfo("sign: "+str(self.sign))
65
 
 
66
 
        self.publisher = rospy.Publisher("/sr_friction_compensation/"+self.joint_name, Float64)
67
 
        self.rate = rospy.Rate(100)
68
 
 
69
 
        self.forces = []
70
 
        self.positions = []
71
 
 
72
 
    def run(self):
73
 
        self.lib.activate()
74
 
        time.sleep(0.5)
75
 
 
76
 
        for i in range(0,self.nb_repetition):
77
 
            self.move_to_start()
78
 
            time.sleep(1)
79
 
            self.record_map( -self.sign )
80
 
            time.sleep(1)
81
 
 
82
 
            print "forces length: ", len(self.forces), " pos length: ", len(self.positions)
83
 
 
84
 
        self.interpolate_map(1)
85
 
 
86
 
        self.forces = []
87
 
        self.positions = []
88
 
 
89
 
        for i in range(0, self.nb_repetition):
90
 
            self.move_to_start(False)
91
 
            time.sleep(1)
92
 
            self.record_map( self.sign, False )
93
 
            time.sleep(1)
94
 
            print "forces length: ", len(self.forces), " pos length: ", len(self.positions)
95
 
 
96
 
 
97
 
        self.interpolate_map(2)
98
 
 
99
 
    def move_to_start(self, start_at_min = True):
100
 
        msg = Float64()
101
 
 
102
 
        #try to get past the minimum
103
 
        if start_at_min:
104
 
            rospy.loginfo("Moving to the starting position ("+str(self.min)+")")
105
 
        else:
106
 
            rospy.loginfo("Moving to the starting position ("+str(self.max)+")")
107
 
        if( start_at_min ):
108
 
            #250 should be big enough to move the finger to the end of its range
109
 
            msg.data = self.sign * 250
110
 
            while (self.lib.get_position(self.joint_name) > self.min) and not rospy.is_shutdown():
111
 
                self.publisher.publish(msg)
112
 
                self.rate.sleep()
113
 
        else:
114
 
            #250 should be big enough to move the finger to the end of its range
115
 
            msg.data = -self.sign * 250
116
 
 
117
 
            while (self.lib.get_position(self.joint_name) < self.max) and not rospy.is_shutdown():
118
 
                self.publisher.publish(msg)
119
 
                self.rate.sleep()
120
 
 
121
 
        msg.data = 0.0
122
 
        for i in range(0,20):
123
 
            self.publisher.publish(msg)
124
 
            self.rate.sleep()
125
 
 
126
 
        rospy.loginfo("Starting position reached")
127
 
 
128
 
    def record_map(self, sign, increasing = True):
129
 
        if increasing:
130
 
            rospy.loginfo("Recording increasing map, until "+str(self.max))
131
 
        else:
132
 
            rospy.loginfo("Recording decreasing map, until"+str(self.min))
133
 
        msg = Float64()
134
 
 
135
 
        if( increasing ):
136
 
            while (self.lib.get_position(self.joint_name) < self.max) and not rospy.is_shutdown():
137
 
                self.map_step(sign, msg)
138
 
        else:
139
 
            while (self.lib.get_position(self.joint_name) > self.min) and not rospy.is_shutdown():
140
 
                self.map_step(sign, msg, False)
141
 
 
142
 
    def map_step(self, sign, msg, increasing=True):
143
 
        #keep the tendon under tension
144
 
        min_force = 0.
145
 
        if increasing:
146
 
            min_force = 0.
147
 
        else:
148
 
            min_force = 70.
149
 
        force, pos = self.find_smallest_force(self.lib.get_position(self.joint_name), sign, min_force)
150
 
        if force != False:
151
 
            self.forces.append(force)
152
 
            self.positions.append(pos)
153
 
            msg.data = sign*min_force
154
 
            self.publisher.publish(msg)
155
 
            rospy.Rate(2).sleep()
156
 
 
157
 
    def find_smallest_force(self, first_position, sign, min_force):
158
 
        msg = Float64()
159
 
 
160
 
        for force in range(min_force, 400):
161
 
            if rospy.is_shutdown():
162
 
                break
163
 
 
164
 
            if abs(self.lib.get_position(self.joint_name) - first_position) > epsilon:
165
 
                #ok, the finger moved, return the necessary force
166
 
                measured_force = self.lib.get_effort(self.joint_name)
167
 
                print "finger moved from ",first_position, " with force ", measured_force
168
 
                return measured_force, first_position
169
 
            else:
170
 
                msg.data = sign*force
171
 
                self.publisher.publish(msg)
172
 
            self.rate.sleep()
173
 
 
174
 
        return False, False
175
 
 
176
 
    def interpolate_map(self, index):
177
 
 
178
 
        interp_pos = numpy.linspace(self.min, self.max, 10)
179
 
        fp = lambda v, x: v[0]+ v[1]*x
180
 
 
181
 
        v = self.linear_interp(-1, -1)
182
 
        print v
183
 
 
184
 
        plot(interp_pos, fp(v, interp_pos), '-')
185
 
 
186
 
        print "forces length: ", len(self.forces), " pos length: ", len(self.positions)
187
 
        plot(self.positions, self.forces, 'o')
188
 
 
189
 
        savefig("friction_compensation_"+self.joint_name+".png")
190
 
 
191
 
    def linear_interp(self, min_index, max_index):
192
 
 
193
 
        fp = lambda v, x: v[0]+ v[1]*x
194
 
        e = lambda v, x, y: (fp(v,x)-y)
195
 
 
196
 
        v0 = [3., 1, 4.]
197
 
 
198
 
        v = 0
199
 
        if min_index == -1 and max_index == -1:
200
 
            v, success = leastsq(e, v0, args=(self.positions,self.forces), maxfev=10000)
201
 
        else:
202
 
            v, success = leastsq(e, v0, args=(self.positions[min_index:max_index],self.forces[min_index:max_index]), maxfev=10000)
203
 
        return v
204
 
 
205
 
 
206
 
if __name__ == '__main__':
207
 
    rospy.init_node('sr_friction_compensation', anonymous=True)
208
 
 
209
 
    sr_fric = SrFrictionCompensation()
210
 
    sr_fric.run()