test_hironx_fullbody.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # -*- coding: utf-8 -*-
3 
4 from test_hironx import *
5 
6 class TestHiroFullbody(TestHiro):
7 
8  def test_collision_detector_disabled(self):
9  self.assertFalse(['co', "CollisionDetector"] in self.robot.getRTCList())
10 
11  def fullbody_init (self):
12  self.filenames = []
13  self.robot.seq_svc.removeJointGroup("larm")
14  self.robot.seq_svc.removeJointGroup("rarm")
15  self.robot.seq_svc.removeJointGroup("head")
16  #self.robot.seq_svc.removeJointGroup("torso") we use torso for sleep certain time, so not remove this
17  self.filename_base = tempfile.mkstemp()[1]
18 
19  def test_fullbody_setJointAngles_Wait (self):
20  self.fullbody_init()
21  # move to initiali position
22  self.robot.setJointAngles([0, 0, 0, 0, -0.6, 0, -120, 15.2, 9.4, 3.2, 0,0,0,0, -0.6, 0, -100,-15.2, 9.4,-3.2, 0, 0, 0, 0], 5);
23  self.robot.waitInterpolation()
24 
25  self.robot.clearLog()
26  self.robot.setJointAngles([0, 0, 0, 0, -0.6, 0, -140, 15.2, 9.4, 3.2, 0,0,0,0, -0.6, 0, -100,-15.2, 9.4,-3.2, 0, 0, 0, 0], 5);
27  self.robot.waitInterpolation()
28  self.robot.setJointAngles([0, 0, 0, 0, -0.6, 0, -100, 15.2, 9.4, 3.2, 0,0,0,0, -0.6, 0, -100,-15.2, 9.4,-3.2, 0, 0, 0, 0], 5);
29  self.robot.waitInterpolation()
30  filename = self.filename_base + "-wait"
31  self.robot.saveLog(filename)
32  q_filename = filename+"."+self.robot.rh.name()+"_q"
33  data = self.load_log_data(q_filename)
34 
35  print "check setJointAngles(wait=True)"
36  self.check_log_data(data, 6, 10.0, -140.0, -100.0)
37  return True
38 
39  def test_fullbody_setJointAngles_NoWait (self):
40  self.fullbody_init()
41  clear_time = [4.5, 4.0, 3.5, 3.0, 2.5, 2.0, 1.5, 1.0]
42  self.robot.setJointAngles([0, 0, 0, 0, -0.6, 0, -100, 15.2, 9.4, 3.2, 0,0,0,0, -0.6, 0, -100,-15.2, 9.4,-3.2, 0, 0, 0, 0], 5);
43  self.robot.waitInterpolation()
44  for i in range(len(clear_time)):
45  self.robot.clearLog()
46  self.robot.setJointAngles([0, 0, 0, 0, -0.6, 0, -140, 15.2, 9.4, 3.2, 0,0,0,0, -0.6, 0, -100,-15.2, 9.4,-3.2, 0, 0, 0, 0], 5);
47  self.robot.setJointAnglesOfGroup("torso", [0], clear_time[i], wait=True);# time.sleep(clear_time[i]);
48 
49  self.robot.setJointAngles([0, 0, 0, 0, -0.6, 0, -100, 15.2, 9.4, 3.2, 0,0,0,0, -0.6, 0, -100,-15.2, 9.4,-3.2, 0, 0, 0, 0], 5);
50  self.robot.waitInterpolation()
51  filename = self.filename_base + "-no-wait-"+str(clear_time[i])
52  self.robot.saveLog(filename)
53  q_filename = filename+"."+self.robot.rh.name()+"_q"
54  data = self.load_log_data(q_filename)
55 
56  print "check setJointAngles(wait=True)"
57  self.check_log_data(data, 6, (10.0 - (5 - clear_time[i])), [-140+i*40/len(clear_time),20], -100.0)
58 
59  def test_fullbody_setJointAngles_Clear (self):
60  self.fullbody_init()
61  # check if clear stops interpolation
62  clear_time = [4.5, 4.0, 3.5, 3.0, 2.5, 2.0, 1.5, 1.0]
63  for i in range(len(clear_time)):
64  self.robot.setJointAngles([0, 0, 0, 0, -0.6, 0, -100, 15.2, 9.4, 3.2, 0,0,0,0, -0.6, 0, -100,-15.2, 9.4,-3.2, 0, 0, 0, 0], 5);
65  self.robot.waitInterpolation()
66  self.robot.clearLog()
67  self.robot.setJointAngles([0, 0, 0, 0, -0.6, 0, -100, 15.2, 9.4, 3.2, 0,0,0,0, -0.6, 0, -100,-15.2, 9.4,-3.2, 0, 0, 0, 0], 5-clear_time[i]);
68  self.robot.waitInterpolation()
69  self.robot.setJointAngles([0, 0, 0, 0, -0.6, 0, -140, 15.2, 9.4, 3.2, 0,0,0,0, -0.6, 0, -100,-15.2, 9.4,-3.2, 0, 0, 0, 0], 5);
70  self.robot.setJointAnglesOfGroup("torso", [0], clear_time[i], wait=True);# time.sleep(clear_time[i]);
71 
72  self.robot.clear()
73  self.robot.waitInterpolation()
74  filename = self.filename_base + "-clear-"+str(clear_time[i])
75  self.robot.saveLog(filename)
76  q_filename = filename+"."+self.robot.rh.name()+"_q"
77  data = self.load_log_data(q_filename)
78 
79  print "check setJointAngles(Clear)"
80  self.check_log_data(data, 6, 5, [-140+(i+1)*40/len(clear_time),20], -100.0, acc_thre=0.2)
81 
82  def test_fullbody_setJointAngles_minus(self):
83  self.fullbody_init()
84  self.robot.el_svc.setServoErrorLimit("all", 0.01) # default is 0.18
85 
86  self.robot.setJointAngles([0, 0, 0, 0, -0.6, 0, -120, 15.2, 9.4, 3.2, 0,0,0,0, -0.6, 0, -100,-15.2, 9.4,-3.2, 0, 0, 0, 0], 3);
87  self.robot.waitInterpolation()
88  self.robot.clearLog()
89  self.robot.setJointAngles([0, 0, 0, 0, -0.6, 0, -120, 15.2, 9.4, 3.2, 0,0,0,0, -0.6, 0, -100,-15.2, 9.4,-3.2, 0, 0, 0, 0], 1);
90  self.robot.waitInterpolation()
91 
92  self.robot.setJointAngles([0, 0, 0, 0, -0.6, 0, -140, 15.2, 9.4, 3.2, 0,0,0,0, -0.6, 0, -100,-15.2, 9.4,-3.2, 0, 0, 0, 0], -1);
93  self.robot.waitInterpolation()
94 
95  self.robot.setJointAngles([0, 0, 0, 0, -0.6, 0, -140, 15.2, 9.4, 3.2, 0,0,0,0, -0.6, 0, -100,-15.2, 9.4,-3.2, 0, 0, 0, 0], 3);
96  self.robot.waitInterpolation()
97 
98  self.robot.setJointAngles([0, 0, 0, 0, -0.6, 0, -120, 15.2, 9.4, 3.2, 0,0,0,0, -0.6, 0, -100,-15.2, 9.4,-3.2, 0, 0, 0, 0], 0);
99  self.robot.waitInterpolation()
100 
101  self.robot.setJointAngles([0, 0, 0, 0, -0.6, 0, -120, 15.2, 9.4, 3.2, 0,0,0,0, -0.6, 0, -100,-15.2, 9.4,-3.2, 0, 0, 0, 0], 1);
102  self.robot.waitInterpolation()
103 
104  self.robot.setJointAngles([0, 0, 0, 0, -0.6, 0, -140, 15.2, 9.4, 3.2, 0,0,0,0, -0.6, 0, -100,-15.2, 9.4,-3.2, 0, 0, 0, 0], 0.1);
105  self.robot.waitInterpolation()
106 
107  self.robot.setJointAngles([0, 0, 0, 0, -0.6, 0, -140, 15.2, 9.4, 3.2, 0,0,0,0, -0.6, 0, -100,-15.2, 9.4,-3.2, 0, 0, 0, 0], 1.0);
108  self.robot.waitInterpolation()
109 
110  self.robot.setJointAngles([0, 0, 0, 0, -0.6, 0, -120, 15.2, 9.4, 3.2, 0,0,0,0, -0.6, 0, -100,-15.2, 9.4,-3.2, 0, 0, 0, 0], 0.05);
111  self.robot.waitInterpolation()
112 
113  self.robot.setJointAngles([0, 0, 0, 0, -0.6, 0, -120, 15.2, 9.4, 3.2, 0,0,0,0, -0.6, 0, -100,-15.2, 9.4,-3.2, 0, 0, 0, 0], 1.0);
114  self.robot.waitInterpolation()
115 
116  filename = self.filename_base + "-minus"
117  self.robot.saveLog(filename)
118  # write pdf file
119  q_filename = filename+"."+self.robot.rh.name()+"_q"
120  self.write_all_joint_pdf(q_filename, "full_minus_check.pdf")
121 
122  # assertion
123  data = self.load_log_data(q_filename)
124  print "check setJointAngles(minus)"
125  self.check_log_data(data, 6, 7.19, -140, -120.0, acc_thre = 1.0)
126 
127  self.robot.el_svc.setServoErrorLimit("all", 0.18) # default is 0.18
128 
129 if __name__ == '__main__':
130  import rostest
131  rostest.rosrun(PKG, 'test_hronx_fullbody', TestHiroFullbody)
test_hironx_fullbody.TestHiroFullbody.test_fullbody_setJointAngles_minus
def test_fullbody_setJointAngles_minus(self)
Definition: test_hironx_fullbody.py:82
test_hironx_fullbody.TestHiroFullbody
Definition: test_hironx_fullbody.py:6
test_hironx.TestHiro.check_log_data
def check_log_data(self, data, idx, tm_data, min_data, max_data, acc_thre=0.06, tm_thre=0.1)
Definition: test_hironx.py:111
test_hironx_fullbody.TestHiroFullbody.fullbody_init
def fullbody_init(self)
Definition: test_hironx_fullbody.py:11
test_hironx_fullbody.TestHiroFullbody.filename_base
filename_base
Definition: test_hironx_fullbody.py:17
test_hironx_fullbody.TestHiroFullbody.test_fullbody_setJointAngles_NoWait
def test_fullbody_setJointAngles_NoWait(self)
Definition: test_hironx_fullbody.py:39
test_hironx.TestHiro.write_all_joint_pdf
def write_all_joint_pdf(self, name, pdf_name)
Definition: test_hironx.py:175
test_hironx_fullbody.TestHiroFullbody.test_fullbody_setJointAngles_Wait
def test_fullbody_setJointAngles_Wait(self)
Definition: test_hironx_fullbody.py:19
test_hironx_fullbody.TestHiroFullbody.test_collision_detector_disabled
def test_collision_detector_disabled(self)
Definition: test_hironx_fullbody.py:8
test_hironx.TestHiro.load_log_data
def load_log_data(self, name)
Definition: test_hironx.py:99
test_hironx.TestHiro
Definition: test_hironx.py:29
test_hironx_fullbody.TestHiroFullbody.test_fullbody_setJointAngles_Clear
def test_fullbody_setJointAngles_Clear(self)
Definition: test_hironx_fullbody.py:59
test_hironx_fullbody.TestHiroFullbody.filenames
filenames
Definition: test_hironx_fullbody.py:12

hironx_ros_bridge
Author(s): Kei Okada , Isaac I.Y. Saito
autogenerated on Thu May 14 2020 03:52:05