Merge remote-tracking branch 'origin/master' into simulation_scenario…

…/external_force

app/hyperparameters.py

@@ -20,6 +20,7 @@
 CONTROL_OPTIMIZATION_ITERATION = 8
 CONTROL_OPTIMIZATION_ITERATION_TENDON = 20
 TENDON_CONST = -5
+TENDON_DISCRETE_FORCES = [10, 15, 20]
 
 TIME_STEP_SIMULATION = 0.00001
 GRASP_TIME = 5

app/mcts_run_setup.py

@@ -2,21 +2,22 @@
 
 import hyperparameters as hp
 
-from rostok.block_builder_chrono.block_builder_chrono_api import \
-    ChronoBlockCreatorInterface as creator
-from rostok.criterion.criterion_calculation import (ForceCriterion, InstantContactingLinkCriterion,
-                                                    InstantForceCriterion,
-                                                    InstantObjectCOGCriterion, SimulationReward,
-                                                    TimeCriterion, FinalPositionCriterion,
-                                                    GraspTimeCriterion)
-from rostok.criterion.simulation_flags import (EventContact, EventContactTimeOut, EventFlyingApart,
-                                               EventGrasp, EventSlipOut, EventStopExternalForce)
+from rostok.control_chrono.tendon_controller import 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.simulation_chrono.simulation_scenario import GraspScenario
+from rostok.trajectory_optimizer.control_optimizer import (
+    CalculatorWithOptimizationDirect, TendonOptimizerCombinationForce)
+from rostok.utils.numeric_utils import Offset
 from rostok.trajectory_optimizer.control_optimizer import (CalculatorWithGraphOptimization,
-                                                           CalculatorWithOptimizationDirect,
-                                                           LinearCableControlOptimization,
-                                                           TendonLikeControlOptimization,
-                                                           LinearControlOptimizationDirect)
+                                                           CalculatorWithOptimizationDirect)
 
 
 def config_independent_torque(grasp_object_blueprint):
@@ -67,3 +68,65 @@ def config_independent_torque(grasp_object_blueprint):
                                                          hp.CONTROL_OPTIMIZATION_BOUNDS,
                                                          hp.CONTROL_OPTIMIZATION_ITERATION)
     return control_optimizer
+
+
+def config_with_tendon(grasp_object_blueprint):
+    # configurate the simulation manager
+
+    simulation_manager = GraspScenario(hp.TIME_STEP_SIMULATION, hp.TIME_SIMULATION)
+    simulation_manager.grasp_object_callback = grasp_object_blueprint #lambda: creator.create_environment_body(
+        #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(grasp_limit_time=hp.GRASP_TIME,
+                             contact_event=event_contact,
+                             verbosity=0,
+                             simulation_stop=1)
+    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)
+
+    #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(ForceCriterion(event_timeout), hp.FORCE_CRITERION_WEIGHT)
+    simulation_rewarder.add_criterion(InstantContactingLinkCriterion(event_grasp),
+                                      hp.INSTANT_CONTACTING_LINK_CRITERION_WEIGHT)
+    simulation_rewarder.add_criterion(InstantForceCriterion(event_grasp),
+                                      hp.INSTANT_FORCE_CRITERION_WEIGHT)
+    simulation_rewarder.add_criterion(InstantObjectCOGCriterion(event_grasp),
+                                      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),
+                                      hp.GRASP_TIME_CRITERION_WEIGHT)
+    simulation_rewarder.add_criterion(
+        FinalPositionCriterion(hp.REFERENCE_DISTANCE, event_grasp, event_slipout),
+        hp.FINAL_POSITION_CRITERION_WEIGHT)
+
+    data = TendonControllerParameters()
+    data.amount_pulley_in_body = 2
+    data.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)]
+    }
+    data.starting_point_parameters = [Offset(-1., True), Offset(5 , True), Offset(0, True)]
+    data.tip_parameters = [Offset(-0.3, True), Offset(-0.005, False, True), Offset(0, True)]
+
+    control_optimizer = TendonOptimizerCombinationForce(simulation_scenario = simulation_manager, rewarder = simulation_rewarder,
+                                            data = data,
+                                            starting_finger_angles=-45,
+                                            tendon_forces = hp.TENDON_DISCRETE_FORCES)
+
+    return control_optimizer

app/handle_one_graph.py → app/one_graph_simulation.py

@@ -1,13 +1,8 @@
-
-import hyperparameters as hp
-import matplotlib.pyplot as plt
-
-import numpy as np
-from mcts_run_setup import config_independent_torque
+from mcts_run_setup import config_independent_torque, config_with_tendon
 
 from rostok.library.obj_grasp.objects import get_object_sphere
