Controlling a Toy Car - VigneshManoj/ambf GitHub Wiki

Controlling a Toy Car with Suspension

This is an example of how to begin using the AMBF Simulator. Let's start by going to the launch file in your local cloned repository. This launch file is located in ambf/ambf_models/descriptions/launch.

# This is the base file for Coordination Application
world config: ./world/world.yaml
color config: ./color/colors.yaml
input devices config: ./input_devices/input_devices.yaml
multibody configs:
  - "./multi-bodies/robots/blender-toy-car.yaml" #0
  - "./multi-bodies/robots/blender-toy-car2.yaml" #1
  - "./multi-bodies/robots/blender-neuro-robot.yaml" #2
  - "./multi-bodies/robots/blender-raven2.yaml" #3
  - "./multi-bodies/robots/blender-mtm.yaml" #4
  - "./multi-bodies/robots/blender-psm.yaml" #5
  - "./multi-bodies/robots/blender-ecm.yaml" #6
  - "./multi-bodies/robots/blender-suj.yaml" #7
  - "./multi-bodies/robots/blender-kuka.yaml" #8
  - "./multi-bodies/robots/blender-pr2.yaml" #9
  - "./multi-bodies/puzzles/parallel_structure.yaml" #10
  - "./multi-bodies/puzzles/puzzle1.yaml" #11
  - "./multi-bodies/puzzles/puzzle2.yaml" #12

We can see that the Toy Car(s) are defined at Index 0 (Simple) and 1 (With Suspension).

Now let's launch the simulator as follows:

cd ~/ambf/bin/<os>
./ambf_simulator -l 1

Do you see the toy-car as shown below? If yes, great, otherwise close the simulator and double check to see that the indexes match the toy-car in your ambf/ambf_models/descriptions/launch file.

Now we need one more modification. Open the ambf/ambf_models/descriptions/multi-bodies/robots/blender-toy-car2.yaml in your favorite text editor. We need to add some additional launch variables which would tell the simulator to publish children and joint names of non-root bodies. By default, the Simulator publishes the joint names and joint positions of root links and not the links that occur down the hierarchy. This can be easily changed in the AMBF description file of a Robot/Multibody.

Let's scroll down to the definition of Body ShockFL

BODY ShockFL:
  name: ShockFL
  mesh: ShockFL.STL
  mass: 0.3
  collision margin: 0.001
  scale: 1.0
  location:
    orientation: {p: -1.772, r: 0.452, y: -0.444}
    position: {x: 0.126, y: -0.206, z: 0.035}
  inertial offset:
    orientation: {p: 0, r: 0, y: 0}
    position: {x: -0.0, y: -0.0, z: 0.0}
  friction: {rolling: 0.01, static: 0.5}
  damping: {angular: 0.1, linear: 0.04}
  restitution: 0
  collision groups: [0]
  color components:
    ambient: {level: 1.0}
    diffuse: {b: 0.1845, g: 0.1845, r: 0.1845}
    specular: {b: 1.0, g: 1.0, r: 1.0}
    transparency: 1.0

We need to add three lines anywhere inside the body definition. The three lines that we want to add are as follows:

  publish children names: true
  publish joint names: true
  publish joint positions: true

Your resulting BODY ShockFL block should look like this:

BODY ShockFL:
  name: ShockFL
  mesh: ShockFL.STL
  mass: 0.3
  collision margin: 0.001
  scale: 1.0
  publish children names: true
  publish joint names: true
  publish joint positions: true
  location:
    orientation: {p: -1.772, r: 0.452, y: -0.444}
    position: {x: 0.126, y: -0.206, z: 0.035}
  inertial offset:
    orientation: {p: 0, r: 0, y: 0}
    position: {x: -0.0, y: -0.0, z: 0.0}
  friction: {rolling: 0.01, static: 0.5}
  damping: {angular: 0.1, linear: 0.04}
  restitution: 0
  collision groups: [0]
  color components:
    ambient: {level: 1.0}
    diffuse: {b: 0.1845, g: 0.1845, r: 0.1845}
    specular: {b: 1.0, g: 1.0, r: 1.0}
    transparency: 1.0

Let's do the exact same thing in the BODY ShockFR block such that it should end up looking like this:

BODY ShockFR:
  name: ShockFR
  mesh: ShockFR.STL
  mass: 0.3
  collision margin: 0.001
  scale: 1.0
  publish children names: true
  publish joint names: true
  publish joint positions: true
  location:
    orientation: {p: -1.772, r: -0.452, y: 0.444}
    position: {x: 0.126, y: 0.185, z: 0.035}
  inertial offset:
    orientation: {p: 0, r: 0, y: 0}
    position: {x: -0.0, y: -0.0, z: 0.0}
  friction: {rolling: 0.01, static: 0.5}
  damping: {angular: 0.1, linear: 0.04}
  restitution: 0
  collision groups: [0]
  color components:
    ambient: {level: 1.0}
    diffuse: {b: 0.1473, g: 0.1473, r: 0.1473}
    specular: {b: 1.0, g: 1.0, r: 1.0}
    transparency: 1.0

Be mindful of the white spaces as YAML is similar to Python in picking whitespace and tabs.

