# Tutorial 4: Joint control, table, balls, etc.

## Tutorial's demo

This demo is started similarly to the Tutorials 1 to 3, with the exception that pam_mujoco should
be start with mujoco_id *tutorial_4*

```bash
pam_mujoco tutorial_4
```

then

```python
python ./tutorial_4.py
```

## Handle and MuJoCo XML files

The source code of Tutorial 4 starts with the MuJoCo simulation configuration:

```python
robot = pam_mujoco.MujocoRobot("robot",
                               control=pam_mujoco.MujocoRobot.JOINT_CONTROL)
ball1 = pam_mujoco.MujocoItem("ball1",
                              control=pam_mujoco.MujocoItem.CONSTANT_CONTROL,
                              color=(1,0,0,1))
ball2 = pam_mujoco.MujocoItem("ball2",
                              control=pam_mujoco.MujocoItem.CONSTANT_CONTROL,
                              color=(0,0,1,1),
                              contact_type=pam_mujoco.ContactTypes.table)
hit_point = pam_mujoco.MujocoItem("hit_point",
                                  control=pam_mujoco.MujocoItem.CONSTANT_CONTROL)
graphics=True
accelerated_time=False
handle = pam_mujoco.MujocoHandle("tutorial_4",
                                 table=True,
                                 robot1=robot,
                                 balls=(ball1,ball2),
                                 hit_points=(hit_point,),
                                 graphics=graphics,
                                 accelerated_time=accelerated_time)

```

The code above requests *pam_mujoco* (via the shared memory) to start a simulation with a joint controlled robot along with 2 balls and a hit point (a graphical market).

Once the (waiting) instance of *pam_mujoco* reads (from the shared memory) this request, the first action it does it to create a corresponding mujoco xml configuration file in the *tmp* folder. In this specific case, the model file is created in:

```bash
/tmp/tutorial_4/tutorial_4.xml
```

The handle requests the configuration file to describe an environment with 1 robot, 2 balls, a hit point and to display a table and indeed this is what pam_mujoco spawns.

On top of this, the handle requests the robot to be joint controlled as well as the balls and the hit point to be also be controlled.

### Model factory

The API of model factory is:

```python
import pam_mujoco
items = pam_mujoco.model_factory(model_name, # arbitrary string
                                 time_step = 0.002,
                                 table=False,nb_balls=1,
                                 robot1=False,robot1_position=[0.1,0.0,-0.44],
                                 robot2=False,robot2_position=[1.6,3.4,-0.44],
                                 robot2_orientation = [-1,0,0,0,-1,0],
                                 goal=False,hit_point=False,
                                 ball_colors=None,
                                 muscles=True)
```

Call to model_factory results in the generation of an MuJoCo-XML-model-file. The absolute path to this file is accessible:

```python
path = items["path"]
```

You may all model_factory with arguments to add ball(s), a goal mark, a hit_point mark, a table or a second robot (goal and hit_point are simple visual markers). You may also request the robot not to have any muscle actuation.

For example, the following script will start the simulation of 2 robots, a table, 3 balls and a goal:

```python
import pam_mujoco

model_name = "tutorial_model"
mujoco_id = "tutorial_mujoco_id"

# generation of a mujoco model xml file
# corresponding to a table, 3 balls, 2 robots
# and a goal (a goal is a simple visual marker)
items = pam_mujoco.model_factory(model_name,
                                 table=True,nb_balls=3,
                                 robot1=True,robot2=True,
                                 goal=True)

# some config for mujoco
config = pam_mujoco.MujocoConfig()
config.graphics = True # set to False if you do not need graphics
config.realtime = True # set to False if you prefer accelerated time

# starting the mujoco simulation
pam_mujoco.init_mujoco(config)
pam_mujoco.execute(mujoco_id,items["path"])
```

The simulation will run forever (or until the corresponding terminal is closed). It is possible to stop it by calling (in another script):

```python
import pam_mujoco
# we are using the same mujoco id as the one used to start the simulation
mujoco_id = "tutorial_mujoco_id"
pam_mujoco.request_stop(mujoco_id)
```

