Skip to content

Commit

Permalink
Merge pull request #131 from aimclub/feature/independent_events
Browse files Browse the repository at this point in the history 
add event builders
  • Loading branch information
@ZharkovKirill
ZharkovKirill committed Nov 2, 2023
2 parents dd10ce8 + 110c895 commit 383811f
Show file tree
Hide file tree
Showing 8 changed files with 311 additions and 109 deletions.
2 changes: 1 addition & 1 deletion app/hyperparameters.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,7 @@
TENDON_CONST = -5
TENDON_DISCRETE_FORCES = [10, 15, 20]

TIME_STEP_SIMULATION = 0.00001
TIME_STEP_SIMULATION = 0.001
GRASP_TIME = 5
FORCE_TEST_TIME = 5
TIME_SIMULATION = 10
Expand Down
163 changes: 121 additions & 42 deletions app/mcts_run_setup.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,73 +3,150 @@
import hyperparameters as hp

from rostok.control_chrono.tendon_controller import TendonController_2p, TendonControllerParameters
from rostok.criterion.criterion_calculation import (
FinalPositionCriterion, GraspTimeCriterion, InstantContactingLinkCriterion,
InstantForceCriterion, InstantObjectCOGCriterion, SimulationReward,
TimeCriterion)
from rostok.criterion.simulation_flags import (EventContact,
EventContactTimeOut,
EventFlyingApart, EventGrasp,
EventSlipOut,
EventStopExternalForce)
from rostok.criterion.criterion_calculation import (FinalPositionCriterion, GraspTimeCriterion,
InstantContactingLinkCriterion,
InstantForceCriterion,
InstantObjectCOGCriterion, SimulationReward,
TimeCriterion)
from rostok.criterion.simulation_flags import (EventContactBuilder, EventContactTimeOutBuilder,
EventFlyingApartBuilder, EventGraspBuilder,
EventSlipOutBuilder, EventStopExternalForceBuilder)
from rostok.simulation_chrono.simulation_scenario import GraspScenario
from rostok.trajectory_optimizer.control_optimizer import (
CalculatorWithOptimizationDirect, TendonOptimizerCombinationForce)
from rostok.trajectory_optimizer.control_optimizer import BruteForceOptimisation1D, ConstTorqueOptiVar, GlobalOptimisationEachSim, TendonForceOptiVar
from rostok.utils.numeric_utils import Offset
from rostok.trajectory_optimizer.control_optimizer import (CalculatorWithGraphOptimization,
CalculatorWithOptimizationDirect)
import rostok.control_chrono.external_force as f_ext


def get_tendon_cfg():
tendon_controller_cfg = TendonControllerParameters()
tendon_controller_cfg.amount_pulley_in_body = 2
tendon_controller_cfg.pulley_parameters_for_body = {
0: [Offset(-0.14, True), Offset(0.005, False, True),
Offset(0, True)],
1: [Offset(-0.14, True), Offset(-0.005, False, True),
Offset(0, True)]
}
tendon_controller_cfg.starting_point_parameters = [
Offset(-0.02, False), Offset(0.025, False),
Offset(0, True)
]
tendon_controller_cfg.tip_parameters = [
Offset(-0.3, True), Offset(-0.005, False, True),
Offset(0, True)
]
return tendon_controller_cfg


def config_independent_torque(grasp_object_blueprint):
# configurate the simulation manager

simulation_manager = GraspScenario(hp.TIME_STEP_SIMULATION, hp.TIME_SIMULATION)
# simulation_manager.grasp_object_callback = lambda: creator.create_environment_body(
# grasp_object_blueprint)

