add offset

aimclub · Aug 6, 2024 · d09e067 · d09e067
1 parent d34988c
commit d09e067
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 5 deletions.
diff --git a/rostok/block_builder_chrono/block_classes.py b/rostok/block_builder_chrono/block_classes.py
@@ -181,6 +181,7 @@ def __init__(self,
                  damping: float = 0.,
                  equilibrium_position: float = 0.,
                  stopper: Optional[list[float]] = None,
+                 offset=[0.0, 0.0],
                  name='unnamed'
                  ):
         super().__init__()
@@ -192,6 +193,7 @@ def __init__(self,
         self.equilibrium_position = equilibrium_position
         self.stopper = stopper
         self.name = name
+        self.offset = offset
 
     def set_prev_body_frame(self, prev_block: BuildingBody, system: chrono.ChSystem):
         """Turns previous body frame using the starting angle. The rotation of the slave body occurs around the z-axis.
@@ -200,7 +202,7 @@ def set_prev_body_frame(self, prev_block: BuildingBody, system: chrono.ChSystem)
             prev_block (BuildingBody): the body block that is a master in the joint
             system (chrono.ChSystem): chrono system to be updated
         """
-        additional_transform = chrono.ChCoordsysD(chrono.ChVectorD(0, 0, 0),
+        additional_transform = chrono.ChCoordsysD(chrono.ChVectorD(0,0, 0),
                                                   rotation_z_q(self.starting_angle))
         transform = prev_block.transformed_frame_out.GetCoord()
         prev_block.transformed_frame_out.SetCoord(
@@ -223,6 +225,9 @@ def connect(self, in_block: BuildingBody, out_block: BuildingBody, system: chron
         self.joint.SetNameString(self.name)
         # The position of the joint frame in the global coordinate system, the rotation is around z axis of that frame
         joint_transform = in_block.transformed_frame_out.GetAbsCoord()
+        # add offset
+        joint_transform = joint_transform * chrono.ChCoordsysD(chrono.ChVectorD(self.offset[0], self.offset[1], 0),
+                                                              chrono.ChQuaternionD(1, 0, 0, 0))
         # the signature is Slave Body, Master Body, Joint Frame
         self.joint.Initialize(out_block.body, in_block.body,
                               chrono.ChFrameD(joint_transform))

diff --git a/rostok/block_builder_chrono/block_connect.py b/rostok/block_builder_chrono/block_connect.py
@@ -65,8 +65,8 @@ def place_and_connect(sequence: list[BLOCK_CLASS_TYPES], system: chrono.ChSystem
                     collision_model.SetFamilyMaskNoCollisionWithFamily(finger_number)
                     if sequence[it - 1].block_type is BlockType.TRANSFORM_INPUT:
                         # aggregate all transforms that should be applied to the input frame of the body
-                        # each body has its own initial position for input frame, and this transformation is additional to it, 
-                        # transformation is performed in the coordinate system of the body 
+                        # each body has its own initial position for input frame, and this transformation is additional to it,
+                        # transformation is performed in the coordinate system of the body
                         i = 1
                         transform = True
                         while transform:

diff --git a/rostok/library/rule_sets/simple_designs.py b/rostok/library/rule_sets/simple_designs.py
@@ -217,7 +217,7 @@ def get_two_link_one_finger():
             "RemoveFinger_P", 
             "RemoveFinger_RP"
     ]
-    rule_vocabul, _ = create_rules()
+    rule_vocabul = create_rules()
     for rule in rules:
         graph.apply_rule(rule_vocabul.get_rule(rule))
 
@@ -235,7 +235,7 @@ def get_one_link_one_finger():
             "RemoveFinger_P",
             "RemoveFinger_RP"
     ]
-    rule_vocabul, _ = create_rules()
+    rule_vocabul = create_rules()
     for rule in rules:
         graph.apply_rule(rule_vocabul.get_rule(rule))