Skip to content

Commit

Permalink
add quadruped
Browse files Browse the repository at this point in the history
  • Loading branch information
@MikhailChaikovskii
MikhailChaikovskii committed Dec 10, 2023
1 parent 4655472 commit dafb032
Show file tree
Hide file tree
Showing 5 changed files with 132 additions and 85 deletions.
7 changes: 4 additions & 3 deletions examples/biped.py
View file
Original file line number Diff line number Diff line change
@@ -1,10 +1,11 @@
from rostok.simulation_chrono.simulation_scenario import WalkingScenario
from rostok.library.rule_sets.leg_rules import get_biped
from rostok.graph_grammar.graph_utils import plot_graph

scenario = WalkingScenario(0.00001, 20)

graph = get_biped()
plot_graph(graph)
control = controll_parameters = {"initial_value": [0]*8}

control = []

scenario.run_simulation(graph, control, starting_positions=[[0,0,0,0], [0,0,0,0]])
scenario.run_simulation(graph, control, starting_positions=[[0,-30,60,-30], [0,30,-60,30]], vis = True, delay=True)
11 changes: 11 additions & 0 deletions examples/quadruped.py
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,11 @@
from rostok.simulation_chrono.simulation_scenario import WalkingScenario
from rostok.library.rule_sets.leg_rules import get_quadruped
from rostok.graph_grammar.graph_utils import plot_graph

scenario = WalkingScenario(0.00001, 20)

graph = get_quadruped()
plot_graph(graph)
control = controll_parameters = {"initial_value": [0]*26}

scenario.run_simulation(graph, control, starting_positions=[[0,0,-5,5], [0,0,-5,5], [0, 30, -60, -30, 60], [0, 30, -60, -30, 60]], vis = True, delay=True)
12 changes: 7 additions & 5 deletions rostok/block_builder_chrono/block_classes.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -264,9 +264,11 @@ def connect(self, in_block: BuildingBody, out_block: BuildingBody, system: chron
self.joint.Initialize(out_block.body, in_block.body,
chrono.ChFrameD(joint_transform))
system.AddLink(self.joint)

self._add_limiting_link(in_block, out_block, system)
if (self.stiffness != 0) or (self.damping != 0):
self._add_spring_damper(in_block, out_block, system)

# if (self.stiffness != 0) or (self.damping != 0):
# self._add_spring_damper(in_block, out_block, system)

# if (self.with_collision):
# eps = 0.005
Expand Down Expand Up @@ -298,8 +300,8 @@ def _add_limiting_link(self, in_block: BuildingBody, out_block: BuildingBody,
p = joint_limiting_link.GetLimit_Rz()
#joint_limiting_link.
p.SetActive(True)
p.SetMax(1)
p.SetMin(0)
p.SetMax(0.01)
p.SetMin(-0.01)
system.AddLink(joint_limiting_link)
system.Update()

Expand Down Expand Up @@ -337,7 +339,7 @@ def __init__(self,
color: Optional[list[int]] = None):

# offset
eps = 0.001
eps = 0
# Create body
material = struct_material2object_material(material)

Expand Down
174 changes: 100 additions & 74 deletions rostok/library/rule_sets/leg_rules.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,38 +12,7 @@
from rostok.graph_grammar.node_vocabulary import NodeVocabulary
from rostok.utils.dataset_materials.material_dataclass_manipulating import (
DefaultChronoMaterialNSC, DefaultChronoMaterialSMC)


def get_density_box(mass: float, box: Box):
volume = box.height_z * box.length_y * box.width_x
return mass / volume


def rotation_y(alpha):
quat_Y_ang_alpha = chrono.Q_from_AngY(np.deg2rad(alpha))
return [quat_Y_ang_alpha.e0, quat_Y_ang_alpha.e1, quat_Y_ang_alpha.e2, quat_Y_ang_alpha.e3]


def rotation_z(alpha):
quat_Z_ang_alpha = chrono.Q_from_AngZ(np.deg2rad(alpha))
return [quat_Z_ang_alpha.e0, quat_Z_ang_alpha.e1, quat_Z_ang_alpha.e2, quat_Z_ang_alpha.e3]


import numpy as np
import pychrono as chrono

from rostok.block_builder_api.block_blueprints import (PrimitiveBodyBlueprint,
RevolveJointBlueprint,
TransformBlueprint)
from rostok.block_builder_api.block_parameters import JointInputType
from rostok.block_builder_api.easy_body_shapes import Box
from rostok.block_builder_chrono.blocks_utils import FrameTransform
from rostok.graph_grammar import rule_vocabulary
from rostok.graph_grammar.node import ROOT
from rostok.graph_grammar.node_vocabulary import NodeVocabulary
from rostok.utils.dataset_materials.material_dataclass_manipulating import (
DefaultChronoMaterialNSC, DefaultChronoMaterialSMC)

from rostok.graph_grammar.graph_utils import plot_graph

def get_density_box(mass: float, box: Box):
volume = box.height_z * box.length_y * box.width_x
Expand All @@ -60,25 +29,28 @@ def rotation_z(alpha):
return [quat_Z_ang_alpha.e0, quat_Z_ang_alpha.e1, quat_Z_ang_alpha.e2, quat_Z_ang_alpha.e3]


def create_rules(tendon=True, smc=False):
def create_rules(tendon=False, smc=False):

if smc:
def_mat = DefaultChronoMaterialSMC()
else:
def_mat = DefaultChronoMaterialNSC()
# blueprint for the palm
body = PrimitiveBodyBlueprint(
Box(0.3, 0.3, 0.3), material=def_mat, color=[255, 0, 0])
Box(0.05, 0.02, 0.2), material=def_mat, color=[100, 100, 0])

body_q = PrimitiveBodyBlueprint(
Box(0.3, 0.02, 0.2), material=def_mat, color=[100, 100, 0])
# blueprint for the base
base = PrimitiveBodyBlueprint(Box(0.02, 0.05, 0.02),
base = PrimitiveBodyBlueprint(Box(0.02, 0.01, 0.02),
material=def_mat,
color=[0, 120, 255],
color=[0, 255, 0],
density=10000)


foot = PrimitiveBodyBlueprint(Box(0.1, 0.05, 0.02),
foot = PrimitiveBodyBlueprint(Box(0.05, 0.02, 0.03),
material=def_mat,
color=[0, 120, 255],
color=[255, 0, 0],
density=100)
# sets effective density for the links, the real links are considered to be extendable.
link_mass = (28 + 1.62 + 2.77) * 1e-3
Expand All @@ -91,13 +63,17 @@ def create_rules(tendon=True, smc=False):
color=[0, 120, 255],
density=get_density_box(link_mass, Box(
0.02, x, 0.02))), length_link))
dummy_link = PrimitiveBodyBlueprint(Box(0.01,0.000001,0.01), material=def_mat, density =0.0001)



