samplerobot_soft_error_limiter.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 try:
4  from hrpsys.hrpsys_config import *
5  import OpenHRP
6 except:
7  print "import without hrpsys"
8  import rtm
9  from rtm import *
10  from OpenHRP import *
11  import waitInput
12  from waitInput import *
13  import socket
14  import time
15 
16 from subprocess import check_output
17 
18 # Tempolarily remove tc which is limit position range
19 def getRTCList ():
20  return filter(lambda x : x[0]!='tc', hcf.getRTCListUnstable())
21 
22 def init ():
23  global hcf, initial_pose, limit_table_list, bodyinfo, hrpsys_version
24  hcf = HrpsysConfigurator()
25  hcf.getRTCList = getRTCList
26  hcf.init ("SampleRobot(Robot)0", "$(PROJECT_DIR)/../model/sample1.wrl")
27  initial_pose = [-7.779e-005, -0.378613, -0.000209793, 0.832038, -0.452564, 0.000244781, 0.31129, -0.159481, -0.115399, -0.636277, 0, 0, 0, -7.77902e-005, -0.378613, -0.000209794, 0.832038, -0.452564, 0.000244781, 0.31129, 0.159481, 0.115399, -0.636277, 0, 0, 0, 0, 0, 0]
28  # load joint limit table from conf file
29  HRPSYS_DIR=check_output(['pkg-config', 'hrpsys-base', '--variable=prefix']).rstrip()
30  f=open("{}/share/hrpsys/samples/SampleRobot/SampleRobot.500.el.conf".format(HRPSYS_DIR))
31  limit_table_str=filter(lambda x : x.find("joint_limit_table") > -1 , f.readlines())[0]
32  limit_table_list=limit_table_str.split(":")[1:]
33  f.close()
34  # set bodyinfo
35  bodyinfo=hcf.getBodyInfo("$(PROJECT_DIR)/../model/sample1.wrl")
36  # set initial pose from sample/controller/SampleController/etc/Sample.pos
37  hcf.seq_svc.setJointAngles(initial_pose, 2.0)
38  hcf.seq_svc.waitInterpolation()
39  hrpsys_version = hcf.seq.ref.get_component_profile().version
40  print("hrpsys_version = %s"%hrpsys_version)
41 
42 def demo ():
43  init()
44  from distutils.version import StrictVersion
45  if StrictVersion(hrpsys_version) >= StrictVersion('315.5.0'):
50 
52  print >> sys.stderr, "1. demo all jointLimitTables"
53  for table_idx in range(len(limit_table_list)/6):
54  testLimitTables(table_idx, True, 5)
55 
56 def rad2deg (ang):
57  return 180.0*ang/3.14159
58 
59 def deg2rad (ang):
60  return 3.14159*ang/180.0
61 
62 def getJointLimitTableInfo (table_idx=0):
63  self_joint_name = limit_table_list[0+table_idx*6].replace(' ', '')
64  target_joint_name = limit_table_list[1+table_idx*6].replace(' ', '')
65  self_jointId = filter( lambda x : x.name == self_joint_name, bodyinfo._get_links())[0].jointId
66  target_jointId = filter( lambda x : x.name == target_joint_name, bodyinfo._get_links())[0].jointId
67  target_llimit=float(limit_table_list[2+table_idx*6])
68  target_ulimit=float(limit_table_list[3+table_idx*6])
69  self_llimits=map(lambda x : float(x), limit_table_list[4+table_idx*6].split(","))
70  self_ulimits=map(lambda x : float(x), limit_table_list[5+table_idx*6].split(","))
71  return [self_joint_name, target_joint_name,
72  self_jointId, target_jointId,
73  target_llimit, target_ulimit,
74  self_llimits, self_ulimits]
75 
76 def testLimitTables (table_idx=0, debug=True, loop_mod=1):
77  [self_joint_name, target_joint_name,
78  self_jointId, target_jointId,
79  target_llimit, target_ulimit,
80  self_llimits, self_ulimits] = getJointLimitTableInfo(table_idx)
81  lret = testOneLimitTable(self_jointId, target_jointId, self_llimits, target_llimit, target_ulimit, -1, debug, loop_mod)
82  uret = testOneLimitTable(self_jointId, target_jointId, self_ulimits, target_llimit, target_ulimit, 1, debug, loop_mod)
83  print >> sys.stderr, "lower limit check(", self_joint_name, ",", target_joint_name,")=", lret
84  print >> sys.stderr, "upper limit check(", self_joint_name, ",", target_joint_name,")=", uret
85  assert(lret)
86  assert(uret)
87 
88 def testOneLimitTable (self_jointId, target_jointId, limit_table, target_llimit, target_ulimit, angle_violation, debug=True, loop_mod=1, thre=1e-2):
89  tmp_pose=map(lambda x : x, initial_pose)
90  ret=[]
91  for idx in range(int(target_ulimit-target_llimit+1)):
92  if idx%loop_mod != 0: # skip if loop_mod is specified
93  continue
94  if debug:
95  print "idx=",idx,
96  # A-1. set safe joint
97  tmp_pose[target_jointId]=deg2rad(target_llimit + idx);
98  tmp_pose[self_jointId]=deg2rad(limit_table[idx]);
99  if idx == 0:
100  hcf.seq_svc.setJointAngles(tmp_pose, 1);
101  else:
102  hcf.seq_svc.setJointAngles(tmp_pose, 0.01);
103  hcf.seq_svc.waitInterpolation()
104  # A-2. check joint limit is not violated
105  # Dummy setJointAngles to wait for command joint angles are static
106  hcf.seq_svc.setJointAngles(tmp_pose, 0.01);hcf.seq_svc.waitInterpolation();
107  # Use RobotHardware's command as SoftErrorLimiter joint angle output
108  el_out1 = hcf.getActualState().command
109  ret1 = abs(rad2deg(el_out1[self_jointId])-limit_table[idx]) < thre and abs(rad2deg(el_out1[target_jointId])- (target_llimit + idx)) < thre
110  # B-1. set violated joint
111  tmp_pose[self_jointId]=deg2rad(limit_table[idx]+angle_violation);
112  hcf.seq_svc.setJointAngles(tmp_pose, 0.01);
113  hcf.seq_svc.waitInterpolation()
114  # B-2. check joint limit is not violated
115  # Dummy setJointAngles to wait for command joint angles are static
116  hcf.seq_svc.setJointAngles(tmp_pose, 0.01);hcf.seq_svc.waitInterpolation()
117  # Use RobotHardware's command as SoftErrorLimiter joint angle output
118  el_out2 = hcf.getActualState().command
119  ret2 = abs(rad2deg(el_out2[self_jointId]) - limit_table[int(round(rad2deg(el_out2[target_jointId])-target_llimit))]) < thre # Check self and target is on limit table
120  ret2 = ret2 and abs(el_out2[self_jointId] - (limit_table[idx]+angle_violation)) > thre # Check result value is not violated value
121  # C. results
122  if debug:
123  print " ret = (", ret1, ",", ret2,")"
124  print " self=(o1=", rad2deg(el_out1[self_jointId]), ", o2=", rad2deg(el_out2[self_jointId]), ", limit=", limit_table[idx], ") ", " target=(o1=", rad2deg(el_out1[target_jointId]), ", o2=", rad2deg(el_out2[target_jointId]), ", limit=", target_llimit + idx, ") [deg]"
125  ret.append(ret1);
126  ret.append(ret2);
127  hcf.seq_svc.waitInterpolation()
128  hcf.seq_svc.setJointAngles(initial_pose, 1);
129  hcf.seq_svc.waitInterpolation()
130  return all(ret)
131 
132 def setAndCheckJointLimit (joint_name):
133  print >> sys.stderr, " ", joint_name
134  # ulimit check
135  link_info=filter(lambda x : x.name==joint_name, bodyinfo._get_links())[0]
136  hcf.seq_svc.setJointAngle(joint_name, math.radians(1)+link_info.ulimit[0], 1)
137  hcf.waitInterpolation()
138  # Dummy setJointAngles to wait for command joint angles are static
139  hcf.seq_svc.setJointAngle(joint_name, math.radians(1)+link_info.ulimit[0], 0.01);hcf.waitInterpolation()
140  # Use RobotHardware's command as SoftErrorLimiter joint angle output
141  tmppose = hcf.getActualState().command
142  ret = tmppose[link_info.jointId] <= link_info.ulimit[0]
143  print >> sys.stderr, " ulimit = ", ret, "(elout=", tmppose[link_info.jointId], ", limit=", link_info.ulimit[0], ")"
144  assert(ret)
145  # llimit check
146  hcf.seq_svc.setJointAngle(joint_name, math.radians(-1)+link_info.llimit[0], 1)
147  hcf.waitInterpolation()
148  # Dummy setJointAngles to wait for command joint angles are static
149  hcf.seq_svc.setJointAngle(joint_name, math.radians(-1)+link_info.llimit[0], 0.01);hcf.waitInterpolation()
150  # Use RobotHardware's command as SoftErrorLimiter joint angle output
151  tmppose = hcf.getActualState().command
152  ret = tmppose[link_info.jointId] >= link_info.llimit[0]
153  print >> sys.stderr, " llimit = ", ret, "(elout=", tmppose[link_info.jointId], ", limit=", link_info.llimit[0], ")"
154  assert(ret)
155  # go to initial
156  hcf.seq_svc.setJointAngles(initial_pose, 1.0)
157  hcf.waitInterpolation()
158 
160  print >> sys.stderr, "2. Check Position limit"
161  setAndCheckJointLimit('LARM_WRIST_Y')
162  setAndCheckJointLimit('LARM_WRIST_P')
163  setAndCheckJointLimit('LARM_SHOULDER_P')
164 
165 def setAndCheckJointVelocityLimit (joint_name, thre=1e-5, dt=0.002):
166  link_info=filter(lambda x : x.name==joint_name, bodyinfo._get_links())[0]
167  # lvlimit and uvlimit existence check
168  if not(len(link_info.lvlimit) == 1 and len(link_info.uvlimit) == 1):
169  print >> sys.stderr, " ", joint_name, " test neglected because no lvlimit and uvlimit are found."
170  return
171  for is_upper_limit in [True, False]: # uvlimit or lvlimit
172  print >> sys.stderr, " ", joint_name, ", uvlimit" if is_upper_limit else ", lvlimit"
173  # Disable error limit for checking vel limit
174  hcf.el_svc.setServoErrorLimit("all", 100000)
175  # Test motion and logging
176  hcf.clearLog()
177  target_angle = (math.degrees( link_info.ulimit[0] if is_upper_limit else link_info.llimit[0] )*0.99) # 0.99 is margin
178  vel_limit = link_info.uvlimit[0] if is_upper_limit else link_info.lvlimit[0]
179  wait_time = abs(target_angle/math.degrees(vel_limit) * 1.1) # 1.1 is margin
180  hcf.setJointAngle(joint_name, math.degrees(initial_pose[link_info.jointId]), 0.1)
181  hcf.waitInterpolation()
182  hcf.setJointAngle(joint_name, target_angle, 0.002)
183  hcf.waitInterpolation()
184  hcf.setJointAngle(joint_name, target_angle, wait_time) # Wait for finishing of joint motion
185  hcf.waitInterpolation()
186  # Dummy setJointAngles to wait for command joint angles are static
187  hcf.setJointAngle(joint_name, target_angle, 0.01);hcf.waitInterpolation()
188  hcf.saveLog("/tmp/test-samplerobot-el-vel-check")
189  # Check whether joint angle is reached
190  # Use RobotHardware's command as SoftErrorLimiter joint angle output
191  reach_angle = math.degrees(hcf.getActualState().command[link_info.jointId])
192  is_reached = abs(reach_angle - target_angle) < thre
193  # Check actual velocity from Datalogger log
194  poslist=[]
195  for line in open("/tmp/test-samplerobot-el-vel-check.SampleRobot(Robot)0_q", "r"):
196  poslist.append(float(line.split(" ")[link_info.jointId+1]))
197  tmp = map(lambda x,y : x-y, poslist[1:], poslist[0:-1])
198  max_ret_vel = max(tmp)/dt if is_upper_limit else min(tmp)/dt
199  is_vel_limited = abs(max_ret_vel - vel_limit) < thre
200  # Enable error limit by reverting limit value and reset joint angle
201  hcf.el_svc.setServoErrorLimit("all", (0.2-0.02))
202  hcf.setJointAngle(joint_name, math.degrees(initial_pose[link_info.jointId]), 0.5)
203  hcf.waitInterpolation()
204  # Check flags and print
205  print >> sys.stderr, " is_reached =", is_reached, ", is_vel_limited =", is_vel_limited,
206  print >> sys.stderr, ", target_angle =", target_angle, "[deg], reach_angle =", reach_angle, "[deg], max_ret_vel =", max_ret_vel, "[rad/s], vel_limit =", vel_limit, "[rad/s]"
207  assert(is_reached and is_vel_limited)
208 
210  print >> sys.stderr, "3. Check Velocity limit"
211  setAndCheckJointVelocityLimit('LARM_WRIST_Y')
212  setAndCheckJointVelocityLimit('LARM_WRIST_P')
213 
214 def setAndCheckJointErrorLimit (joint_name, thre=1e-5):
215  link_info=filter(lambda x : x.name==joint_name, bodyinfo._get_links())[0]
216  for is_upper_limit in [True, False]: # uvlimit or lvlimit
217  print >> sys.stderr, " ", joint_name, ", uvlimit" if is_upper_limit else ", lvlimit"
218  # Disable error limit for checking vel limit
219  error_limit = 0.002 if is_upper_limit else -0.002 # [rad]
220  hcf.el_svc.setServoErrorLimit("all", abs(error_limit))
221  # Test motion and logging
222  hcf.clearLog()
223  target_angle = 3.0 if is_upper_limit else -3.0 # [deg]
224  wait_time = 0.2
225  hcf.setJointAngle(joint_name, math.degrees(initial_pose[link_info.jointId]), 0.1)
226  hcf.waitInterpolation()
227  hcf.setJointAngle(joint_name, target_angle, 0.002)
228  hcf.waitInterpolation()
229  hcf.setJointAngle(joint_name, target_angle, wait_time) # Wait for finishing of joint motion
230  hcf.waitInterpolation()
231  # Dummy setJointAngles to wait for command joint angles are static
232  hcf.setJointAngle(joint_name, target_angle, 0.01);hcf.waitInterpolation()
233  hcf.saveLog("/tmp/test-samplerobot-el-err-check")
234  # Check whether joint angle is reached
235  # Use RobotHardware's command as SoftErrorLimiter joint angle output
236  reach_angle = math.degrees(hcf.getActualState().command[link_info.jointId])
237  is_reached = abs(reach_angle - target_angle) < thre
238  # Check actual velocity from Datalogger log
239  poslist=[]
240  for line in open("/tmp/test-samplerobot-el-err-check.SampleRobot(Robot)0_q", "r"):
241  poslist.append(float(line.split(" ")[link_info.jointId+1]))
242  refposlist=[]
243  for line in open("/tmp/test-samplerobot-el-err-check.el_q", "r"):
244  refposlist.append(float(line.split(" ")[link_info.jointId+1]))
245  tmp = map(lambda x,y : x-y, refposlist, poslist)
246  max_ret_err = max(tmp) if is_upper_limit else min(tmp)
247  is_err_limited = abs(max_ret_err - error_limit) < thre
248  # Enable error limit by reverting limit value and reset joint angle
249  hcf.el_svc.setServoErrorLimit("all", (0.2-0.02))
250  hcf.setJointAngle(joint_name, math.degrees(initial_pose[link_info.jointId]), 0.5)
251  hcf.waitInterpolation()
252  # Check flags and print
253  print >> sys.stderr, " is_reached =", is_reached, ", is_err_limited =", is_err_limited,
254  print >> sys.stderr, ", target_angle =", target_angle, "[deg], reach_angle =", reach_angle, "[deg], max_ret_err =", max_ret_err, "[rad], err_limit =", error_limit, "[rad]"
255  assert(is_reached and is_err_limited)
256 
258  print >> sys.stderr, "4. Check Error limit"
259  setAndCheckJointErrorLimit('LARM_WRIST_Y')
260  setAndCheckJointErrorLimit('LARM_WRIST_P')
261 
262 
263 if __name__ == '__main__':
264  demo()
#define max(a, b)
def setAndCheckJointErrorLimit(joint_name, thre=1e-5)
#define min(a, b)
def testOneLimitTable(self_jointId, target_jointId, limit_table, target_llimit, target_ulimit, angle_violation, debug=True, loop_mod=1, thre=1e-2)
def testLimitTables(table_idx=0, debug=True, loop_mod=1)
def setAndCheckJointVelocityLimit(joint_name, thre=1e-5, dt=0.002)


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Thu May 6 2021 02:41:51