Skip to content

Commit cfaa21c

Browse files
caguerocaguero
committed
Merge from default.
2 parents 94845d6 + ccfbf93 commit cfaa21c

File tree

29 files changed

+1715
-10
lines changed

29 files changed

+1715
-10
lines changed

‎attach_gripper/tutorial.md

+3-3
Original file line numberDiff line numberDiff line change
@@ -24,7 +24,7 @@ Start up gazebo and make sure you can load the models from the two previous tuto
2424

2525
update the contents to make the model body larger and re-position the wheels accordingly:
2626

27-
<include src='https://bitbucket.org/osrf/gazebo_tutorials/raw/attach_gripper/files/model.sdf' />
27+
<include src='https://bitbucket.org/osrf/gazebo_tutorials/raw/default/attach_gripper/files/model.sdf' />
2828

2929
[[file:files/Mobile_base_large.png|640px]]
3030

@@ -40,15 +40,15 @@ Start up gazebo and make sure you can load the models from the two previous tuto
4040

4141
populate it with the following contents:
4242

43-
<include src='https://bitbucket.org/osrf/gazebo_tutorials/raw/attach_gripper/files/model.config' />
43+
<include src='https://bitbucket.org/osrf/gazebo_tutorials/raw/default/attach_gripper/files/model.config' />
4444

4545
1. Next, create the model SDF file:
4646

4747
gedit ~/.gazebo/models/simple_mobile_manipulator/manipulator.sdf
4848

4949
and populate with following contents:
5050

51-
<include src='https://bitbucket.org/osrf/gazebo_tutorials/raw/attach_gripper/files/manipulator.sdf' />
51+
<include src='https://bitbucket.org/osrf/gazebo_tutorials/raw/default/attach_gripper/files/manipulator.sdf' />
5252

5353
1. Make sure the `model.config` and `manipulator.sdf` files above are saved, start Gazebo and spawn the model above by using the **insert** tab and choosing **Simple Mobile Manipulator** model. You should see something similar to:
5454

‎build_model/tutorial.md

+1-1
Original file line numberDiff line numberDiff line change
@@ -48,7 +48,7 @@ gedit box.sdf
4848
~~~
4949