dummy_link = PrimitiveBodyBlueprint(Box(0.01,0.001,0.01), material=def_mat, density = 10000)
x_translation_values = [0.07, 0.107, 0.144]
X_TRANSLATIONS = list(
map(lambda x: FrameTransform([x, 0, 0], [1, 0, 0, 0]), x_translation_values))

X_TRANSLATIONS_NEGATIVE = list(
map(lambda x: FrameTransform([-x, 0, 0], [1, 0, 0, 0]), x_translation_values))
x_translation_transform = list(
map(lambda x: TransformBlueprint(x), X_TRANSLATIONS))
x_translation_transform_negative = list(
map(lambda x: TransformBlueprint(x), X_TRANSLATIONS_NEGATIVE))
z_translation_values = [0.055, 0.092, 0.129]
Z_TRANSLATIONS_POSITIVE = list(
map(lambda x: FrameTransform([0, 0, x], [1, 0, 0, 0]), z_translation_values))
Expand All @@ -107,11 +83,10 @@ def create_rules(tendon=True, smc=False):
# rotation to 180 degrees around vertical axis
REVERSE_Y = FrameTransform([0, 0, 0], [0, 0, 1, 0])

turn_values = [0, 30, 60]
TURNS_POSITIVE = list(map(lambda x: FrameTransform(
[0, 0, 0], rotation_y(x)), turn_values))
TURNS_NEGATIVE = list(map(lambda x: FrameTransform(
[0, 0, 0], rotation_y(-x)), turn_values))
z_translation_positive_transforms = list(
map(lambda x: TransformBlueprint(x), Z_TRANSLATIONS_POSITIVE))
z_translation_negative_transforms = list(
map(lambda x: TransformBlueprint(x), Z_TRANSLATIONS_NEGATIVE))
JOINT_TURN_POSITIVE = FrameTransform([0,0,0], rotation_y(90))
JOINT_TURN_NEGATIVE = FrameTransform([0,0,0], rotation_y(-90))
joint_turn_positive = TransformBlueprint(JOINT_TURN_POSITIVE)
Expand Down Expand Up @@ -171,6 +146,8 @@ def create_rules(tendon=True, smc=False):

