Precariously balanced rock gazebo simulation
Zhiang Chen, zch@asu.edu
export GAZEBO_MODEL_PATH=~/catkin_ws/src/pbr_gazebo/models${GAZEBO_MODEL_PATH:+:${GAZEBO_MODEL_PATH}}$
roslaunch pbr_gazebo prismatic_box.launch
rosrun pbr_gazebo pulse_motion_server.py
rosrun pbr_gazebo pulse_motion_client.py 1 2
The pulse motion is deployed by a cosine function: .
There are two arguments required for pulse_motion_client.py. The first one is the amplitude of displacement, A; the second one is the fruquence of displacement, F.
To reset shaking table displacement,
rosrun pbr_gazebo pulse_motion_client.py 0 0
rqt
is recommended to plot the motion. There are three important topics:
/prismatic_box_controller/prismatic_joint_controller/command/data
/prismatic_box_controller/joint_states/velocity[0]
/prismatic_box_controller/joint_states/position[0]
To tune the PID parameters, prismatic_box_controller.yaml
roslaunch pbr_gazebo granite_dell.launch
roslaunch pbr_gazebo prismatic_controller.launch
rosrun pbr_gazebo pulse_motion_server.py
rosrun pbr_gazebo pulse_motion_client.py 1 2
You might need to wait until granite_dell
is fully launched in gazebo to launch the prismatic_controller
.
To tune the PID parameters, prismatic_controller_granite_dell.yaml
roslaunch pbr_gazebo double_rock.launch
rosrun pbr_gazebo pulse_motion_server.py
rosrun pbr_gazebo pulse_motion_client.py 1 2
roslaunch pbr_gazebo prismatic_large_box.launch
roslaunch pbr_gazebo add_double_rock_pbr_2.launch
rosrun pbr_gazebo pulse_motion_server.py
You need to modify the yaw in the robot model pbr_gazebo/models/double_rock/model.urdf
. In tag <link name='box'>
, change yaw in radiance as desired orientation.
For example, if the yaw is modified to 1.0471975511965976 (60 degress), run the following commands to start shake experiments.
roslaunch pbr_gazebo double_rock.launch
rosrun pbr_gazebo pulse_motion_server.py
rosrun pbr_gazebo pulse_motion_smart_client_double_rock_on_site.py --yaw 60
roslaunch pbr_gazebo prismatic_large_box.launch
rosrun pbr_gazebo pulse_motion_server.py
rosrun pbr_gazebo pulse_motion_smart_client_double_rock.py --yaw 90
pulse_motion_smart_client_double_rock.py
has one argument, the initial yaw (degrees) of PBR.
PBR and box models can be loaded after the environment has been loaded in Gazebo
roslaunch pbr_gazebo add_pbr.launch
roslaunch pbr_gazebo add_box.launch
There are box models with different dimensions. They can be specified in add_box.launch
file to be loaded.
The pulse motion is deployed by a cosine function: . It is conducted by a ros action server, pulse_motion_server.py. To send the motion commands, two action clients are supported.
- pulse_motion_client.py: Single pulse motion. Two arguments are required for pulse_motion_client.py. The first one is the amplitude of displacement, A; the second one is the fruquence of displacement, F. e.g.
rosrun pbr_gazebo pulse_motion_client.py 1 2
. - pulse_motion_smart_client.py: Repeated pulse-motion experiments. It needs the lists of displacement amplitudes and frequences defined in the python file. Then it will repeat the experiment of loading PBR, triggering single pulse motion, deleting PBR, and resetting shake table. The following video shows that the experiments are automatically repeated in the simulation.
roslaunch pbr_gazebo prismatic_box.launch
rosrun pbr_gazebo pulse_motion_server.py
rosrun pbr_gazebo pulse_motion_smart_client.py
The lists of displacement amplitudes and frequences need to be defined in pulse_motion_smart_client.py.
roslaunch pbr_gazebo double_rock.launch
roslaunch pbr_gazebo add_double_rock_pbr.launch
rosrun pbr_gazebo motion_from_file.py
rostopic pub /ground_motion_server/goal pbr_gazebo/AFActionGoal "header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: ''
goal_id:
stamp:
secs: 0
nsecs: 0
id: ''
goal:
A: 1.0
F: 1.0"
When AF = 0. the shake table position will be reset. Otherwise (AF !=0), it will trigger ground motion from a .csv file where desired velocities are defined.
apt-get update && apt-get upgrade
apt-get install ros-kinetic-ros-control
apt-get install ros-kinetic-gazebo-ros-pkgs ros-kinetic-gazebo-ros-control
apt-get install ros-kinetic-joint-state-controller
apt-get install ros-kinetic-effort-controllers
apt-get install ros-kinetic-position-controllers
More interesting videos can be found on our Youtube playlist