5050
Copy the following contents into [box.sdf](http://bitbucket.org/osrf/gazebo_tutorials/raw/default/build_model/files/box.sdf):
51-
<include src='http://bitbucket.org/osrf/gazebo_tutorials/raw/build_model/files/box.sdf' />
51+
<include src='http://bitbucket.org/osrf/gazebo_tutorials/raw/default/build_model/files/box.sdf' />
5252

5353
Note that the origin of the Box-geometry is at the geometric center of the box, so in order to have the bottom of the box flush with the ground plane, an origin of `<pose>0 0 0.5 0 0 0</pose>` is added to raise the box above the ground plane.
5454
> **Tip:** The above example sets the simple box model to be static, which makes the model immovable. This feature is useful during model creation. Once you are done creating your model, set the `<static>` tag to false if you want your model to be movable.

‎dem/tutorial.md

+1-1
Original file line numberDiff line numberDiff line change
@@ -46,7 +46,7 @@ $ gdalwarp -ts 129 129 /tmp/mtsthelens.dem /tmp/media/dem/mtsthelens_129.dem
4646

4747
A DEM file in Gazebo is loaded in the same way that you load a heightmap image. Gazebo automatically detects if the file is a plain image or a DEM file. Create the file `volcano.world` and copy the next content. Save the file anywhere you want, for example, in /tmp.
4848

49-
<include src='http://bitbucket.org/osrf/gazebo_tutorials/raw/dem/files/volcano.world' />
49+
<include src='http://bitbucket.org/osrf/gazebo_tutorials/raw/default/dem/files/volcano.world' />
5050

5151
The `<heightmap><size>` element in the code above, tells Gazebo whether to load the DEM with the original dimensions (when `<size>` is not present) or to scale it (when `<size>` is present). In case you prefer to scale the DEM, the `<size>` element tells Gazebo the size in meters that the terrain will have in the simulation. If you want to maintain the correct aspect ratio, be sure to properly calculate the width, height and elevation (which is the third number in `<size>`). In our example, the DEM will be scaled to a square of 150 x 150 meters and a max elevation of 50 meters.
5252

‎drcsim_atlas_siminterface/files/Asi_walk.png

285 KB
Loading

‎drcsim_atlas_siminterface/files/walk.py

+179
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,179 @@
1+
#! /usr/bin/env python
2+
import roslib; roslib.load_manifest('atlas_sim_interface_tutorial')
3+
4+
from atlas_msgs.msg import AtlasSimInterfaceCommand, AtlasSimInterfaceState, AtlasState
5+
from geometry_msgs.msg import Pose, Point
6+
from std_msgs.msg import String
7+
from tf.transformations import quaternion_from_euler, euler_from_quaternion
8+
9+
import math
10+
import rospy
11+
import sys
12+
13+
class AtlasWalk():
14+
15+
def walk(self):
16+
# Initialize atlas mode and atlas_sim_interface_command publishers
17+
self.mode = rospy.Publisher('/atlas/mode', String, None, False, \
18+
True, None)
19+
self.asi_command = rospy.Publisher('/atlas/atlas_sim_interface_command', AtlasSimInterfaceCommand, None, False, True, None)
20+
21+
# Assume that we are already in BDI Stand mode
22+
23+
# Initialize some variables before starting.
24+
self.step_index = 0
25+
self.is_swaying = False
26+
27+
# Subscribe to atlas_state and atlas_sim_interface_state topics.
28+
self.asi_state = rospy.Subscriber('/atlas/atlas_sim_interface_state', AtlasSimInterfaceState, self.asi_state_cb)
29+
self.atlas_state = rospy.Subscriber('/atlas/atlas_state', AtlasState, self.atlas_state_cb)
30+
31+
# Walk in circles until shutdown.
32+
while not rospy.is_shutdown():
33+
rospy.spin()
34+
print("Shutting down")
35+
36+
# /atlas/atlas_sim_interface_state callback. Before publishing a walk command, we need
37+
# the current robot position
38+
def asi_state_cb(self, state):
39+
try:
40+
x = self.robot_position.x
41+
except AttributeError:
42+
self.robot_position = Point()
43+
self.robot_position.x = state.pos_est.position.x
44+
self.robot_position.y = state.pos_est.position.y
45+
self.robot_position.z = state.pos_est.position.z
46+
47+
if self.is_static:
48+
self.static(state)
49+
else:
50+
self.dynamic(state)
51+
52+
# /atlas/atlas_state callback. This message provides the orientation of the robot from the torso IMU
53+
# This will be important if you need to transform your step commands from the robot's local frame to world frame
54+
def atlas_state_cb(self, state):
55+
# If you don't reset to harnessed, then you need to get the current orientation
56+
roll, pitch, yaw = euler_from_quaternion([state.orientation.x, state.orientation.y, state.orientation.z, state.orientation.w])
57+
58+
# An example of commanding a dynamic walk behavior.
59+
def dynamic(self, state):
60+
command = AtlasSimInterfaceCommand()
61+
command.behavior = AtlasSimInterfaceCommand.WALK
62+
63+
# k_effort is all 0s for full BDI controll of all joints.
64+
command.k_effort = [0] * 28
65+
66+
# Observe next_step_index_needed to determine when to switch steps.
67+
self.step_index = state.walk_feedback.next_step_index_needed
68+
69+
# A walk behavior command needs to know three additional steps beyond the current step needed to plan
70+
# for the best balance
71+
for i in range(4):
72+
step_index = self.step_index + i
73+
is_right_foot = step_index % 2
74+
75+
command.walk_params.step_queue[i].step_index = step_index
76+
command.walk_params.step_queue[i].foot_index = is_right_foot
77+
78+
# A duration of 0.63s is a good default value
79+
command.walk_params.step_queue[i].duration = 0.63
80+
81+
# As far as I can tell, swing_height has yet to be implemented
82+
command.walk_params.step_queue[i].swing_height = 0.2
83+
84+
# Determine pose of the next step based on the step_index
85+
command.walk_params.step_queue[i].pose = self.calculate_pose(step_index)
86+
87+
# Publish this command every time we have a new state message
88+
self.asi_command.publish(command)
89+
# End of dynamic walk behavior
90+
91+
# An example of commanding a static walk/step behavior.
92+
def static(self, state):
93+
94+
# When the robot status_flags are 1 (SWAYING), you can publish the next step command.
95+
if (state.step_feedback.status_flags == 1 and not self.is_swaying):
96+
self.step_index += 1
97+
self.is_swaying = True
98+
print("Step " + str(self.step_index))
99+
elif (state.step_feedback.status_flags == 2):
100+
self.is_swaying = False
101+
102+
is_right_foot = self.step_index % 2
103+
104+
command = AtlasSimInterfaceCommand()
105+
command.behavior = AtlasSimInterfaceCommand.STEP
106+
107+
# k_effort is all 0s for full bdi control of all joints
108+
command.k_effort = [0] * 28
109+
110+
# step_index should always be one for a step command
111+
command.step_params.desired_step.step_index = 1
112+
command.step_params.desired_step.foot_index = is_right_foot
113+
114+
# duration has far as I can tell is not observed
115+
command.step_params.desired_step.duration = 0.63
116+
117+
# swing_height is not observed
118+
command.step_params.desired_step.swing_height = 0.1
119+
120+
if self.step_index > 30:
121+
print(str(self.calculate_pose(self.step_index)))
122+
# Determine pose of the next step based on the number of steps we have taken
123+
command.step_params.desired_step.pose = self.calculate_pose(self.step_index)
124+
125+
# Publish a new step command every time a state message is received
126+
self.asi_command.publish(command)
127+
# End of static walk behavior
128+
129+
# This method is used to calculate a pose of step based on the step_index
130+
# The step poses just cause the robot to walk in a circle
131+
def calculate_pose(self, step_index):
132+
# Right foot occurs on even steps, left on odd
133+
is_right_foot = step_index % 2
134+
is_left_foot = 1 - is_right_foot
135+
136+
# There will be 60 steps to a circle, and so our position along the circle is current_step
137+
current_step = step_index % 60
138+
139+
# yaw angle of robot around circle
140+
theta = current_step * math.pi / 30
141+
142+
R = 2 # Radius of turn
143+
W = 0.3 # Width of stride
144+
145+
# Negative for inside foot, positive for outside foot
146+
offset_dir = 1 - 2 * is_left_foot
147+
148+
# Radius from center of circle to foot
149+
R_foot = R + offset_dir * W/2
150+
151+
# X, Y position of foot
152+
X = R_foot * math.sin(theta)
153+
Y = (R - R_foot*math.cos(theta))
154+
155+
# Calculate orientation quaternion
156+
Q = quaternion_from_euler(0, 0, theta)
157+
pose = Pose()
158+
pose.position.x = self.robot_position.x + X
159+
pose.position.y = self.robot_position.y + Y
160+
161+
# The z position is observed for static walking, but the foot
162+
# will be placed onto the ground if the ground is lower than z
163+
pose.position.z = 0
164+
165+
pose.orientation.x = Q[0]
166+
pose.orientation.y = Q[1]
167+
pose.orientation.z = Q[2]
168+
pose.orientation.w = Q[3]
169+
170+
return pose
171+
172+
if __name__ == '__main__':
173+
rospy.init_node('walking_tutorial')
174+
walk = AtlasWalk()
175+
if len(sys.argv) > 0:
176+
walk.is_static = (sys.argv[-1] == "static")
177+
else:
178+
walk.is_static = False
179+
walk.walk()

‎drcsim_atlas_siminterface/tutorial.md

+115
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,115 @@
1+
# Introduction
2+
3+
This tutorial describes how to use the Atlas Sim Interface to command Atlas to walk dynamically or step statically.
4+
5+
## Setup
6+
7+
We assume that you've already done the [DRCSim installation step](http://gazebosim.org/tutorials?tut=drcsim_install).
8+
9+
If you haven't done so, make sure to source the environment setup.sh files with every new terminal you open:
10+
11+
~~~
12+
source /usr/share/drcsim/setup.sh
13+
~~~
14+
15+
To save on typing, you can add this script to your **.bashrc** files, so it's automatically sourced every time you start a new terminal.
16+
17+
~~~
18+
echo 'source /usr/share/drcsim/setup.sh' >> ~/.bashrc
19+
source ~/.bashrc
20+
~~~
21+
22+
But remember to remove them from your **.bashrc** file when they are not needed any more.
23+
24+
### Create a ROS Package Workspace
25+
26+
If you haven't already, create a ros directory in your home directory and add it to your $ROS\_PACKAGE\_PATH. From the command line
27+
28+
~~~
29+
mkdir ~/ros
30+
export ROS_PACKAGE_PATH=${HOME}/ros:${ROS_PACKAGE_PATH}
31+
~~~
32+
33+
Use [roscreate-pkg]( http://ros.org/wiki/roscreate) to create a ROS package for this tutorial, depending on **rospy** and **atlas_msgs**:
34+
35+
~~~
36+
cd ~/ros
37+
roscreate-pkg atlas_sim_interface_tutorial rospy atlas_msgs
38+
~~~
39+
40+
### Create a ROS Node
41+
Download [`walk.py`](http://bitbucket.org/osrf/gazebo_tutorials/raw/default/drcsim_atlas_interface/files/walk.py) into **`~/ros/atlas_sim_interface_tutorial/scripts/walk.py`**. This file contains the following code:
42+
43+
<include src='http://bitbucket.org/osrf/gazebo_tutorials/raw/default/drcsim_atlas_interface/files/walk.py' />
44+
45+
## The code explained
46+
47+
This node needs the following imports.
48+
49+
<include from='@#! /usr/bin/env python@' to='@import sys@' src='http://bitbucket.org/osrf/gazebo_tutorials/raw/default/drcsim_atlas_interface/files/walk.py' />
50+
51+
Initializing the publishers and subscribers for this node. We publish to `/atlas/atlas_sim_interface_command` and `/atlas/mode` and listen to `/atlas/atlas_sim_interface_state` and `/atlas/state`.
52+
53+
<include from='@class AtlasWalk():@' to='@print("Shutting down")@' src='http://bitbucket.org/osrf/gazebo_tutorials/raw/default/drcsim_atlas_interface/files/walk.py' />
54+
55+
This is the `atlas_sim_interface_state` callback. It provides a lot of useful information. We can get the robot's current position (as estimated by the BDI controller). This position is what is needed to transform a local step coordinate to a global step coordinate.
56+
57+
<include from='/36/' to='/45/' src='http://bitbucket.org/osrf/gazebo_tutorials/raw/default/drcsim_atlas_interface/files/walk.py' />
58+
59+
There are two types of walking behavior, static and dynamic. Dynamic is much faster, but foot placement is not as precise. Also, it is much easier to give bad walking commands that cause the atlas robot to fall. Static, is stable throughout the entire step trajectory.
60+
61+
<include from='@.*# /atlas/atlas_sim_interface_state callback.*@' to='@.*self.dynamic(state).*' src='http://bitbucket.org/osrf/gazebo_tutorials/raw/default/drcsim_atlas_interface/files/walk.py' />
62+
63+
If the robot is rotated to the world frame, the orientation may need to be accounted for in positioning the steps. This is how you can do that. However, this node does not make use of orientation.
64+
65+
<include from='@.*# /atlas/atlas_state callback.*@' to='@.*roll, pitch, yaw =.*' src='http://bitbucket.org/osrf/gazebo_tutorials/raw/default/drcsim_atlas_interface/files/walk.py' />
66+
67+
This function walks the robot dynamically in a circle. It is necessary to publish 4 steps at any time, starting with the `next_step_index_needed`. This helps the walking controller plan for a stable walking trajectory. Some message fields aren't used or implemented in this walking behavior. Dynamic walking behavior is best for flat surfaces with no obstructions.
68+
69+
<include from='@.*An example of commanding a dynamic walk behavior.*' to='@.*# End of dynamic walk behavior.*' src='http://bitbucket.org/osrf/gazebo_tutorials/raw/default/drcsim_atlas_interface/files/walk.py' />
70+
71+
This is an example of static walking/step behavior. You only specify one step at a time, and you have to check the step_feedback field in the state message to determine when you can send the next step command. It is statically stable throughout the entire step trajectory. If you need to step over objects, or step onto steps this behavior is necessary.
72+
73+
<include from='@.*# An example of commanding a static walk/step behavior.*@' to='@.*# End of static walk behavior.*@' src='http://bitbucket.org/osrf/gazebo_tutorials/raw/default/drcsim_atlas_interface/files/walk.py' />
74+
75+
This method calculates the pose of a step around a circle, based on the current step_index
76+
77+
<include from='@.*# This method is used to calculate a pose of step based on the step_index.*' to='@.*return pose.*@' src='http://bitbucket.org/osrf/gazebo_tutorials/raw/default/drcsim_atlas_interface/files/walk.py' />
78+
79+
Main method to run walk. It checks if static is specified or not.
80+
81+
<include from="@if __name__ == '__main__':@" to='$' src='http://bitbucket.org/osrf/gazebo_tutorials/raw/default/drcsim_atlas_interface/files/walk.py' />
82+
83+
# Running
84+
85+
Ensure that the above python file is executable
86+
87+
~~~
88+
chmod +x ~/ros/atlas_sim_interface_tutorial/scripts/walk.py
89+
~~~
90+
91+
Start up simulation
92+
93+
~~~
94+
roslaunch drcsim_gazebo atlas_sandia_hands.launch
95+
~~~
96+
97+
Rosrun the executable, specifying static if desired
98+
99+
Dynamic
100+
101+
~~~
102+
rosrun atlas_sim_interface_tutorial walk.py
103+
~~~
104+
105+
Static
106+
107+
~~~
108+
rosrun atlas_sim_interface_tutorial walk.py static
109+
~~~
110+
111+
## What you should see
112+
113+
Atlas should begin walking in a circle. Swiftly if it is dynamic behavior, or slowly if static behavior like the image below.
114+
115+
[[file:files/Asi_walk.png|800px]]

‎drcsim_control_sync/files/640px-Sync_exhaust_stats.png

105 KB
Loading

‎drcsim_control_sync/files/Atlas_sync.png

94.5 KB
Loading

‎drcsim_control_sync/files/Sync_stats.png

79.7 KB
Loading

‎drcsim_control_sync/files/atlas_sync.launch

+16
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,16 @@
1+
<launch>
2+
<!-- Control Synchronization Parameters -->
3+
<!-- delay_window_size: paging window that will allow delay_max_per_window-seconds of delay. -->
4+
<!-- delay_max_per_window: total cumulative delay in seconds allotted per delay_window_size. -->
5+
<!-- delay_max_per_step: maximum delay per simulation time step. -->
6+
<param name="/atlas/delay_window_size" type="double" value="25.0"/>
7+
<param name="/atlas/delay_max_per_window" type="double" value="1.0"/>
8+
<param name="/atlas/delay_max_per_step" type="double" value="0.1"/>
9+
10+
<arg name="gzname" default="gazebo"/>
11+
<!-- default launch file for starting an Atlas robot -->
12+
<include file="$(find drcsim_gazebo)/launch/atlas.launch">
13+
<arg name="gzname" value="$(arg gzname)"/>
14+
<arg name="gzworld" value="atlas.world"/>
15+
</include>
16+
</launch>

0 commit comments

Comments
 (0)