diff --git a/giskardpy/executor.py b/giskardpy/executor.py index e6c26a91a..1c85a46fd 100644 --- a/giskardpy/executor.py +++ b/giskardpy/executor.py @@ -15,7 +15,7 @@ AuxiliaryVariableManager, AuxiliaryVariable, ) -from giskardpy.motion_statechart.context import BuildContext +from giskardpy.motion_statechart.context import BuildContext, ExecutionContext from giskardpy.motion_statechart.motion_statechart import MotionStatechart from giskardpy.qp.exceptions import EmptyProblemException from giskardpy.qp.qp_controller import QPController @@ -120,19 +120,30 @@ def build_context(self) -> BuildContext: control_cycle_variable=self._control_cycles_variable, ) + @property + def execution_context(self) -> ExecutionContext: + return ExecutionContext( + world=self.world, + external_collision_data_data=self.collision_scene.get_external_collision_data(), + self_collision_data_data=self.collision_scene.get_self_collision_data(), + auxiliar_variables_data=self.auxiliary_variable_manager.resolve_auxiliary_variables(), + control_cycle_counter=self.control_cycles, + ) + def tick(self): self.control_cycles += 1 self.collision_scene.sync() self.collision_scene.check_collisions() - self.motion_statechart.tick(self.build_context) + execution_context = self.execution_context + self.motion_statechart.tick(execution_context) if self.qp_controller is None: return next_cmd = self.qp_controller.get_cmd( world_state=self.world.state.data, life_cycle_state=self.motion_statechart.life_cycle_state.data, - external_collisions=self.collision_scene.get_external_collision_data(), - self_collisions=self.collision_scene.get_self_collision_data(), - auxiliary_variables=self.auxiliary_variable_manager.resolve_auxiliary_variables(), + external_collisions=execution_context.external_collision_data_data, + self_collisions=execution_context.self_collision_data_data, + auxiliary_variables=execution_context.auxiliar_variables_data, ) self.world.apply_control_commands( next_cmd, @@ -162,7 +173,7 @@ def _set_velocity_acceleration_jerk_to_zero(self): def _compile_qp_controller(self, controller_config: QPControllerConfig): ordered_dofs = sorted( self.world.active_degrees_of_freedom, - key=lambda dof: self.world.state._index[dof.name], + key=lambda dof: self.world.state._index[dof.id], ) constraint_collection = ( self.motion_statechart.combine_constraint_collections_of_nodes() diff --git a/giskardpy/model/better_pybullet_syncer.py b/giskardpy/model/better_pybullet_syncer.py index 3f250dd88..e2cb4d1ac 100644 --- a/giskardpy/model/better_pybullet_syncer.py +++ b/giskardpy/model/better_pybullet_syncer.py @@ -98,7 +98,7 @@ def sync_world_model(self) -> None: self.objects_in_order = [] for body in sorted( - self._world.bodies_with_enabled_collision, key=lambda b: b.name + self._world.bodies_with_enabled_collision, key=lambda b: b.id ): self.add_object(body) self.objects_in_order.append(self.body_to_bpb_obj[body]) diff --git a/giskardpy/model/bpb_wrapper.py b/giskardpy/model/bpb_wrapper.py index 4bee891ae..a7de5e3ce 100644 --- a/giskardpy/model/bpb_wrapper.py +++ b/giskardpy/model/bpb_wrapper.py @@ -1,6 +1,7 @@ import os import tempfile from typing import List, Tuple, Optional +from uuid import UUID import giskardpy_bullet_bindings as pb import trimesh @@ -27,8 +28,8 @@ def create_collision(pb_collision: pb.Collision, world: World) -> GiskardCollision: collision = GiskardCollision( - body_a=world.get_kinematic_structure_entity_by_name(pb_collision.obj_a.name), - body_b=world.get_kinematic_structure_entity_by_name(pb_collision.obj_b.name), + body_a=world.get_kinematic_structure_entity_by_id(pb_collision.obj_a.name), + body_b=world.get_kinematic_structure_entity_by_id(pb_collision.obj_b.name), contact_distance_input=pb_collision.contact_distance, map_P_pa=pb_collision.map_P_pa, map_P_pb=pb_collision.map_P_pb, @@ -123,7 +124,7 @@ def create_shape_from_link( shape = create_compound_shape(shapes_poses=shapes) # else: # shape = create_shape_from_geometry(link.collisions[0]) - return create_object(link.name, shape, pb.Transform.identity()) + return create_object(link.id, shape, pb.Transform.identity()) def create_compound_shape( @@ -189,7 +190,7 @@ def convert_to_decomposed_obj_and_save_in_tmp( def create_object( - name: PrefixedName, + name: UUID, shape: pb.CollisionShape, transform: Optional[pb.Transform] = None, ) -> pb.CollisionObject: diff --git a/giskardpy/model/collision_matrix_manager.py b/giskardpy/model/collision_matrix_manager.py index d983810bc..52ee40fc6 100644 --- a/giskardpy/model/collision_matrix_manager.py +++ b/giskardpy/model/collision_matrix_manager.py @@ -4,7 +4,7 @@ from enum import Enum from typing import List, Optional, Set, Dict, Any, Self -from krrood.adapters.json_serializer import SubclassJSONSerializer +from krrood.adapters.json_serializer import SubclassJSONSerializer, to_json, from_json from giskardpy.utils.utils import JsonSerializableEnum from semantic_digital_twin.adapters.world_entity_kwargs_tracker import ( @@ -40,24 +40,20 @@ def to_json(self) -> Dict[str, Any]: **super().to_json(), "type_": self.type_.to_json(), "distance": self.distance, - "body_group1_names": [body.name.to_json() for body in self.body_group1], - "body_group2_names": [body.name.to_json() for body in self.body_group2], + "body_group1_ids": [to_json(body.id) for body in self.body_group1], + "body_group2_ids": [to_json(body.id) for body in self.body_group2], } @classmethod def _from_json(cls, data: Dict[str, Any], **kwargs) -> Self: tracker = KinematicStructureEntityKwargsTracker.from_kwargs(kwargs) body_group1 = [ - tracker.get_kinematic_structure_entity( - PrefixedName.from_json(name, **kwargs) - ) - for name in data["body_group1_names"] + tracker.get_kinematic_structure_entity(from_json(id_)) + for id_ in data["body_group1_ids"] ] body_group2 = [ - tracker.get_kinematic_structure_entity( - PrefixedName.from_json(name, **kwargs) - ) - for name in data["body_group2_names"] + tracker.get_kinematic_structure_entity(from_json(id_)) + for id_ in data["body_group2_ids"] ] return cls( type_=CollisionAvoidanceTypes.from_json(data["type_"], **kwargs), diff --git a/giskardpy/model/collisions.py b/giskardpy/model/collisions.py index fa4d78d38..2b16de525 100644 --- a/giskardpy/model/collisions.py +++ b/giskardpy/model/collisions.py @@ -263,7 +263,7 @@ def transform_self_collision( new_link_a, new_link_b = world.compute_chain_reduced_to_controlled_connections( link_a, link_b ) - if new_link_a.name > new_link_b.name: + if new_link_a.id > new_link_b.id: collision = collision.reverse() new_link_a, new_link_b = new_link_b, new_link_a collision.body_a = new_link_a diff --git a/giskardpy/motion_statechart/auxilary_variable_manager.py b/giskardpy/motion_statechart/auxilary_variable_manager.py index c7f7af24a..8b40bf327 100644 --- a/giskardpy/motion_statechart/auxilary_variable_manager.py +++ b/giskardpy/motion_statechart/auxilary_variable_manager.py @@ -61,12 +61,17 @@ def create_vector3(name: PrefixedName, provider: Callable[[], List[float]]): @dataclass class AuxiliaryVariableManager: variables: List[AuxiliaryVariable] = field(default_factory=list) + data: np.ndarray = field(default_factory=lambda: np.array([], dtype=np.float64)) + + def add_variable(self, variable: AuxiliaryVariable): + self.variables.append(variable) + self.data = np.append(self.data, 0.0) def create_float_variable( self, name: PrefixedName, provider: Callable[[], float] = None ) -> AuxiliaryVariable: v = AuxiliaryVariable(name=name, provider=provider) - self.variables.append(v) + self.add_variable(v) return v def create_point3( @@ -81,7 +86,9 @@ def create_point3( z = AuxiliaryVariable( name=PrefixedName("z", str(name)), provider=lambda: provider()[2] ) - self.variables.extend([x, y, z]) + self.add_variable(x) + self.add_variable(y) + self.add_variable(z) return Point3(x, y, z) def create_transformation_matrix( @@ -94,9 +101,11 @@ def create_transformation_matrix( name=PrefixedName(f"t[{row},{column}]", str(name)), provider=lambda r=row, c=column: provider()[r, c], ) - self.variables.append(auxiliary_variable) + self.add_variable(auxiliary_variable) transformation_matrix[row, column] = auxiliary_variable return transformation_matrix def resolve_auxiliary_variables(self) -> np.ndarray: - return np.array([v.resolve() for v in self.variables]) + for i, v in enumerate(self.variables): + self.data[i] = v.resolve() + return self.data diff --git a/giskardpy/motion_statechart/context.py b/giskardpy/motion_statechart/context.py index 02083e994..a620117f1 100644 --- a/giskardpy/motion_statechart/context.py +++ b/giskardpy/motion_statechart/context.py @@ -1,5 +1,6 @@ from dataclasses import dataclass +import numpy as np from typing_extensions import Self from giskardpy.model.collision_world_syncer import CollisionWorldSynchronizer @@ -19,9 +20,6 @@ class BuildContext: qp_controller_config: QPControllerConfig control_cycle_variable: AuxiliaryVariable - def to_execution_context(self): - return ExecutionContext(world=self.world) - @classmethod def empty(cls) -> Self: return cls( @@ -36,3 +34,7 @@ def empty(cls) -> Self: @dataclass class ExecutionContext: world: World + external_collision_data_data: np.ndarray + self_collision_data_data: np.ndarray + auxiliar_variables_data: np.ndarray + control_cycle_counter: int diff --git a/giskardpy/motion_statechart/goals/collision_avoidance.py b/giskardpy/motion_statechart/goals/collision_avoidance.py index 043cbdb11..a1960dad0 100644 --- a/giskardpy/motion_statechart/goals/collision_avoidance.py +++ b/giskardpy/motion_statechart/goals/collision_avoidance.py @@ -515,7 +515,7 @@ def add_self_collision_avoidance_constraints(self, context: BuildContext): body_a_original, body_b_original ) ) - if body_b.name < body_a.name: + if body_b.id < body_a.id: body_a, body_b = body_b, body_a counter[body_a, body_b] = max( [ diff --git a/giskardpy/motion_statechart/motion_statechart.py b/giskardpy/motion_statechart/motion_statechart.py index 03ec25f62..639ab8ebd 100644 --- a/giskardpy/motion_statechart/motion_statechart.py +++ b/giskardpy/motion_statechart/motion_statechart.py @@ -7,6 +7,7 @@ import numpy as np import rustworkx as rx from krrood.adapters.json_serializer import SubclassJSONSerializer +from line_profiler.explicit_profiler import profile from typing_extensions import List, MutableMapping, ClassVar, Self, Type import semantic_digital_twin.spatial_types.spatial_types as cas @@ -177,12 +178,19 @@ def compile(self): parameters=[self.observation_symbols(), self.life_cycle_symbols()], sparse=False, ) + self._compiled_updater.bind_args_to_memory_view( + arg_idx=0, numpy_array=self.motion_statechart.observation_state.data + ) + self._compiled_updater.bind_args_to_memory_view( + arg_idx=1, numpy_array=self.data + ) def __getitem__(self, node: MotionStatechartNode) -> LifeCycleValues: return LifeCycleValues(super().__getitem__(node)) - def update_state(self, observation_state: np.ndarray): - self.data = self._compiled_updater(observation_state, self.data) + @profile + def update_state(self): + np.copyto(self.data, self._compiled_updater.evaluate()) def __str__(self) -> str: return str( @@ -228,24 +236,29 @@ def compile(self, context: BuildContext): ], sparse=False, ) - - def update_state( - self, - life_cycle_state: np.ndarray, - world_state: np.ndarray, - external_collision_data: np.ndarray, - self_collision_data: np.ndarray, - auxiliar_variables: np.ndarray, - ): - self.data = self._compiled_updater( - self.data, - life_cycle_state, - world_state, - external_collision_data, - self_collision_data, - auxiliar_variables, + self._compiled_updater.bind_args_to_memory_view( + arg_idx=0, numpy_array=self.data + ) + self._compiled_updater.bind_args_to_memory_view( + arg_idx=1, numpy_array=self.motion_statechart.life_cycle_state.data + ) + self._compiled_updater.bind_args_to_memory_view( + arg_idx=2, numpy_array=context.world.state.data + ) + self._compiled_updater.bind_args_to_memory_view( + arg_idx=3, numpy_array=context.collision_scene.external_collision_data + ) + self._compiled_updater.bind_args_to_memory_view( + arg_idx=4, numpy_array=context.collision_scene.self_collision_data + ) + self._compiled_updater.bind_args_to_memory_view( + arg_idx=5, numpy_array=context.auxiliary_variable_manager.data ) + @profile + def update_state(self): + np.copyto(self.data, self._compiled_updater.evaluate()) + @dataclass(repr=False, eq=False) class StateHistoryItem: @@ -520,25 +533,17 @@ def combine_constraint_collections_of_nodes(self) -> ConstraintCollection: ) return combined_constraint_collection - def _update_observation_state(self, context: BuildContext): - self.observation_state.update_state( - life_cycle_state=self.life_cycle_state.data, - world_state=context.world.state.data, - external_collision_data=context.collision_scene.get_external_collision_data(), - self_collision_data=context.collision_scene.get_self_collision_data(), - auxiliar_variables=context.auxiliary_variable_manager.resolve_auxiliary_variables(), - ) + def _update_observation_state(self, context: ExecutionContext): + self.observation_state.update_state() for node in self.nodes: if self.life_cycle_state[node] == LifeCycleValues.RUNNING: - observation_overwrite = node.on_tick( - context=context.to_execution_context() - ) + observation_overwrite = node.on_tick(context=context) if observation_overwrite is not None: self.observation_state[node] = observation_overwrite def _update_life_cycle_state(self, context: ExecutionContext): previous = self.life_cycle_state.data.copy() - self.life_cycle_state.update_state(self.observation_state.data) + self.life_cycle_state.update_state() self._trigger_life_cycle_callbacks( previous, self.life_cycle_state.data, context ) @@ -573,18 +578,18 @@ def _trigger_life_cycle_callbacks( case _: pass - def tick(self, context: BuildContext): + def tick(self, context: ExecutionContext): """ Executes a single tick of the motion statechart. First the observation state is updated, then the life cycle state is updated. :param context: The context required to execute the tick. """ self._update_observation_state(context) - self._update_life_cycle_state(context.to_execution_context()) + self._update_life_cycle_state(context) self._raise_if_cancel_motion() self.history.append( next_item=StateHistoryItem( - control_cycle=context.control_cycle_variable.evaluate(), + control_cycle=context.control_cycle_counter, life_cycle_state=self.life_cycle_state, observation_state=self.observation_state, ) diff --git a/test/conftest.py b/test/conftest.py index 3c998652d..0721a60b2 100644 --- a/test/conftest.py +++ b/test/conftest.py @@ -125,7 +125,7 @@ def from_world(cls, world: World) -> Self: root=world.get_body_by_name("bot"), _world=world, ) - world.add_semantic_annotation(boxbot, skip_duplicates=True) + world.add_semantic_annotation(boxbot) @pytest.fixture() diff --git a/test/test_motion_statechart/test_json_parsing.py b/test/test_motion_statechart/test_json_parsing.py index afe904e67..7f50d7acd 100644 --- a/test/test_motion_statechart/test_json_parsing.py +++ b/test/test_motion_statechart/test_json_parsing.py @@ -170,7 +170,7 @@ def test_executing_json_parsed_statechart(): ) world.add_degree_of_freedom(dof) root_C_tip = RevoluteConnection( - parent=root, child=tip, axis=Vector3.Z(), dof_name=dof.name + parent=root, child=tip, axis=Vector3.Z(), dof_id=dof.id ) world.add_connection(root_C_tip) @@ -179,7 +179,7 @@ def test_executing_json_parsed_statechart(): ) world.add_degree_of_freedom(dof) root_C_tip2 = RevoluteConnection( - parent=root, child=tip2, axis=Vector3.Z(), dof_name=dof.name + parent=root, child=tip2, axis=Vector3.Z(), dof_id=dof.id ) world.add_connection(root_C_tip2) diff --git a/test/test_motion_statechart/test_motion_statechart.py b/test/test_motion_statechart/test_motion_statechart.py index c0c187f91..0b8e488ce 100644 --- a/test/test_motion_statechart/test_motion_statechart.py +++ b/test/test_motion_statechart/test_motion_statechart.py @@ -1,10 +1,10 @@ import json +import time from dataclasses import dataclass from math import radians import numpy as np import pytest -import time import semantic_digital_twin.spatial_types.spatial_types as cas from giskardpy.executor import Executor @@ -421,7 +421,7 @@ def test_joint_goal(): ) world.add_degree_of_freedom(dof) root_C_tip = RevoluteConnection( - parent=root, child=tip, axis=cas.Vector3.Z(), dof_name=dof.name + parent=root, child=tip, axis=cas.Vector3.Z(), dof_id=dof.id ) world.add_connection(root_C_tip) @@ -430,7 +430,7 @@ def test_joint_goal(): ) world.add_degree_of_freedom(dof) root_C_tip2 = RevoluteConnection( - parent=root, child=tip2, axis=cas.Vector3.Z(), dof_name=dof.name + parent=root, child=tip2, axis=cas.Vector3.Z(), dof_id=dof.id ) world.add_connection(root_C_tip2) @@ -1088,6 +1088,59 @@ def test_cart_goal_1eef(pr2_world: World): kin_sim.tick_until_end() +def test_long_goal(pr2_world: World): + msc = MotionStatechart() + msc.add_nodes( + [ + cart_goal := CartesianPose( + root_link=pr2_world.root, + tip_link=pr2_world.get_kinematic_structure_entity_by_name( + "base_footprint" + ), + goal_pose=TransformationMatrix.from_xyz_rpy( + x=50, reference_frame=pr2_world.root + ), + ), + JointPositionList( + goal_state=JointState.from_str_dict( + { + "torso_lift_joint": 0.2999225173357618, + "head_pan_joint": 0.041880780651479044, + "head_tilt_joint": -0.37, + "r_upper_arm_roll_joint": -0.9487714747527726, + "r_shoulder_pan_joint": -1.0047307505973626, + "r_shoulder_lift_joint": 0.48736790658811985, + "r_forearm_roll_joint": -14.895833882874182, + "r_elbow_flex_joint": -1.392377908925028, + "r_wrist_flex_joint": -0.4548695149411013, + "r_wrist_roll_joint": 0.11426798984097819, + "l_upper_arm_roll_joint": 1.7383062350263658, + "l_shoulder_pan_joint": 1.8799810286792007, + "l_shoulder_lift_joint": 0.011627231224188975, + "l_forearm_roll_joint": 312.67276414458695, + "l_elbow_flex_joint": -2.0300928925694675, + "l_wrist_flex_joint": -0.1, + "l_wrist_roll_joint": -6.062015047706399, + }, + world=pr2_world, + ) + ), + ] + ) + msc.add_node(EndMotion.when_true(cart_goal)) + + kin_sim = Executor( + world=pr2_world, + controller_config=QPControllerConfig.create_default_with_50hz(), + ) + kin_sim.compile(motion_statechart=msc) + t = time.perf_counter() + kin_sim.tick_until_end(1_000_000) + after = time.perf_counter() + diff = after - t + print(diff / kin_sim.control_cycles) + + def test_cart_goal_sequence_at_build(pr2_world: World): tip = pr2_world.get_kinematic_structure_entity_by_name("base_footprint") root = pr2_world.get_kinematic_structure_entity_by_name("odom_combined") @@ -1266,7 +1319,9 @@ def test_pointing_cone(pr2_world: World): kin_sim.tick_until_end() # Check if angle between pointing axis and tip->goal vector is within the cone - root_V_pointing_axis = pr2_world.transform(target_frame=root, spatial_object=pointing_axis) + root_V_pointing_axis = pr2_world.transform( + target_frame=root, spatial_object=pointing_axis + ) root_V_pointing_axis.scale(1) v_pointing = root_V_pointing_axis.to_np()[:3] @@ -1284,9 +1339,10 @@ def test_pointing_cone(pr2_world: World): angle = angle_between_vector(v_pointing, v_goal) - assert angle <= cone_theta + pointing_cone.threshold, ( - f"PointingCone failed: final angle {angle:.6f} rad > cone_theta {cone_theta:.6f} rad" - ) + assert ( + angle <= cone_theta + pointing_cone.threshold + ), f"PointingCone failed: final angle {angle:.6f} rad > cone_theta {cone_theta:.6f} rad" + def test_align_planes(pr2_world: World): tip = pr2_world.get_kinematic_structure_entity_by_name("r_gripper_tool_frame") @@ -1624,7 +1680,7 @@ def test_open(self, pr2_world): ) pr2_world.add_degree_of_freedom(dof) root_T_door = RevoluteConnection( - dof_name=dof.name, + dof_id=dof.id, parent=pr2_world.root, child=door_world.root, axis=-cas.Vector3.Z(),