simulation_manager.grasp_object_callback = grasp_object_blueprint
event_contact = EventContact()
simulation_manager.add_event(event_contact)
event_timeout = EventContactTimeOut(hp.FLAG_TIME_NO_CONTACT, event_contact)
simulation_manager.add_event(event_timeout)
event_flying_apart = EventFlyingApart(hp.FLAG_FLYING_APART)
simulation_manager.add_event(event_flying_apart)
event_slipout = EventSlipOut(hp.FLAG_TIME_SLIPOUT)
simulation_manager.add_event(event_slipout)
event_grasp = EventGrasp(
event_contact_builder = EventContactBuilder()
simulation_manager.add_event_builder(event_contact_builder)
event_timeout_builder = EventContactTimeOutBuilder(hp.FLAG_TIME_NO_CONTACT,
event_contact_builder)
simulation_manager.add_event_builder(event_timeout_builder)
event_flying_apart_builder = EventFlyingApartBuilder(hp.FLAG_FLYING_APART)
simulation_manager.add_event_builder(event_flying_apart_builder)
event_slipout_builder = EventSlipOutBuilder(hp.FLAG_TIME_SLIPOUT)
simulation_manager.add_event_builder(event_slipout_builder)
event_grasp_builder = EventGraspBuilder(
grasp_limit_time=hp.GRASP_TIME,
contact_event=event_contact,
event_contact_builder=event_contact_builder,
verbosity=0,
)
simulation_manager.add_event(event_grasp)
event_stop_external_force = EventStopExternalForce(grasp_event=event_grasp,
force_test_time=hp.FORCE_TEST_TIME)
simulation_manager.add_event(event_stop_external_force)
simulation_manager.add_event_builder(event_grasp_builder)
event_stop_external_force = EventStopExternalForceBuilder(
event_grasp_builder=event_grasp_builder, force_test_time=hp.FORCE_TEST_TIME)
simulation_manager.add_event_builder(event_stop_external_force)

#create criterion manager
simulation_rewarder = SimulationReward(verbosity=0)
#create criterions and add them to manager
simulation_rewarder.add_criterion(TimeCriterion(hp.GRASP_TIME, event_timeout, event_grasp),
hp.TIME_CRITERION_WEIGHT)
simulation_rewarder.add_criterion(
TimeCriterion(hp.GRASP_TIME, event_timeout_builder, event_grasp_builder),
hp.TIME_CRITERION_WEIGHT)
#simulation_rewarder.add_criterion(ForceCriterion(event_timeout), hp.FORCE_CRITERION_WEIGHT)
simulation_rewarder.add_criterion(InstantContactingLinkCriterion(event_grasp),
simulation_rewarder.add_criterion(InstantContactingLinkCriterion(event_grasp_builder),
hp.INSTANT_CONTACTING_LINK_CRITERION_WEIGHT)
simulation_rewarder.add_criterion(InstantForceCriterion(event_grasp),
simulation_rewarder.add_criterion(InstantForceCriterion(event_grasp_builder),
hp.INSTANT_FORCE_CRITERION_WEIGHT)
simulation_rewarder.add_criterion(InstantObjectCOGCriterion(event_grasp),
simulation_rewarder.add_criterion(InstantObjectCOGCriterion(event_grasp_builder),
hp.OBJECT_COG_CRITERION_WEIGHT)
n_steps = int(hp.GRASP_TIME / hp.TIME_STEP_SIMULATION)
print(n_steps)
simulation_rewarder.add_criterion(GraspTimeCriterion(event_grasp, n_steps),
simulation_rewarder.add_criterion(GraspTimeCriterion(event_grasp_builder, n_steps),
hp.GRASP_TIME_CRITERION_WEIGHT)
simulation_rewarder.add_criterion(
FinalPositionCriterion(hp.REFERENCE_DISTANCE, event_grasp, event_slipout),
FinalPositionCriterion(hp.REFERENCE_DISTANCE, event_grasp_builder, event_slipout_builder),
hp.FINAL_POSITION_CRITERION_WEIGHT)

control_optimizer = CalculatorWithOptimizationDirect(simulation_manager, simulation_rewarder,
hp.CONTROL_OPTIMIZATION_BOUNDS,
hp.CONTROL_OPTIMIZATION_ITERATION)
return control_optimizer
const_optivar = ConstTorqueOptiVar(simulation_rewarder, -45)
direct_args = {"maxiter": 2}
global_const = GlobalOptimisationEachSim([simulation_manager], const_optivar, (0, 10),
direct_args)

return global_const


def config_tendon(grasp_object_blueprint):
obj_forces = []
obj_forces.append(f_ext.RandomForces(1e6, 100, 20))
obj_forces.append(f_ext.NullGravity(0))
obj_forces = f_ext.ExternalForces(obj_forces)
simulation_manager = GraspScenario(hp.TIME_STEP_SIMULATION, hp.TIME_SIMULATION, TendonController_2p, obj_external_forces=obj_forces)

simulation_manager.grasp_object_callback = grasp_object_blueprint
event_contact_builder = EventContactBuilder()
simulation_manager.add_event_builder(event_contact_builder)
event_timeout_builder = EventContactTimeOutBuilder(hp.FLAG_TIME_NO_CONTACT,
event_contact_builder)
simulation_manager.add_event_builder(event_timeout_builder)
event_flying_apart_builder = EventFlyingApartBuilder(hp.FLAG_FLYING_APART)
simulation_manager.add_event_builder(event_flying_apart_builder)
event_slipout_builder = EventSlipOutBuilder(hp.FLAG_TIME_SLIPOUT)
simulation_manager.add_event_builder(event_slipout_builder)
event_grasp_builder = EventGraspBuilder(
grasp_limit_time=hp.GRASP_TIME,
event_contact_builder=event_contact_builder,
verbosity=0,
)
simulation_manager.add_event_builder(event_grasp_builder)
event_stop_external_force = EventStopExternalForceBuilder(
event_grasp_builder=event_grasp_builder, force_test_time=hp.FORCE_TEST_TIME)
simulation_manager.add_event_builder(event_stop_external_force)

#create criterion manager
simulation_rewarder = SimulationReward(verbosity=0)
#create criterions and add them to manager
simulation_rewarder.add_criterion(
TimeCriterion(hp.GRASP_TIME, event_timeout_builder, event_grasp_builder),
hp.TIME_CRITERION_WEIGHT)
#simulation_rewarder.add_criterion(ForceCriterion(event_timeout), hp.FORCE_CRITERION_WEIGHT)
simulation_rewarder.add_criterion(InstantContactingLinkCriterion(event_grasp_builder),
hp.INSTANT_CONTACTING_LINK_CRITERION_WEIGHT)
simulation_rewarder.add_criterion(InstantForceCriterion(event_grasp_builder),
hp.INSTANT_FORCE_CRITERION_WEIGHT)
simulation_rewarder.add_criterion(InstantObjectCOGCriterion(event_grasp_builder),
hp.OBJECT_COG_CRITERION_WEIGHT)
n_steps = int(hp.GRASP_TIME / hp.TIME_STEP_SIMULATION)
print(n_steps)
simulation_rewarder.add_criterion(GraspTimeCriterion(event_grasp_builder, n_steps),
hp.GRASP_TIME_CRITERION_WEIGHT)
simulation_rewarder.add_criterion(
FinalPositionCriterion(hp.REFERENCE_DISTANCE, event_grasp_builder, event_slipout_builder),
hp.FINAL_POSITION_CRITERION_WEIGHT)
tendon_controller_cfg = get_tendon_cfg()
tendon_optivar = TendonForceOptiVar(tendon_controller_cfg, simulation_rewarder, -45)
tendon_optivar.is_vis = True
brute_tendon = BruteForceOptimisation1D(hp.TENDON_DISCRETE_FORCES, [simulation_manager],
tendon_optivar,
num_cpu_workers=1)

return brute_tendon

"""
def config_with_tendon(grasp_object_blueprint):
# configurate the simulation manager
Expand Down Expand Up @@ -203,4 +280,6 @@ def config_combination_force_tendon_multiobject_parallel(grasp_object_blueprint,
control_optimizer = ParralelOptimizerCombinationForce(simulation_managers,
simulation_rewarder, data, hp.TENDON_DISCRETE_FORCES, starting_finger_angles=-25)
return control_optimizer
return control_optimizer
"""
28 changes: 8 additions & 20 deletions app/one_graph_simulation.py
View file
Original file line number Diff line number Diff line change
@@ -1,31 +1,19 @@
from mcts_run_setup import config_independent_torque, config_with_tendon
from mcts_run_setup import config_tendon

from rostok.library.obj_grasp.objects import get_object_sphere
from rostok.library.rule_sets.simple_designs import (
get_three_link_one_finger, get_three_link_one_finger_independent)
from rostok.simulation_chrono.simulation_utils import SimulationResult
get_three_link_one_finger, get_three_link_one_finger_independent, get_two_link_three_finger, )

# create blueprint for object to grasp
grasp_object_blueprint = get_object_sphere(0.05)
grasp_object_blueprint = get_object_sphere(0.05, mass=0.2)

# create reward counter using run setup function
# control_optimizer = config_with_const_troques(grasp_object_blueprint)
control_optimizer = config_independent_torque(grasp_object_blueprint)
control_optimizer = config_with_tendon(grasp_object_blueprint)

simulation_rewarder = control_optimizer.rewarder
simulation_manager = control_optimizer.simulation_scenario

control_optimizer = config_tendon(grasp_object_blueprint)

graph = get_three_link_one_finger_independent()
graph = get_three_link_one_finger()
control = [10]

data = control_optimizer.optim_parameters2data_control(control, graph)

vis = True

#simulation_output: SimulationResult = simulation_manager.run_simulation(graph, data, [[-45.0, 0.0],[-45,0],[-45,0]], vis, True)
simulation_output: SimulationResult = simulation_manager.run_simulation(graph, data, [[0.0, 0, 0]], vis, True)
graph = graph = get_two_link_three_finger()

res = simulation_rewarder.calculate_reward(simulation_output)
print('reward', res)
rewsss = control_optimizer.calculate_reward(graph)
pass
Loading

0 comments on commit 383811f

Please sign in to comment.