Calc stifness

aimclub · Aug 1, 2023 · 7082026 · 7082026
1 parent 7052d44
commit 7082026
Show file tree

Hide file tree

Showing 3 changed files with 13 additions and 8 deletions.
diff --git a/examples/example_simulation_tendon.py b/examples/example_simulation_tendon.py
@@ -129,14 +129,17 @@ def rotation_x(alpha):
 telo1 = sim.robot.get_graph().body_map_ordered[20].body
 telo2 = sim.robot.get_graph().body_map_ordered[21].body
 telo3 = sim.robot.get_graph().body_map_ordered[22].body
-
-
+weighted_m = 0.05 * telo1.GetMass() + 0.15 * telo2.GetMass() + 0.2 * telo3.GetMass()
+total_m = telo1.GetMass() + telo2.GetMass() + telo3.GetMass()
+CoG_finger = weighted_m / total_m
+torque_on_base_j = 9.8 * weighted_m
+stifness = torque_on_base_j / np.deg2rad(45)
 j1 = sim.robot.get_graph().joint_map_ordered[23].joint
 j2 = sim.robot.get_graph().joint_map_ordered[24].joint
 j3 = sim.robot.get_graph().joint_map_ordered[25].joint
-STEPS = 1000*1
-VIS = False
-TENDON_FORCE = 0.5
+STEPS = 1000*10
+VIS = True
+TENDON_FORCE = 0   
 VINOS = -0.04
 force_tendon11 = TendonForce([VINOS, -0.04, 0])
 force_tendon12 = TendonForce([VINOS, 0.04, 0])
@@ -202,6 +205,7 @@ def rotation_x(alpha):
     all_data = [data_11, data_12, data_21, data_22, data_31, data_tip]
     for a, b  in zip(forces, all_data):
         a.update(0,b)
+        pass
 
     if VIS:
         vis.Run()

diff --git a/rostok/block_builder_api/block_blueprints.py b/rostok/block_builder_api/block_blueprints.py
@@ -49,6 +49,7 @@ class RevolveJointBlueprint(JointBlueprintType):
     density: float = 1000.0
     stiffness: float = 0.
     damping: float = 0.
+    equilibrium_position: float = 0.
     with_collision: bool = True
 
 

diff --git a/rostok/library/rule_sets/ruleset_old_style_graph.py b/rostok/library/rule_sets/ruleset_old_style_graph.py
@@ -43,9 +43,9 @@ def rotation_z(alpha):
     turn_90_transform = TransformBlueprint(FrameTransform([0, 0, 0], rotation_y(90)))
     #revolve = RevolveJointBlueprint(JointInputType.POSITION)
     revolve = RevolveJointBlueprint(JointInputType.TORQUE, 0.02, 0.04)
-    revolve1 = RevolveJointBlueprint(JointInputType.TORQUE, 0.02, 0.04, density = 100, stiffness=0.1, damping=0.001, starting_angle=-45)
-    revolve2 = RevolveJointBlueprint(JointInputType.TORQUE, 0.02, 0.04, density = 100, stiffness=0.06, damping=0.001)
-    revolve3 = RevolveJointBlueprint(JointInputType.TORQUE, 0.02, 0.04, density = 100, stiffness=0.04, damping=0.001)
+    revolve1 = RevolveJointBlueprint(JointInputType.TORQUE, 0.02, 0.04, density = 10, stiffness=0.12 * 4, damping=0.001, starting_angle=-45, equilibrium_position=-0.785398 / 4)
+    revolve2 = RevolveJointBlueprint(JointInputType.TORQUE, 0.02, 0.04, density = 10, stiffness=0.05 * 4, damping=0.001, equilibrium_position=(-0.785398 /  2) / 4)
+    revolve3 = RevolveJointBlueprint(JointInputType.TORQUE, 0.02, 0.04, density = 10, stiffness=0.015 * 4 , damping=0.001, equilibrium_position=(-0.785398 / 2) / 4)
 
     #revolve_45 = RevolveJointBlueprint(JointInputType.TORQUE, starting_angle=45)
     #no_control = RevolveJointBlueprint(JointInputType.UNCONTROL)