-
-from rostok.library.rule_sets.simple_designs import get_three_link_one_finger_independent
+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
@@ -16,12 +11,14 @@
 # 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
 
 graph = get_three_link_one_finger_independent()
-control = [0.01, 0.01, 0.01]
+graph = get_three_link_one_finger()
+control = [10]
 
 data = control_optimizer.optim_parameters2data_control(control, graph)
 

rostok/block_builder_api/easy_body_shapes.py

@@ -1,4 +1,4 @@
-from dataclasses import dataclass
+from dataclasses import dataclass, field
 from pathlib import Path
 from typing import Union
 
@@ -34,6 +34,13 @@ class Ellipsoid:
 class FromMesh:
     path: Path
 
+@dataclass
+class ConvexHull:
+    points: list[tuple[float, float, float]] = field(default_factory=list)
+
+    def __hash__(self) -> int:    
+        return hash(("ConvexHull", self.points))
+
 
 # All types of shape
-ShapeTypes = Union[Box, Cylinder, Sphere, Ellipsoid, FromMesh]
+ShapeTypes = Union[Box, Cylinder, Sphere, Ellipsoid, FromMesh, ConvexHull]

rostok/block_builder_chrono/block_comprehension.py

@@ -0,0 +1,9 @@
+from rostok.block_builder_chrono.block_builder_chrono_api import \
+    ChronoBlockCreatorInterface
+
+
+def calc_volume_body(blueprint):
+    """Calculate the volume of the body."""
+    body = ChronoBlockCreatorInterface.init_block_from_blueprint(blueprint)
+    volume = body.body.GetMass() / body.body.GetDensity()
+    return volume

rostok/block_builder_chrono/blocks_utils.py

@@ -1,12 +1,11 @@
-from collections import namedtuple
 from enum import Enum
+from math import exp
 
 import numpy as np
 import pychrono.core as chrono
 
+from rostok.block_builder_api.block_parameters import FrameTransform
 from rostok.block_builder_chrono.block_types import BlockBody
-from rostok.block_builder_api.block_parameters import  FrameTransform
-
 
 
 def frame_transform_to_chcoordsys(transform: FrameTransform):
@@ -73,8 +72,12 @@ def evaluate(self, time, rest_angle, angle, vel, link):
             torque: torque, that is created by spring
         """
         torque = 0
-        if self.spring_coef > 10**-3:
-            torque = -self.spring_coef * (angle - rest_angle) - self.damping_coef * vel
+
+        if self.spring_coef > 10**-5:
+            if angle <= 0:
+                torque = -self.spring_coef * (angle - rest_angle) - self.damping_coef * vel
+            else:
+                torque = -self.damping_coef * vel - (exp((angle) * 20) - 1)
         else:
             torque = -self.damping_coef * vel
         return torque

rostok/control_chrono/control_utils.py

@@ -0,0 +1,18 @@
+from dataclasses import dataclass, field
+
+from rostok.control_chrono.external_force import ForceControllerTemplate
+
+
+@dataclass
+class ForceTorqueContainer:
+    controller_list: list[ForceControllerTemplate] = field(default_factory=list)
+
+    def update_all(self, time: float, data=None):
+        for i in self.controller_list:
+            i.update(time, data)
+
+    def add(self, controller: ForceControllerTemplate):
+        if controller.is_bound:
+            self.controller_list.append(controller)
+        else:
+            raise Exception("Force controller should be bound to body, before use")

rostok/control_chrono/controller.py

@@ -1,16 +1,14 @@
-from math import sin
-from typing import Any, Dict, List, Tuple
 from abc import abstractmethod
-from dataclasses import dataclass, field
-from matplotlib.pyplot import cla
+from math import sin
+from typing import Dict, List
 
 import pychrono.core as chrono
-from typing import Callable, List
-from rostok.block_builder_chrono.block_classes import (ChronoRevolveJoint, JointInputTypeChrono)
 
-from rostok.virtual_experiment.sensors import Sensor
+from rostok.block_builder_chrono.block_classes import (ChronoRevolveJoint,
+                                                       JointInputTypeChrono)
 from rostok.virtual_experiment.built_graph_chrono import BuiltGraphChrono
 
+
 class RobotControllerChrono:
     """General controller. Any controller should be subclass of this class.
     