Now let's launch the simulator as described in Launching the Simulator. Do you see a toy car as shown below? If yes, great, otherwise close the simulator and double check to see if you have any unintended whitespaces or extra tab spaces in the ambf/ambf_models/descriptions/launch file above. Correct these and relaunch the simulator

Controlling using the Python Client

Now let's use the provided python client for finer control of the Toy Car. you can use the following python code snippet:

# Import the Client from ambf_client package
from ambf_client import Client
import time

# Create a instance of the client
_client = Client()

# Connect the client which in turn creates callable objects from ROS topics
# and initiates a shared pool of threads for bi-directional communication
_client.connect()

print('\n\n----')
raw_input("We can see what objects the client has found. Press Enter to continue...")
# You can print the names of objects found. We should see all the links found
print(_client.get_obj_names())

# Lets get a handle the chassis of the toy car
chassis_handle = _client.get_obj_handle('Chassis')

# Let's get a handle to the two front suspensions which have the wheels
# as their children
shock_fr_handle = _client.get_obj_handle('ShockFR')
shock_fl_handle = _client.get_obj_handle('ShockFL')

# Let's sleep for a very brief moment to give the internal callbacks
# to sync up new data from the running simulator
time.sleep(0.2)

print('\n\n----')
raw_input("Let's Get Some Pose Info. Press Enter to continue...")
# Not we can print the pos and rotation of object in the World Frame
print('Chassis Pos:')
print(chassis_handle.get_pos())
print('Chassis Rot:')
print(chassis_handle.get_rot())


print('\n\n----')
raw_input("Let's get Joints and Children Info. Press Enter to continue...")
# We can get the number of children and joints connected to each object as
chassis_num_joints = chassis_handle.get_num_joints() # Get the number of joints of this object
chassis_joint_names = chassis_handle.get_joint_names() # Get a list of children names belonging to this obj
chassis_children_names = chassis_handle.get_children_names() # Get a list of children names belonging to this obj
print('Number of Joints in Chassis:')
print(chassis_num_joints)

print(' ')
print('Joint names for Chassis:')
print(chassis_joint_names)

print(' ')
print('Name of Chassis\' children:')
print(chassis_children_names)

print('\n\n----')
raw_input("Control Wheel Angles. Press Enter to continue...")
# In this example, rather than controlling the joints from Chassis, lets control
# them from lower down the hierarchy. In this case, we shall use the FR Shock and FL
# Shock which parent the FR Wheel and FL Wheel respectively.

# Set the position of FL wheel
shock_fl_handle.set_joint_pos(0, 1.57)

# Set the Position of FR Wheel
shock_fr_handle.set_joint_pos(0, -1.57)

print('\n\n----')
raw_input("Control Wheel Torques. Press Enter to continue...")
# Now lets directly control the wheel torques.
shock_fl_handle.set_joint_effort(0, 0.6)
# Set some torque for FR Wheel
shock_fr_handle.set_joint_effort(0, 0.9)
# Lets sleep for a few seconds to see the effect
time.sleep(5)

shock_fl_handle.set_joint_effort(0, 0.0)
# Set some torque for FR Wheel
shock_fr_handle.set_joint_effort(0, 0.0)

print('\n\n----')
raw_input("Set force on Chassis for 5 secs. Press Enter to continue...")
# Let's directly control the forces and torques on the Chassis Link
# Notice that these are in the world frame. Another important thing to notice
# is that unlike position control, forces control requires a continuous update
# to meet a watchdog timing condition otherwise the forces are cleared. This
# is purely for safety reasons to prevent unchecked forces in case of malfunctioning
# python client code
for i in range(0, 500):
    chassis_handle.set_force(100, 0, 0)  # Set 100 N in the World Z axis
    time.sleep(0.01)  # Run the loop for 5 seconds

print('\n\n----')
raw_input("Set torque on Chassis for 2 secs. Press Enter to continue...")
# Similarly we can individually apply the torque
for i in range(0, 200):
    chassis_handle.set_torque(0, 0, 60)  # Set 100 Nm in the World Z axis
    time.sleep(0.01)  # Run the loop for 2 seconds

print('\n\n----')
raw_input("Set force on Chassis in Body Frame 5 secs. Press Enter to continue...")
# The above force and torques were in the World frame. In many instances we care
# about setting the wrench in body frame. This is just as convenient. Lets see how to
# do that.
#
# Make sure you have PyKDL installed
from PyKDL import Rotation, Vector

for i in range(0, 500):
    cur_rot = chassis_handle.get_rot()
    # Rotation of Chassis in World
    R_cINw = Rotation.Quaternion(cur_rot.x, cur_rot.y, cur_rot.z, cur_rot.w)
    fw = Vector(15, 0, 0) # Lets set a force along body's X axis
    fc = R_cINw * fw  # Converted force in World Frame that would act in Body Frame
    chassis_handle.set_force(fc.x(), fc.y(), fc.z())  # Set 100 N in the World Z axis
    time.sleep(0.01)  # Run the loop for 5 seconds

print('\n\n----')
raw_input("Let's clean up. Press Enter to continue...")
# Lastly to cleanup
_client.clean_up()