Skip to content
Permalink

Comparing changes

This is a direct comparison between two commits made in this repository or its related repositories. View the default comparison for this range or learn more about diff comparisons.

Open a pull request

Create a new pull request by comparing changes across two branches. If you need to, you can also . Learn more about diff comparisons here.
base repository: ami-iit/adam
Failed to load repositories. Confirm that selected base ref is valid, then try again.
Loading
base: 1114f1454a7c5a8a665ac4edb72687c49444100b
Choose a base ref
..
head repository: ami-iit/adam
Failed to load repositories. Confirm that selected head ref is valid, then try again.
Loading
compare: 01533b626e60bf3104faf92d9a89fb25c7320ce8
Choose a head ref
Showing with 30 additions and 45 deletions.
  1. +6 −13 src/adam/core/rbd_algorithms.py
  2. +5 −7 src/adam/model/model.py
  3. +19 −25 src/adam/model/tree.py
19 changes: 6 additions & 13 deletions src/adam/core/rbd_algorithms.py
View file
Original file line number Diff line number Diff line change
@@ -482,7 +482,6 @@ def aba(
"""
model = self.model.reduce(self.model.actuated_joints)
joints = list(model.joints.values())
joints.sort(key=lambda joint: joint.idx)

NB = model.N

@@ -529,6 +528,7 @@ def get_tree_transform(self, joints) -> "Array":
)

tree_transform = get_tree_transform(self, joints)
p = lambda i: list(model.tree.graph).index(joints[i].parent)

# Pass 1
for i, joint in enumerate(joints[1:], start=1):
@@ -539,11 +539,8 @@ def get_tree_transform(self, joints) -> "Array":
i_X_pi[i] = joint.spatial_transform(q) @ tree_transform[i]
v_J = joint.motion_subspace() * q_dot

# TODO: reassign idx after reducing the model
pi = model.tree.get_idx_from_name(joint.child)

v[i] = i_X_pi[i] @ v[pi] + v_J
c[i] = i_X_pi[i] @ c[pi] + self.math.spatial_skew(v[i]) @ v_J
v[i] = i_X_pi[i] @ v[p(i)] + v_J
c[i] = i_X_pi[i] @ c[p(i)] + self.math.spatial_skew(v[i]) @ v_J

IA[i] = model.tree.get_node_from_name(joint.parent).link.spatial_inertia()

@@ -569,11 +566,9 @@ def get_tree_transform(self, joints) -> "Array":
Ia = IA[i] - U[i] / D[i] @ U[i].T
pa = pA[i] + Ia @ c[i] + U[i] * u[i] / D[i]

pi = model.tree.get_idx_from_name(joint.child)

if joint.parent != self.root_link or not model.floating_base:
IA[pi] += i_X_pi[i].T @ Ia @ i_X_pi[i]
pA[pi] += i_X_pi[i].T @ pa
IA[p(i)] += i_X_pi[i].T @ Ia @ i_X_pi[i]
pA[p(i)] += i_X_pi[i].T @ pa
continue

a[0] = B_X_W @ g if model.floating_base else self.math.solve(-IA[0], pA[0])
@@ -583,11 +578,9 @@ def get_tree_transform(self, joints) -> "Array":
if joint.parent == self.root_link:
continue

pi = model.tree.get_idx_from_name(joint.child)

sdd[i - 1] = (u[i] - U[i].T @ a[i]) / D[i]

a[i] += i_X_pi[i].T @ a[pi] + joint.motion_subspace() * sdd[i - 1] + c[i]
a[i] += i_X_pi[i].T @ a[p(i)] + joint.motion_subspace() * sdd[i - 1] + c[i]

# Squeeze sdd
s_ddot = self.math.vertcat(*[sdd[i] for i in range(sdd.shape[0])])
12 changes: 5 additions & 7 deletions src/adam/model/model.py
View file
Original file line number Diff line number Diff line change
@@ -98,14 +98,12 @@ def reduce(self, joints_name_list: List[str]) -> "Model":
)

tree = self.tree.reduce(joints_name_list)
joints_list = list(
filter(
lambda joint: joint.name in self.actuated_joints,
self.joints.values(),
)
)

joints_list = [
node.parent_arc for node in tree.graph.values() if node.name != tree.root
]
joints_list.sort(key=lambda joint: joint.idx)
# update nodes dict

links = {node.name: node.link for node in tree.graph.values()}
joints = {joint.name: joint for joint in joints_list}
frames = {
44 changes: 19 additions & 25 deletions src/adam/model/tree.py
View file
Original file line number Diff line number Diff line change
@@ -82,15 +82,6 @@ def reduce(self, considered_joint_names: List[str]) -> "Tree":
Returns:
Tree: the reduced tree
"""
# find the nodes between two fixed joints
nodes_to_lump = list(
{
joint.child
for node in self.graph.values()
for joint in node.arcs
if joint.name not in considered_joint_names
}
)

relative_transform = (
lambda node: node.link.math.inv(
@@ -101,21 +92,17 @@ def reduce(self, considered_joint_names: List[str]) -> "Tree":
else node.parent_arc.spatial_transform(0)
)

last = []
leaves = [node for node in self.graph.values() if node.children == last]
# find the tree leaves and proceed until the root
leaves = [node for node in self.graph.values() if node.children == []]

while all(leaf.name != self.root for leaf in leaves):
for leaf in leaves:
if leaf is self.graph[self.root]:
continue

if leaf.parent_arc.name not in considered_joint_names:
# create the new node
if leaf.parent_arc.name not in considered_joint_names + [self.root]:
new_node = Node(
name=leaf.parent.name,
link=None,
arcs=[],
children=None,
children=[],
parent=None,
parent_arc=None,
)
@@ -129,22 +116,29 @@ def reduce(self, considered_joint_names: List[str]) -> "Tree":
# update the parents
new_node.parent = self.graph[leaf.parent.name].parent
new_node.parent_arc = self.graph[new_node.name].parent_arc
new_node.parent_arc.parent = (
leaf.children[0].parent_arc.name if leaf.children != [] else []
)

# update the children
new_node.children = leaf.children
new_node.children = [
child for child in leaf.children if child.name in self.graph
]

for child in new_node.children:
child.parent = new_node.link
child.parent_arc = new_node.parent_arc

# update the arcs
if leaf.arcs != []:
for arc in leaf.arcs:
if arc.name in considered_joint_names:
new_node.arcs.append(arc)
new_node.arcs = (
[arc for arc in leaf.arcs if arc.name in considered_joint_names]
if leaf.arcs != []
else []
)
for j in new_node.arcs:
j.parent = new_node.link.name

logging.debug(f"Removing {leaf.name}")
self.graph.pop(leaf.name)
self.graph[new_node.name] = new_node
self.ordered_nodes_list.remove(leaf.name)
leaves = [
self.get_node_from_name((leaf.parent.name))
for leaf in leaves