@@ -21,7 +19,7 @@ class RobotControllerChrono:
             functions: list of functions currently attached to joints
     """
 
-    def __init__(self, built_graph: BuiltGraphChrono, parameters: Dict[str, Any]):
+    def __init__(self, built_graph: BuiltGraphChrono, parameters):
         """Initialize class fields and call the initialize_functions() to set starting state"""
         self.built_graph = built_graph
         self.graph = built_graph.graph
@@ -83,119 +81,3 @@ def update_functions(self, time, robot_data, environment_data):
             current_const = self.parameters['sin_parameters'][i][2] * time * self.parameters[
                 'sin_parameters'][i][0] * sin(self.parameters['sin_parameters'][i][1] * time)
             func.Set_yconst(current_const)
-
-
-@dataclass
-class ForceTorque:
-    """Forces and torques are given in the xyz convention
-    """
-    force: tuple[float, float, float] = (0, 0, 0)
-    torque: tuple[float, float, float] = (0, 0, 0)
-
-
-class ForceControllerTemplate():
-    """Base class for creating force and moment actions. 
-    To use it, you need to implement the get_force_torque method, 
-    which determines the force and torque at time t. Vectors are 
-    specified in global coordinate system.
-    """
-
-    def __init__(self) -> None:
-
-        self.x_force_chrono = chrono.ChFunction_Const(0)
-        self.y_force_chrono = chrono.ChFunction_Const(0)
-        self.z_force_chrono = chrono.ChFunction_Const(0)
-
-        self.x_torque_chrono = chrono.ChFunction_Const(0)
-        self.y_torque_chrono = chrono.ChFunction_Const(0)
-        self.z_torque_chrono = chrono.ChFunction_Const(0)
-
-        self.force_vector_chrono = [self.x_force_chrono, self.y_force_chrono, self.z_force_chrono]
-        self.torque_vector_chrono = [
-            self.x_torque_chrono, self.y_torque_chrono, self.z_torque_chrono
-        ]
-        self.force_maker_chrono = chrono.ChForce()
-        self.torque_maker_chrono = chrono.ChForce()
-        self.is_binded = False
-        self.setup_makers()
-
-    @abstractmethod
-    def calculate_impact(self, time, *args) -> tuple[float, float, float]:
-        pass
-
-
-    def get_force_torque(self, time: float, data, *args) -> ForceTorque:
-        impact = ForceTorque()
-        impact.force = self.calculate_impact(time, *args)
-        return impact
-
-    def update(self, time: float, data=None):
-        force_torque = self.get_force_torque(time, data)
-        for val, functor in zip(force_torque.force + force_torque.torque,
-                                self.force_vector_chrono + self.torque_vector_chrono):
-            functor.Set_yconst(val)
-
-    def setup_makers(self):
-        self.force_maker_chrono.SetMode(chrono.ChForce.FORCE)
-        self.force_maker_chrono.SetAlign(chrono.ChForce.WORLD_DIR)
-        self.torque_maker_chrono.SetMode(chrono.ChForce.TORQUE)
-        self.torque_maker_chrono.SetAlign(chrono.ChForce.WORLD_DIR)
-
-        self.force_maker_chrono.SetF_x(self.x_force_chrono)
-        self.force_maker_chrono.SetF_y(self.y_force_chrono)
-        self.force_maker_chrono.SetF_z(self.z_force_chrono)
-
-        self.torque_maker_chrono.SetF_x(self.x_torque_chrono)
-        self.torque_maker_chrono.SetF_y(self.y_torque_chrono)
-        self.torque_maker_chrono.SetF_z(self.z_torque_chrono)
-
-    def bind_body(self, body: chrono.ChBody):
-        body.AddForce(self.force_maker_chrono)
-        body.AddForce(self.torque_maker_chrono)
-        self.is_binded = True
-
-
-CALLBACK_TYPE = Callable[[float, Any], ForceTorque]
-
-
-class ForceControllerOnCallback(ForceControllerTemplate):
-
-    def __init__(self, callback: CALLBACK_TYPE) -> None:
-        super().__init__()
-        self.callback = callback
-
-    def get_force_torque(self, time: float, data) -> ForceTorque:
-        return self.callback(time, data)
-
-
-class YaxisShaker(ForceControllerTemplate):
-
-    def __init__(self, amp: float = 5, amp_offset: float = 1, freq: float = 5, start_time: float = 0.0) -> None:
-        super().__init__()
-        self.amp = amp
-        self.amp_offset = amp_offset
-        self.freq = freq
-        self.start_time = start_time
-
-    def get_force_torque(self, time: float, data) -> ForceTorque:
-        impact = ForceTorque()
-        y_force = 0
-        if time >= self.start_time:
-            y_force = self.amp * sin(self.freq * (time - self.start_time)) + self.amp_offset
-        impact.force = (0, y_force, 0)
-        return impact
-
-
-@dataclass
-class ForceTorqueContainer:
-    controller_list: list[ForceControllerTemplate] = field(default_factory=list)
-
-    def update_all(self, time: float, data=None):
-        for i in self.controller_list:
-            i.update(time, data)
-
-    def add(self, controller: ForceControllerTemplate):
-        if controller.is_binded:
-            self.controller_list.append(controller)
-        else:
-            raise Exception("Force controller should bind to body, before use")