node_vocab.create_node(label="BO", is_terminal=True,
block_blueprint=body)
node_vocab.create_node(label="BOQ", is_terminal=True,
block_blueprint=body_q)

node_vocab.create_node(label="LBL") # Left_Biped_Leg

Expand All @@ -181,16 +158,36 @@ def create_rules(tendon=True, smc=False):
node_vocab.create_node(label="LHQL") # Left_Hind_Quadruped_Leg
node_vocab.create_node(label="RHQL") # Right_Hind_Quadruped_Leg

node_vocab.create_node(label="TP")
node_vocab.create_node(label="TP1",
is_terminal=True,
block_blueprint=z_translation_positive_transforms[0])
node_vocab.create_node(label="TP2",
is_terminal=True,
block_blueprint=z_translation_positive_transforms[1])
node_vocab.create_node(label="TP3",
is_terminal=True,
block_blueprint=z_translation_positive_transforms[2])
node_vocab.create_node(label="TN")
node_vocab.create_node(label="TN1",
is_terminal=True,
block_blueprint=z_translation_negative_transforms[0])
node_vocab.create_node(label="TN2",
is_terminal=True,
block_blueprint=z_translation_negative_transforms[1])
node_vocab.create_node(label="TN3",
is_terminal=True,
block_blueprint=z_translation_negative_transforms[2])
node_vocab.create_node(label="RE", is_terminal=True,
block_blueprint=reverse_transform) # Reverse
node_vocab.create_node(label="JT", is_terminal=True,
block_blueprint=joint_turn_positive)
block_blueprint=joint_turn_positive) # joint transform
node_vocab.create_node(label="RJT", is_terminal=True,
block_blueprint=joint_turn_negative)
node_vocab.create_node(label="RT")
block_blueprint=joint_turn_negative) # inverse joint transfrom
node_vocab.create_node(label="DB", is_terminal=True, block_blueprint=dummy_link)
node_vocab.create_node(label="FT", is_terminal=True,
block_blueprint=foot)
node_vocab.create_node(label="RT")
node_vocab.create_node(label="RT1",
is_terminal=True,
block_blueprint=x_translation_transform[0])
Expand All @@ -200,6 +197,17 @@ def create_rules(tendon=True, smc=False):
node_vocab.create_node(label="RT3",
is_terminal=True,
block_blueprint=x_translation_transform[2])
node_vocab.create_node(label="RTN")
node_vocab.create_node(label="RTN1",
is_terminal=True,
block_blueprint=x_translation_transform_negative[0])
node_vocab.create_node(label="RTN2",
is_terminal=True,
block_blueprint=x_translation_transform_negative[1])
node_vocab.create_node(label="RTN3",
is_terminal=True,
block_blueprint=x_translation_transform_negative[2])

node_vocab.create_node(label="FG")
node_vocab.create_node(label="FG1")
if tendon:
Expand Down Expand Up @@ -229,28 +237,39 @@ def create_rules(tendon=True, smc=False):
block_blueprint=link[1])
node_vocab.create_node(label="L3", is_terminal=True,
block_blueprint=link[2])



rule_vocab = rule_vocabulary.RuleVocabulary(node_vocab)

rule_vocab.create_rule("Init_Biped", ["ROOT"], ["BO", "LBL", "RBL"], 0, 0,
[(0, 1), (0, 2)])

rule_vocab.create_rule("AddFinger", ["F"], ["RT", "B", "JB", "L", "FG"], 0, 0, [(0, 1), (1, 2),
(2, 3), (3, 4)])
rule_vocab.create_rule("Init_Quadruped", ["ROOT"], ["BOQ", "LFQL", "RFQL", "LHQL", "RHQL"], 0, 0,
[(0, 1), (0, 2), (0, 3), (0, 4)])


