Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

add event builders #131

Merged
merged 6 commits into from
Nov 2, 2023
Merged

add event builders #131

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
808f14d
add event builders
MikhailChaikovskii Nov 1, 2023
e283233
123
ZharkovKirill Nov 1, 2023
64c4bb4
fix return
MikhailChaikovskii Nov 1, 2023
0382ba3
piymav na bug
ZharkovKirill Nov 1, 2023
86b10a2
fix activate
MikhailChaikovskii Nov 1, 2023
110c895
simulation with external force
MikhailChaikovskii Nov 1, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion app/hyperparameters.py
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Loading