Inspect Skeleton: Navigating Bodynodes, Joints, and Dofs¶
This example demonstrates how to navigate the components of the given skeleton including body nodes, joints, dofs, shapes, markers, and so on.
python inspect_skeleton.py data/sdf/atlas/atlas_v3_no_head.sdf
Result¶
----------------------------------------
[Basic information]
mass = 146.554000
# DoFs = 33
[BodyNode]
root_bodynode[0] = [BodyNode(0): pelvis]
[BodyNode(0): pelvis]
mass = 17.8820Kg
parent = None
childs = [[BodyNode(1): ltorso], [BodyNode(9): l_uglut], [BodyNode(21): r_uglut]]
COM = [ 0.0111 0. 0.0271]
# dependent dofs = 6
# shapenodes = [[ShapeNode(0:0)], [ShapeNode(0:1)]]
# markers = 0
[BodyNode(1): ltorso]
mass = 2.4090Kg
parent = [BodyNode(0): pelvis]
childs = [[BodyNode(2): mtorso]]
COM = [ -2.37984000e-02 -3.15366000e-06 7.46835000e-02]
# dependent dofs = 7
# shapenodes = [[ShapeNode(1:0)], [ShapeNode(1:1)]]
# markers = 0
[BodyNode(2): mtorso]
mass = 0.6900Kg
parent = [BodyNode(1): ltorso]
childs = [[BodyNode(3): utorso]]
COM = [-0.02066266 -0.0131245 0.1925674 ]
# dependent dofs = 8
# shapenodes = [[ShapeNode(2:0)], [ShapeNode(2:1)]]
# markers = 0
...
Code¶
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 | if __name__ == '__main__':
import sys
import pydart2 as pydart
if len(sys.argv) != 2:
print("Usage: inspect_skeleton.py [*.urdf/*.sdf]")
exit(0)
skel_path = sys.argv[1]
print("skeleton path = %s" % skel_path)
pydart.init()
print("pydart init OK")
world = pydart.World(1.0 / 1000.0)
print("World init OK")
world.g = [0.0, 0.0, -9.8]
print("gravity = %s" % str(world.g))
skel = world.add_skeleton(skel_path)
print("Skeleton add OK")
print('----------------------------------------')
print('[Basic information]')
print('\tmass = %.6f' % skel.m)
print('\t# DoFs = %s' % skel.ndofs)
print('[BodyNode]')
print('root_bodynode[0] = ' + str(skel.root_bodynode(index=0)))
for body in skel.bodynodes:
print("\t" + str(body))
print("\t\tmass = %.4fKg" % body.m)
print("\t\tparent = " + str(body.parent_bodynode))
print("\t\tchilds = " + str(body.child_bodynodes))
print("\t\tCOM = " + str(body.C))
print("\t\t# dependent dofs = %d" % len(body.dependent_dofs))
print("\t\t# shapenodes = %s" % str(body.shapenodes))
print("\t\t# markers = %d" % len(body.markers))
print("J = %s" % str(body.J))
print('[DegreeOfFreedom]')
for dof in skel.dofs:
print("\t" + str(dof) + " belongs to " + str(dof.joint))
# print("\t\tindex in skeleton = " + str(dof.index_in_skeleton()))
# print("\t\tposition = " + str(dof.position()))
print('[Joint]')
for joint in skel.joints:
print("\t" + str(joint))
print("\t\tparent = " + str(joint.parent_bodynode))
print("\t\tchild = " + str(joint.child_bodynode))
print("\t\tdofs = " + str(joint.dofs))
print('[Markers]')
for marker in skel.markers:
print("\t" + str(marker) + " attached to " + str(marker.bodynode))
print("\t\t" + str(marker.world_position()))
print('[Position]')
print('\tpositions = %s' % str(skel.q))
print('\tvelocities = %s' % str(skel.dq))
print('\tstates = %s' % str(skel.x))
print('[Limits]')
print('\tposition_lower_limits = %s' % str(skel.q_lower))
print('\tposition_upper_limits = %s' % str(skel.q_upper))
print('\tforce_lower_limits = %s' % str(skel.tau_lower))
print('\tforce_upper_limits = %s' % str(skel.tau_upper))
print('[Lagrangian]')
print('\tmass matrix = %s' % str(skel.M))
print('\tcoriolis_and_gravity_forces = %s' % str(skel.c))
print('\tconstraint_forces = %s' % str(skel.constraint_forces()))
print('----------------------------------------')
|