Skip to content

Commit

Permalink
123
Browse files Browse the repository at this point in the history
  • Loading branch information
@ZharkovKirill
ZharkovKirill committed Nov 1, 2023
1 parent 808f14d commit e283233
Show file tree
Hide file tree
Showing 3 changed files with 107 additions and 44 deletions.
128 changes: 102 additions & 26 deletions app/mcts_run_setup.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,53 +3,70 @@
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 (EventContactBuilder,
EventContactTimeOutBuilder,
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)
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_builder = EventContactBuilder()
simulation_manager.add_event_builder(event_contact_builder)
event_timeout_builder = EventContactTimeOutBuilder(hp.FLAG_TIME_NO_CONTACT, 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_builder=event_contact_builder,
event_contact_builder=event_contact_builder,
verbosity=0,
)
simulation_manager.add_event_builder(event_grasp_builder)
event_stop_external_force = EventStopExternalForceBuilder(grasp_event_builder=event_grasp_builder,
force_test_time=hp.FORCE_TEST_TIME)
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(
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)
Expand All @@ -65,11 +82,68 @@ def config_independent_torque(grasp_object_blueprint):
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):

simulation_manager = GraspScenario(hp.TIME_STEP_SIMULATION, hp.TIME_SIMULATION, TendonController_2p)

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 +277,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
"""
21 changes: 4 additions & 17 deletions app/one_graph_simulation.py
View file
Original file line number Diff line number Diff line change
@@ -1,31 +1,18 @@
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

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

# 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)

res = simulation_rewarder.calculate_reward(simulation_output)
print('reward', res)
control_optimizer.calculate_reward(graph)
2 changes: 1 addition & 1 deletion rostok/criterion/simulation_flags.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -465,7 +465,7 @@ def event_check(self, current_time: float, step_n: int, robot_data, env_data):
return EventCommands.CONTINUE


class EventStopExternalForceBuilder():
class EventStopExternalForceBuilder(EventBuilder):
def __init__(self, force_test_time: float, event_grasp_builder: EventGraspBuilder):
super().__init__(event_class=EventStopExternalForce)
self.event_grasp_builder = event_grasp_builder
Expand Down

0 comments on commit e283233

Please sign in to comment.