In [ ]:
import random

import pychrono as chrono
from example_vocabulary import get_terminal_graph_three_finger, PALM_LIST, RM_MOUNTS, LM_MOUNTS, J_NODES, B_NODES

import rostok.virtual_experiment.simulation_step as step
from rostok.block_builder.basic_node_block import SimpleBody
from rostok.block_builder.node_render import (ChronoBodyEnv, DefaultChronoMaterial, FrameTransform)
from rostok.criterion.criterion_calc import criterion_calc
from rostok.criterion.flags_simualtions import FlagMaxTime
from rostok.graph_grammar.node import BlockWrapper
from rostok.graph_grammar.nodes_division import nodes_division, sort_left_right
from rostok.trajectory_optimizer.control_optimizer import num_joints
from rostok.trajectory_optimizer.trajectory_generator import create_torque_traj_from_x

In this tutorial we will use the created before graph to start the simulations of the corresponding mechanism with random control and get its reward for trying to grasp an object.

At first, lets set the parameters of simulation and get the graph

In [ ]:
# Constans
MAX_TIME = 1
TIME_STEP = 1e-3

graph = get_terminal_graph_three_finger()

Second step - set the random trajectories for joints using rostok.trajectory_optimizer.trajectory_generator module. One must get the number of joints using num_joints function from rostok.trajectory_optimizer.control_optimizer module and get a random number for each joint. Pass the obtained values to the create_torque_traj_from_x function together with graph and simulation parameters to get trajectories.

In [ ]:
# Create trajectory
number_trq = num_joints(graph)
const_torque_koef = [random.random() for _ in range(number_trq)]
arr_trj = create_torque_traj_from_x(graph, const_torque_koef, MAX_TIME, TIME_STEP)

Lets set the object that our robot will try to grasp. For the object we need

  • Material
  • Friction coefficient
  • Dumping factor
  • Shape
  • Starting position and rotation.

The parameters of the material one should set using DefaultChronoMaterial from rostok.block_builder.node_render (see documentation). Shape and position are parameters of the ChronoBodyEnv class.

In [ ]:
# Create object to grasp
matich = DefaultChronoMaterial()
matich.Friction = 0.65
matich.DampingF = 0.65
obj = BlockWrapper(ChronoBodyEnv,
                   shape=SimpleBody.BOX,
                   material=matich,
                   pos=FrameTransform([0, 1, 0], [0, -0.048, 0.706, 0.706]))

A few more parameters of the simulation. Set gravity acceleration and use flags to set MAX_TIME of the simulation. Here we use the simulation without gravity.

In [ ]:
# Configurate simulation
config_sys = {"Set_G_acc": chrono.ChVectorD(0, 0, 0)}
flags = [FlagMaxTime(MAX_TIME)]

Actual simulation is performed by the instance of the SimulationStepOptimization class, one only need to set parameters and pass the graph of the mechanism and the object to grasp. In this tutorial we use only one flag and one parameter of the system, but there are other options (see documentation)

In [ ]:
sim = step.SimulationStepOptimization(arr_trj, graph, obj)
sim.set_flags_stop_simulation(flags)
sim.change_config_system(config_sys)

# Start simulation
sim_output = sim.simulate_system(TIME_STEP, True)

At this point the simulation is finished and the sim_output contains data that we can use to calculate the reward.
Calculation of reward is complicated process and requires additional parameters.

In [ ]:
WEIGHTS = [5, 1, 1, 5]
GAIT_PERIOD = 2.5

J_NODES_NEW = nodes_division(sim.grab_robot, J_NODES)
B_NODES_NEW = nodes_division(sim.grab_robot, B_NODES)
RB_NODES_NEW = sort_left_right(sim.grab_robot, RM_MOUNTS, B_NODES)
LB_NODES_NEW = sort_left_right(sim.grab_robot, LM_MOUNTS, B_NODES)
RJ_NODES_NEW = sort_left_right(sim.grab_robot, RM_MOUNTS, J_NODES)
LJ_NODES_NEW = sort_left_right(sim.grab_robot, LM_MOUNTS, J_NODES)

reward = criterion_calc(sim_output, B_NODES_NEW, J_NODES_NEW, LB_NODES_NEW, RB_NODES_NEW, WEIGHTS,
                        GAIT_PERIOD)
print(round(reward,4))
-2.6699

The final result of the simulation is a scalar reward that is based on the categories described here
We only obtained the reward for the random control. In the next tutorial we will use rewards to obtain the optimal trajectories for joints and get the corresponding maximum reward for the mechanism.