## Interacting with objects 

### Interacting with a ball

The script above started a simulation containing 3 simulated balls. These balls are subject to the (simulated) gravity and will just fall forever. Not very exciting.

o80 can be used to interact the simulated balls, e.g. forcing the position/velocity of the balls and/or getting information about the balls.

To request the addition of a corresponding o80 backend to the simulation process, the script has to be updated:

 ```python

# the mujoco xml model generated including 3 balls
items = pam_mujoco.model_factory(model_name,
                                 table=True,nb_balls=3,
                                 robot1=True,robot2=True,
                                 goal=True)

# getting informations about the 3 generated balls
balls = items["ball"]

config = pam_mujoco.MujocoConfig()
config.graphics = True
config.realtime = True

pam_mujoco.init_mujoco(config)

# adding a o80 backend to send command 
# (and receiving information) to (from) the
# first ball
pam_mujoco.add_o80_ball_control("ball0",balls[0])

# starting the mujoco simulation. The o80 backend
# will listen to commands and write info about the ball
pam_mujoco.execute(mujoco_id,items["path"])
```

The method "add_o80_for_ball" reads "add an o80 backend to the MuJoCo simulation". Consequently, the ball is no longer subject to gravity but gets fully controlled based on user commands: one interact with the first ball via o80 frontend instantiated in another script (or another process in the same script):

```python
import pam_mujoco
#  note: "ball0": argument passed to "add_o80_for_ball" in the previous script
frontend_ball = o80_pam.BallFrontEnd("ball0") 

# getting the information about the ball
ball = frontend_ball.latest()
position = ball.get_position()
velocity = ball.get_velocity()

# forcing position and velocity to the ball:
target_position = [1,1,1]
target_velocity = [0,0,0]
frontend_ball = add_command(target_position,target_velocity,o80.Mode.QUEUE)
frontend_ball.pulse()
```

The script above will move the ball to [1,1,1]

Something similar can be done for the other balls, or the goal (replace "Ball" with "Goal", e.g. add_o80_for_goal).

### Detecting contact

As presented above, this allows control of a simulated ball:

```python
pam_mujoco.add_o80_ball_control("ball0",
                                balls[0])
```

This ball may enter in contact with the table and/or the racket of the robot(s). It is possible to retrieve related information.

First, in the same way that a o80 backend was added to the MuJoCo simulation via the method "add_o80_ball_control". Contact controllers have to be added to the MuJoCo simulation:

```python
ball = items["ball"][0]
table = items["table"]
robot = items["robot"]
# adding controller for monitoring contact between
# the racket of the robot and the ball
# ("contact_robot_ball" is an arbitrary id for this contact)
pam_mujoco.add_robot1_contact("contact_robot_ball",ball,robot)
# same, but for contact with the table
pam_mujoco.add_table_contact("contact_table_ball",ball,table)
```
Once the simulation started, corresponding information can be requested in another script via:

```python
import pam_mujoco
# note the contact id is the same as above
contact = pam_mujoco.get_contact("contact_table_ball") 
```
"contact" is the instance of a class providing the public attributes:

```python
# True if a contact ever occurred
contact.contact_occurred
# position (x y z) of the first contact (if any)
contact.position
# time stamp (int, nanoseconds) of the first contact (if any)
contact.time_stamp
# minimal distance between ball and table,
# useful in case no contact ever occurred
contact.minimal_distance
```
A limitation is that information about only the first contact is stored (i.e. not the history of contacts).

You may reset the contact information in order to start monitoring new contact:

```python
pam_mujoco.reset_contact("contact_table_ball")
```

### Playing a recorded ball trajectory

We recorded trajectories of real ball being launched and bouncing on a table tennis table. A Python-API allows to query these trajectories.
For example:

```python
import context
index,trajectory_points = list(context.BallTrajectories().random_trajectory())
```
This returns an index and a list of trajectory points.

The index allows to retrieve the name of the file in which the returned trajectory has been stored:

```python
file_name = context.BallTrajectories().get_file_name(index)
```

You may find the file in the folder /opt/mpi-is/context/trajectories/.