rule_vocab.create_rule("AddLeftLeg", ["LBL"], ["TN", "B", "JT", "J","DB","RJT","FG"],0,0,[(0, 1), (1, 2),
(2, 3), (3, 4),(4,5),(5,6)])
rule_vocab.create_rule("AddRightLeg", ["RBL"], ["TP", "B", "JT", "J","DB","RJT","FG"],0,0,[(0, 1), (1, 2),
(2, 3), (3, 4),(4,5),(5,6)])
# rule_vocab.create_rule("AddLeftLeg", ["LBL"], ["TN", "B", "J","DB","FG"],0,0,[(0, 1), (1, 2),
# (2, 3), (3, 4)])
# rule_vocab.create_rule("AddRightLeg", ["RBL"], ["TP", "B", "J","DB","FG"],0,0,[(0, 1), (1, 2),
# (2, 3), (3, 4)])
# rule_vocab.create_rule("AddLeftLeg", ["LBL"], ["TN", "B","FG"],0,0,[(0, 1), (1, 2)])
# rule_vocab.create_rule("AddRightLeg", ["RBL"], ["TP", "B","FG"],0,0,[(0, 1), (1, 2)])

rule_vocab.create_rule("AddLeftFrontLeg", ["LFQL"], ["TN", "RT2","B", "JT", "J","DB","RJT","FG"],0,0,[(0, 1), (1, 2),
(2, 3), (3, 4),(4,5),(5,6),(6,7)])
rule_vocab.create_rule("AddRightFrontLeg", ["RFQL"], ["TP", "RT2","B", "JT", "J","DB","RJT","FG"],0,0,[(0, 1), (1, 2),
(2, 3), (3, 4),(4,5),(5,6),(6,7)])
rule_vocab.create_rule("AddLeftHindLeg", ["LHQL"], ["TN", "RTN2", "B", "JT", "J","DB","RJT","FG"],0,0,[(0, 1), (1, 2),
(2, 3), (3, 4),(4,5),(5,6),(6,7)])
rule_vocab.create_rule("AddRightHindLeg", ["RHQL"], ["TP", "RTN2", "B", "JT", "J","DB","RJT","FG"],0,0,[(0, 1), (1, 2),
(2, 3), (3, 4),(4,5),(5,6),(6,7)])



rule_vocab.create_rule("Terminal_Radial_Translate1",
["RT"], ["RT1"], 0, 0, [])
rule_vocab.create_rule("Terminal_Radial_Translate2",
["RT"], ["RT2"], 0, 0, [])
rule_vocab.create_rule("Terminal_Radial_Translate3",
["RT"], ["RT3"], 0, 0, [])

rule_vocab.create_rule("Phalanx", ["FG"], [
"J", "L", "FG"], 0, 0, [(0, 1), (1, 2)])
# rule_vocab.create_rule("Phalanx_1", ["FG1"], ["B", "JB", "L", "FG"], 0, 0, [(0, 1), (1, 2), (2, 3)])
Expand All @@ -270,7 +289,7 @@ def create_rules(tendon=True, smc=False):
rule_vocab.create_rule("Terminal_Link2", ["L"], ["L2"], 0, 0, [])
rule_vocab.create_rule("Terminal_Link3", ["L"], ["L3"], 0, 0, [])
#rule_vocab.create_rule("Remove_FG", ["FG"], [], 0, 0, [])
rule_vocab.create_rule("AddFoot", ["FG"], ["J", "FT"], 0, 0, [(0, 1), (1, 2)])
rule_vocab.create_rule("AddFoot", ["FG"], ["J", "FT"], 0, 0, [(0, 1)])
# rule_vocab.create_rule("Remove_FG1", ["FG1"], [], 0, 0, [])

rule_vocab.create_rule("Terminal_Positive_Translate1", [
Expand All @@ -285,26 +304,33 @@ def create_rules(tendon=True, smc=False):
"TN"], ["TN2"], 0, 0, [])
rule_vocab.create_rule("Terminal_Negative_Translate3", [
"TN"], ["TN3"], 0, 0, [])
rule_vocab.create_rule("Terminal_Positive_Turn_0", [
"TURN_P"], ["TURN_P_0"], 0, 0, [])
rule_vocab.create_rule("Terminal_Positive_Turn_1", [
"TURN_P"], ["TURN_P_1"], 0, 0, [])
rule_vocab.create_rule("Terminal_Positive_Turn_2", [
"TURN_P"], ["TURN_P_2"], 0, 0, [])
rule_vocab.create_rule("Terminal_Negative_Turn_0", [
"TURN_N"], ["TURN_N_0"], 0, 0, [])
rule_vocab.create_rule("Terminal_Negative_Turn_1", [
"TURN_N"], ["TURN_N_1"], 0, 0, [])
rule_vocab.create_rule("Terminal_Negative_Turn_2", [
"TURN_N"], ["TURN_N_2"], 0, 0, [])

