test_footstep_controller.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 # 1. roslaunch hrpsys_gazebo_tutorials gazebo_jaxon_no_controllers.launch
4 # 2. rtmlaunch hrpsys_gazebo_tutorials jaxon_hrpsys_bringup.launch KINEMATICS_MODE:=true
5 # 3. rosrun jsk_footstep_controller footstep-controller.l __name:=footstep_controller
6 # 4. rosrun jsk_footstep_controller test_footstep_controller.py
7 
8 import rospy
9 import actionlib
10 import tf
11 
12 from jsk_footstep_msgs.msg import Footstep, FootstepArray, ExecFootstepsAction, ExecFootstepsGoal
13 from geometry_msgs.msg import Pose
14 # pose ... matrix
15 
16 from tf.transformations import *
17 
18 def matToMsg(mat):
19  q = quaternion_from_matrix(mat)
20  p = translation_from_matrix(mat)
21  pose = Pose()
22  pose.orientation.x = q[0]
23  pose.orientation.y = q[1]
24  pose.orientation.z = q[2]
25  pose.orientation.w = q[3]
26  pose.position.x = p[0]
27  pose.position.y = p[1]
28  pose.position.z = p[2]
29  return pose
30 
31 def msgToMat(pose):
32  return concatenate_matrices(translation_matrix([pose.position.x,
33  pose.position.y,
34  pose.position.z]),
35  quaternion_matrix([pose.orientation.x,
36  pose.orientation.y,
37  pose.orientation.z,
38  pose.orientation.w]))
39 def transRotToMat(trans, rot):
40  return concatenate_matrices(translation_matrix([trans[0],
41  trans[1],
42  trans[2]]),
43  quaternion_matrix([rot[0],
44  rot[1],
45  rot[2],
46  rot[3]]))
47 
48 def sendFootstep(footstep):
49  pub_footsteps.publish(footstep)
50  goal = ExecFootstepsGoal(footstep=footstep)
51  client.send_goal(goal)
52  client.wait_for_result()
53 
54 def oneFootstep(origin, leg, x=0, y=0, z=0, roll=0, pitch=0, yaw=0):
55  msg = Footstep()
56  msg.leg = leg
57  translation = [x, y, z]
58  q = quaternion_from_euler(roll, pitch, yaw)
59  offset = transRotToMat(translation, q)
60  target = concatenate_matrices(origin, offset)
61  print(origin)
62  msg.pose = matToMsg(target)
63  msg.dimensions.x = 0.23
64  msg.dimensions.y = 0.13
65  msg.dimensions.z = 0.001
66  return msg
67 
68 def getCurrentLeftLeg():
69  tf_listener.waitForTransform('lleg_end_coords', 'odom', rospy.Time(0), rospy.Duration(10))
70  (trans, rot) = tf_listener.lookupTransform('odom', 'lleg_end_coords', rospy.Time(0))
71  return transRotToMat(trans, rot)
72 
73 def currentSpot():
74  origin = getCurrentLeftLeg()
75  footsteps = FootstepArray()
76  footsteps.footsteps = [oneFootstep(origin, Footstep.LEFT, 0, 0),
77  oneFootstep(origin, Footstep.RIGHT, 0, -0.2),
78  oneFootstep(origin, Footstep.LEFT, 0, 0),
79  oneFootstep(origin, Footstep.RIGHT, 0, -0.2),
80  oneFootstep(origin, Footstep.LEFT, 0, 0),
81  oneFootstep(origin, Footstep.RIGHT, 0, -0.2),
82  oneFootstep(origin, Footstep.LEFT, 0, 0),
83  oneFootstep(origin, Footstep.RIGHT, 0, -0.2)]
84  footsteps.header.frame_id = "odom"
85  footsteps.header.stamp = rospy.Time.now()
86  sendFootstep(footsteps)
87 
88 def straightSteps():
89  origin = getCurrentLeftLeg()
90  footsteps = FootstepArray()
91  footsteps.footsteps = [oneFootstep(origin, Footstep.LEFT, 0, 0),
92  oneFootstep(origin, Footstep.RIGHT, 0.2, -0.2),
93  oneFootstep(origin, Footstep.LEFT, 0.4, 0),
94  oneFootstep(origin, Footstep.RIGHT, 0.6, -0.2),
95  oneFootstep(origin, Footstep.LEFT, 0.8, 0),
96  oneFootstep(origin, Footstep.RIGHT, 1.0, -0.2),
97  oneFootstep(origin, Footstep.LEFT, 1.2, 0),
98  oneFootstep(origin, Footstep.RIGHT, 1.2, -0.2)]
99  footsteps.header.frame_id = "odom"
100  footsteps.header.stamp = rospy.Time.now()
101  sendFootstep(footsteps)
102 
103 def backSteps():
104  origin = getCurrentLeftLeg()
105  footsteps = FootstepArray()
106  footsteps.footsteps = [oneFootstep(origin, Footstep.LEFT, 0, 0),
107  oneFootstep(origin, Footstep.RIGHT, -0.2, -0.2),
108  oneFootstep(origin, Footstep.LEFT, -0.4, 0),
109  oneFootstep(origin, Footstep.RIGHT, -0.6, -0.2),
110  oneFootstep(origin, Footstep.LEFT, -0.8, 0),
111  oneFootstep(origin, Footstep.RIGHT, -1.0, -0.2),
112  oneFootstep(origin, Footstep.LEFT, -1.2, 0),
113  oneFootstep(origin, Footstep.RIGHT, -1.2, -0.2)]
114  footsteps.header.frame_id = "odom"
115  footsteps.header.stamp = rospy.Time.now()
116  sendFootstep(footsteps)
117 
118 def stairUp():
119  origin = getCurrentLeftLeg()
120  footsteps = FootstepArray()
121  footsteps.footsteps = [oneFootstep(origin, Footstep.LEFT, 0, 0, 0),
122  oneFootstep(origin, Footstep.RIGHT, 0.2, -0.2, 0.0),
123  oneFootstep(origin, Footstep.LEFT, 0.4, 0, 0.1),
124  oneFootstep(origin, Footstep.RIGHT, 0.6, -0.2, 0.1),
125  oneFootstep(origin, Footstep.LEFT, 0.8, 0, 0.2),
126  oneFootstep(origin, Footstep.RIGHT, 1.0, -0.2, 0.2),
127  oneFootstep(origin, Footstep.LEFT, 1.2, 0, 0.3),
128  oneFootstep(origin, Footstep.RIGHT, 1.2, -0.2, 0.3)]
129  footsteps.header.frame_id = "odom"
130  footsteps.header.stamp = rospy.Time.now()
131  sendFootstep(footsteps)
132 
133 def stairDown():
134  origin = getCurrentLeftLeg()
135  footsteps = FootstepArray()
136  footsteps.footsteps = [oneFootstep(origin, Footstep.LEFT, 0, 0, 0),
137  oneFootstep(origin, Footstep.RIGHT, 0.2, -0.2, 0.0),
138  oneFootstep(origin, Footstep.LEFT, 0.4, 0, -0.1),
139  oneFootstep(origin, Footstep.RIGHT, 0.6, -0.2, -0.1),
140  oneFootstep(origin, Footstep.LEFT, 0.8, 0, -0.2),
141  oneFootstep(origin, Footstep.RIGHT, 1.0, -0.2, -0.2),
142  oneFootstep(origin, Footstep.LEFT, 1.2, 0, -0.3),
143  oneFootstep(origin, Footstep.RIGHT, 1.2, -0.2, -0.3)]
144  footsteps.header.frame_id = "odom"
145  footsteps.header.stamp = rospy.Time.now()
146  sendFootstep(footsteps)
147 
148 def slopeUp():
149  origin = getCurrentLeftLeg()
150  footsteps = FootstepArray()
151  footsteps.footsteps = [oneFootstep(origin, Footstep.LEFT, 0, 0, 0),
152  oneFootstep(origin, Footstep.RIGHT, 0.2, -0.2, 0.0),
153  oneFootstep(origin, Footstep.LEFT, 0.4, 0, 0.1, pitch=-0.2),
154  oneFootstep(origin, Footstep.RIGHT, 0.6, -0.2, 0.1, pitch=-0.2),
155  oneFootstep(origin, Footstep.LEFT, 0.8, 0, 0.2, pitch=-0.2),
156  oneFootstep(origin, Footstep.RIGHT, 1.0, -0.2, 0.2, pitch=-0.2),
157  oneFootstep(origin, Footstep.LEFT, 1.2, 0, 0.3, pitch=0.0),
158  oneFootstep(origin, Footstep.RIGHT, 1.2, -0.2, 0.3, pitch=0.0)]
159  footsteps.header.frame_id = "odom"
160  footsteps.header.stamp = rospy.Time.now()
161  sendFootstep(footsteps)
162 
163 def slopeDown():
164  origin = getCurrentLeftLeg()
165  footsteps = FootstepArray()
166  footsteps.footsteps = [oneFootstep(origin, Footstep.LEFT, 0, 0, 0),
167  oneFootstep(origin, Footstep.RIGHT, 0.2, -0.2, 0.0),
168  oneFootstep(origin, Footstep.LEFT, 0.4, 0, -0.1, pitch=0.2),
169  oneFootstep(origin, Footstep.RIGHT, 0.6, -0.2, -0.1, pitch=0.2),
170  oneFootstep(origin, Footstep.LEFT, 0.8, 0, -0.2, pitch=0.2),
171  oneFootstep(origin, Footstep.RIGHT, 1.0, -0.2, -0.2, pitch=0.2),
172  oneFootstep(origin, Footstep.LEFT, 1.2, 0, -0.3, pitch=0.0),
173  oneFootstep(origin, Footstep.RIGHT, 1.2, -0.2, -0.3, pitch=0.0)]
174  footsteps.header.frame_id = "odom"
175  footsteps.header.stamp = rospy.Time.now()
176  sendFootstep(footsteps)
177 
178 
179 if __name__ == "__main__":
180  rospy.init_node("test_footstep_controller")
181  tf_listener = tf.TransformListener()
182  client = actionlib.SimpleActionClient('footstep_controller', ExecFootstepsAction)
183  pub_footsteps = rospy.Publisher('footstep', FootstepArray, latch=True, queue_size=1)
184  rospy.loginfo("Waiting for footstep controller")
185  client.wait_for_server()
186  rospy.loginfo("Found footstep controller")
187  currentSpot()
188  straightSteps()
189  backSteps()
190  stairUp()
191  stairDown()
192  slopeUp()
193  slopeDown()
test_footstep_controller.sendFootstep
def sendFootstep(footstep)
Definition: test_footstep_controller.py:48
test_footstep_controller.msgToMat
def msgToMat(pose)
Definition: test_footstep_controller.py:31
test_footstep_controller.oneFootstep
def oneFootstep(origin, leg, x=0, y=0, z=0, roll=0, pitch=0, yaw=0)
Definition: test_footstep_controller.py:54
test_footstep_controller.stairUp
def stairUp()
Definition: test_footstep_controller.py:118
test_footstep_controller.stairDown
def stairDown()
Definition: test_footstep_controller.py:133
test_footstep_controller.getCurrentLeftLeg
def getCurrentLeftLeg()
Definition: test_footstep_controller.py:68
test_footstep_controller.slopeUp
def slopeUp()
Definition: test_footstep_controller.py:148
test_footstep_controller.matToMsg
def matToMsg(mat)
Definition: test_footstep_controller.py:18
test_footstep_controller.backSteps
def backSteps()
Definition: test_footstep_controller.py:103
test_footstep_controller.currentSpot
def currentSpot()
Definition: test_footstep_controller.py:73
test_footstep_controller.transRotToMat
def transRotToMat(trans, rot)
Definition: test_footstep_controller.py:39
test_footstep_controller.slopeDown
def slopeDown()
Definition: test_footstep_controller.py:163
test_footstep_controller.straightSteps
def straightSteps()
Definition: test_footstep_controller.py:88

jsk_footstep_controller
Author(s):
autogenerated on Fri May 14 2021 02:52:04