```trajectory_points``` is a list of trajectory points providing a position and velocity attributes:

```python
position = trajectory_points[0].position # x,y,z
velocity = trajectory_points[0].velocity # x,y,z
``` 
You may also retrieve a trajectory via an index:

```python
trajectory_points = list(context.BallTrajectories().get_trajectory(index))
```

To know which index correspond to which file:

```python
# prints list of files and related indexes.
# you may call this in a python terminal
import context
context.BallTrajectories().print_index_files()
```

You may use an o80 ball frontend to replay a ball trajectory in mujoco, for example:

```python
frontend_ball = o80_pam.BallFrontEnd("ball0")
_,trajectory_points = list(context.BallTrajectories().random_trajectory())
duration_s = 0.01
duration = o80.Duration_us.milliseconds(int(duration_s*1000))
total_duration = duration_s * len(trajectory_points)
for traj_point in trajectory_points:
    frontend_ball.add_command(traj_point,position,traj_point.velocity,
                               duration,o80.Mode.QUEUE)
frontend_ball.pulse()
```


### Hybrid: Controlling a ball until contact

It is possible to use an o80 frontend to control a ball, but letting MuJoCo's physic engine taking over once a contact occured.

Above, the function "add_o80_ball_control" was introduced. It was used to set an o80 backend for ball control in the MuJoCo simulation.

An alternative method is: "add_o80_ball_control_until_contact".

For example:

```python
ball = items["ball"][0]
table = items["table"]
pam_mujoco.add_table_contact("table",ball,table)
pam_mujoco.add_o80_ball_control_until_contact("ball","table",ball)
```
The above will also add a o80 backend for ball control to the mujoco simulation. Similarly as when "add_o80_ball_control" was used (and with the exact same API), it is possible to control the ball's position and velocity. But as soon as a contact is detected between the table and the ball, o80's command will be ignored, and the ball with be from that time managed by Mujoco's physic engine. 

Once the contact is reset:

```
pam_mujoco.reset_contact("table")
```

## Goal, hit point

Goal and HitPoint are visual markers. They can also be added to the MuJoCo's model, for example:

```python
items = pam_mujoco.model_factory(model_name,
                                 table=True,
                                 nb_balls=1,
                                 robot1=True,
                                 goal=True,
                                 hit_point=True)
goal = items["goal"]
hit_point = items["hit_point"]

# to allow frontend to control the goal                                                                                                                                                                                                                                                   
pam_mujoco.add_o80_goal_control("goal",goal)

# to allow frontend to control the hit point                                                                                                                                                                                                                                              
pam_mujoco.add_o80_hit_point_control("hit_point",hit_point)
```

They can be controlled similarly to balls, for example:

```python
hit_point = o80_pam.HitPointFrontEnd("hit_point")
goal = o80_pam.GoalFrontEnd("goal")

position = (1,1,1)
velocity = (0,0,0)
goal.add_command(position,velocity,o80.Mode.QUEUE)
goal.pulse()
```
 
## Joint Control

MuJoCo simulated PAM-robot can be joint controlled (i.e. setting for each degree of freedom a desired joint angle and a desired angular velocity). 

For this, it is required to add a joint controller, for example:

```python
items = pam_mujoco.model_factory(model_name,
                                 table=True,
                                 nb_balls=2,
                                 robot1=True,
                                 goal=True,
                                 hit_point=True,
                                 ball_colors=((1,0,0,1),(0,0,1,1)))
robot = items["robot"]
pam_mujoco.add_o80_joint_control("robot",robot)
```
This allows to send robot control command via an o80 frontend, for example:

```python
robot = o80_pam.JointFrontEnd("robot")
joints = (math.pi/4.0,-math.pi/4.0,
          math.pi/4.0,-math.pi/4.0)
joint_velocities = (0,0,0,0)
duration = o80.Duration_us.seconds(2)
robot.add_command(joints,joint_velocities,duration,o80.Mode.QUEUE)
robot.pulse()
```
The frontend can also be used to query the robot state:

```
observation = robot.latest()
observation.get_positions()
observation.get_velocities()
```