return rule_vocab

from rostok.graph_grammar.node import GraphGrammar

def get_biped():
graph = GraphGrammar()
rules = [
"Init_Biped", "AddLeftLeg", "AddRightLeg", "Phalanx", "Phalanx", "AddFoot", "AddFoot", "Terminal_Link2", "Terminal_Link2"
"Init_Biped", "AddLeftLeg", "AddRightLeg", "Phalanx", "Phalanx","Phalanx", "Phalanx", "AddFoot", "AddFoot",
"Terminal_Link2", "Terminal_Link2", "Terminal_Link3","Terminal_Link3","Terminal_Positive_Translate1",
"Terminal_Negative_Translate1"
]
rule_vocabul = create_rules()
for rule in rules:
graph.apply_rule(rule_vocabul.get_rule(rule))

return graph


def get_quadruped():
graph = GraphGrammar()
rules = [
"Init_Quadruped",
"AddLeftFrontLeg", "Phalanx","Phalanx", "AddFoot", "Terminal_Link2", "Terminal_Link3","Terminal_Negative_Translate1",
"AddRightFrontLeg", "Phalanx", "Phalanx", "AddFoot", "Terminal_Link2", "Terminal_Link3", "Terminal_Positive_Translate1",
"AddLeftHindLeg", "Phalanx", "Phalanx","Phalanx", "AddFoot", "Terminal_Link3","Terminal_Link2","Terminal_Link1","Terminal_Negative_Translate1",
"AddRightHindLeg", "Phalanx", "Phalanx","Phalanx", "AddFoot", "Terminal_Link3","Terminal_Link2","Terminal_Link1","Terminal_Positive_Translate1",
]
rule_vocabul = create_rules()
for rule in rules:
Expand Down
13 changes: 10 additions & 3 deletions rostok/simulation_chrono/simulation_scenario.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -136,6 +136,7 @@ def get_scenario_name(self):
from rostok.block_builder_api.easy_body_shapes import Box
from rostok.utils.dataset_materials.material_dataclass_manipulating import (
DefaultChronoMaterialNSC, DefaultChronoMaterialSMC)
from rostok.block_builder_api.block_parameters import (DefaultFrame, FrameTransform)
class WalkingScenario(ParametrizedSimulation):
def __init__(self,
step_length,
Expand Down Expand Up @@ -170,7 +171,7 @@ def run_simulation(self,
if self.smc:
system = ChronoSystems.chrono_SMC_system(gravity_list=[0, 0, 0])
else:
system = ChronoSystems.chrono_NSC_system(gravity_list=[0, -10, 0])
system = ChronoSystems.chrono_NSC_system(gravity_list=[0, -1, 0])
# setup the auxiliary
env_creator = EnvCreator([])
vis_manager = ChronoVisManager(delay)
Expand All @@ -180,7 +181,7 @@ def run_simulation(self,
def_mat = DefaultChronoMaterialSMC()
else:
def_mat = DefaultChronoMaterialNSC()
floor = creator.create_environment_body(EnvironmentBodyBlueprint(Box(3, 0.01, 3), material=def_mat, color=[255, 0, 0]))
floor = creator.create_environment_body(EnvironmentBodyBlueprint(Box(1, 0.1, 1), material=def_mat, color=[215, 255, 0]))
floor.body.SetNameString("Floor")
floor.body.SetPos(chrono.ChVectorD(0,0,0))
#floor.body.SetBodyFixed(True)
Expand All @@ -192,10 +193,16 @@ def run_simulation(self,

# add design and determine the outer force

# simulation.add_design(graph,
# controller_data,
# self.controller_cls,
# Frame=FrameTransform([0, 0.25, 0], [3**0.5/2, 0, 0, 1/2]),
# starting_positions=starting_positions, is_fixed=False)
simulation.add_design(graph,
controller_data,
self.controller_cls,
starting_positions=starting_positions)
Frame=FrameTransform([0, 0.25, 0], [0,0,0,1]),
starting_positions=starting_positions, is_fixed=False)

# setup parameters for the data store

Expand Down

0 comments on commit dafb032

Please sign in to comment.