Skip to content

Commit

Permalink
[dartpy] Format code using black
Browse files Browse the repository at this point in the history
  • Loading branch information
jslee02 committed Mar 13, 2024
1 parent ba99a8e commit 5f43592
Show file tree
Hide file tree
Showing 30 changed files with 568 additions and 5,065 deletions.
321 changes: 260 additions & 61 deletions pixi.lock

Large diffs are not rendered by default.

2 changes: 2 additions & 0 deletions pixi.toml
Original file line number Diff line number Diff line change
Expand Up @@ -34,6 +34,8 @@ pkg-config = ">=0.29.2,<0.30"
pytest = ">=8.1.1,<8.2"
doxygen = ">=1.10.0,<1.11"
setuptools = ">=69.1.1,<69.2"
black = ">=24.2.0,<24.3"
isort = ">=5.13.2,<5.14"

[target.linux-64.dependencies]
freeglut = ">=3.2.2,<3.3"
Expand Down
78 changes: 48 additions & 30 deletions python/examples/biped_stand/main.py
Original file line number Diff line number Diff line change
Expand Up @@ -41,14 +41,14 @@ def __init__(self, world, skel):
self.world = world
self.skel = skel
self.dofs = self.skel.getNumDofs()
self.left_heel = self.skel.getBodyNode('h_heel_left')
self.left_heel = self.skel.getBodyNode("h_heel_left")
self.left_foot = [
self.skel.getDof('j_heel_left_1').getIndexInSkeleton(),
self.skel.getDof('j_toe_left').getIndexInSkeleton(),
self.skel.getDof("j_heel_left_1").getIndexInSkeleton(),
self.skel.getDof("j_toe_left").getIndexInSkeleton(),
]
self.right_foot = [
self.skel.getDof('j_heel_right_1').getIndexInSkeleton(),
self.skel.getDof('j_toe_right').getIndexInSkeleton(),
self.skel.getDof("j_heel_right_1").getIndexInSkeleton(),
self.skel.getDof("j_toe_right").getIndexInSkeleton(),
]
self.timestep = world.getTimeStep()
self.Kp = np.eye(self.dofs)
Expand Down Expand Up @@ -83,15 +83,17 @@ def __init__(self, world, skel):
self.world.addSimpleFrame(self.ext_force_simple_frame)

def customPreStep(self):
q = self.skel.getPositions();
dq = self.skel.getVelocities();
q = self.skel.getPositions()
dq = self.skel.getVelocities()
constraint_forces = self.skel.getConstraintForces()

# SPD tracking
invM = np.linalg.inv(self.skel.getMassMatrix() + self.Kd * self.timestep)
p = np.matmul(-self.Kp, q + dq * self.timestep - self.q_d)
d = np.matmul(-self.Kd, dq)
ddq = np.matmul(invM, -self.skel.getCoriolisAndGravityForces() + p + d + constraint_forces)
ddq = np.matmul(
invM, -self.skel.getCoriolisAndGravityForces() + p + d + constraint_forces
)

self.torques = p + d + np.matmul(-self.Kd, ddq) * self.timestep

Expand All @@ -101,21 +103,37 @@ def customPreStep(self):

offset = com[0] - cop[0]
if offset < 0.1 and offset > 0.0:
k1 = 200
k2 = 100
kd = 10
self.torques[self.left_foot[0]] += -k1 * offset + kd * (self.pre_offset - offset)
self.torques[self.left_foot[1]] += -k2 * offset + kd * (self.pre_offset - offset)
self.torques[self.right_foot[0]] += -k1 * offset + kd * (self.pre_offset - offset)
self.torques[self.right_foot[1]] += -k2 * offset + kd * (self.pre_offset - offset)
k1 = 200
k2 = 100
kd = 10
self.torques[self.left_foot[0]] += -k1 * offset + kd * (
self.pre_offset - offset
)
self.torques[self.left_foot[1]] += -k2 * offset + kd * (
self.pre_offset - offset
)
self.torques[self.right_foot[0]] += -k1 * offset + kd * (
self.pre_offset - offset
)
self.torques[self.right_foot[1]] += -k2 * offset + kd * (
self.pre_offset - offset
)
elif offset > -0.2 and offset < -0.05:
k1 = 2000
k2 = 100
kd = 100
self.torques[self.left_foot[0]] += -k1 * offset + kd * (self.pre_offset - offset)
self.torques[self.left_foot[1]] += -k2 * offset + kd * (self.pre_offset - offset)
self.torques[self.right_foot[0]] += -k1 * offset + kd * (self.pre_offset - offset)
self.torques[self.right_foot[1]] += -k2 * offset + kd * (self.pre_offset - offset)
self.torques[self.left_foot[0]] += -k1 * offset + kd * (
self.pre_offset - offset
)
self.torques[self.left_foot[1]] += -k2 * offset + kd * (
self.pre_offset - offset
)
self.torques[self.right_foot[0]] += -k1 * offset + kd * (
self.pre_offset - offset
)
self.torques[self.right_foot[1]] += -k2 * offset + kd * (
self.pre_offset - offset
)

# Just to make sure no illegal torque is used
for i in range(6):
Expand All @@ -128,7 +146,7 @@ def customPreStep(self):
if self.ext_force_duration <= 0:
self.ext_force_duration = 0
self.ext_force = np.zeros(3)
spine = self.skel.getBodyNode('h_spine')
spine = self.skel.getBodyNode("h_spine")
spine.addExtForce(self.ext_force)
if self.ext_force_duration > 0:
arrow_head = spine.getTransform().translation()
Expand All @@ -149,18 +167,18 @@ def external_force(self):


def main():
world = dart.utils.SkelParser.readWorld('dart://sample/skel/fullbody1.skel')
world = dart.utils.SkelParser.readWorld("dart://sample/skel/fullbody1.skel")
world.setGravity([0, -9.81, 0])

biped = world.getSkeleton('fullbody1')
biped.getDof('j_pelvis_rot_y').setPosition(-0.20)
biped.getDof('j_thigh_left_z').setPosition(0.15)
biped.getDof('j_shin_left').setPosition(-0.40)
biped.getDof('j_heel_left_1').setPosition(0.25)
biped.getDof('j_thigh_right_z').setPosition(0.15)
biped.getDof('j_shin_right').setPosition(-0.40)
biped.getDof('j_heel_right_1').setPosition(0.25)
biped.getDof('j_abdomen_2').setPosition(0.00)
biped = world.getSkeleton("fullbody1")
biped.getDof("j_pelvis_rot_y").setPosition(-0.20)
biped.getDof("j_thigh_left_z").setPosition(0.15)
biped.getDof("j_shin_left").setPosition(-0.40)
biped.getDof("j_heel_left_1").setPosition(0.25)
biped.getDof("j_thigh_right_z").setPosition(0.15)
biped.getDof("j_shin_right").setPosition(-0.40)
biped.getDof("j_heel_right_1").setPosition(0.25)
biped.getDof("j_abdomen_2").setPosition(0.00)

node = MyWorldNode(world, biped)

Expand Down
9 changes: 3 additions & 6 deletions python/examples/contacts_pointcloud/main.py
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@


class ContactVisualizingNode(dart.gui.osg.RealTimeWorldNode):
""" WorldNode subclass, which extracts the contacts of the last simulation
"""WorldNode subclass, which extracts the contacts of the last simulation
step and adds them to a pointloud for visualization.
Note: Simulation must run for the contacts to be displayed.
Expand All @@ -36,14 +36,11 @@ def customPreRefresh(self):
self._pointCloudShape.setPoint(contactPoints)

def _getListOfContactPoints(self):
return [
c.point
for c in self.getWorld().getLastCollisionResult().getContacts()
]
return [c.point for c in self.getWorld().getLastCollisionResult().getContacts()]


def setAlpha(skeleton, alphaValue):
""" setAlpha(skeleton : dartpy.dynamics.Skeleton, alphaValue : float) -> None
"""setAlpha(skeleton : dartpy.dynamics.Skeleton, alphaValue : float) -> None
sets the transparency (alpha) value for each body node of the skeleton,
assuming that the VisualAspect is contained within the first ShapeNode
Expand Down
11 changes: 6 additions & 5 deletions python/examples/drag_and_drop/main.py
Original file line number Diff line number Diff line change
Expand Up @@ -9,31 +9,32 @@ def main():

tf.set_translation([4, -4, 0])
frame = dart.gui.osg.InteractiveFrame(
dart.dynamics.Frame.World(), 'interactive frame', tf, 2)
dart.dynamics.Frame.World(), "interactive frame", tf, 2
)
world.addSimpleFrame(frame)

tf.set_translation([-4, 4, 0])
draggable = dart.dynamics.SimpleFrame(frame, 'draggable', tf)
draggable = dart.dynamics.SimpleFrame(frame, "draggable", tf)
draggable.setShape(dart.dynamics.BoxShape([1, 1, 1]))
draggable.getVisualAspect(True).setColor([0.9, 0, 0])
world.addSimpleFrame(draggable)

tf.set_translation([8, 0, 0])
x_marker = dart.dynamics.SimpleFrame(dart.dynamics.Frame.World(), 'X', tf)
x_marker = dart.dynamics.SimpleFrame(dart.dynamics.Frame.World(), "X", tf)
x_shape = dart.dynamics.BoxShape([0.2, 0.2, 0.2])
x_marker.setShape(x_shape)
x_marker.getVisualAspect(True).setColor([0.9, 0, 0])
world.addSimpleFrame(x_marker)

tf.set_translation([0, 8, 0])
y_marker = dart.dynamics.SimpleFrame(dart.dynamics.Frame.World(), 'Y', tf)
y_marker = dart.dynamics.SimpleFrame(dart.dynamics.Frame.World(), "Y", tf)
y_shape = dart.dynamics.BoxShape([0.2, 0.2, 0.2])
y_marker.setShape(y_shape)
y_marker.getVisualAspect(True).setColor([0, 0.9, 0])
world.addSimpleFrame(y_marker)

tf.set_translation([0, 0, 8])
z_marker = dart.dynamics.SimpleFrame(dart.dynamics.Frame.World(), 'Z', tf)
z_marker = dart.dynamics.SimpleFrame(dart.dynamics.Frame.World(), "Z", tf)
z_shape = dart.dynamics.BoxShape([0.2, 0.2, 0.2])
z_marker.setShape(z_shape)
z_marker.getVisualAspect(True).setColor([0, 0, 0.9])
Expand Down
8 changes: 6 additions & 2 deletions python/examples/hello_world/main.py
Original file line number Diff line number Diff line change
Expand Up @@ -9,11 +9,15 @@ def main():
ground = urdfParser.parseSkeleton("dart://sample/urdf/KR5/ground.urdf")
world.addSkeleton(kr5)
world.addSkeleton(ground)
print('Robot {} is loaded'.format(kr5.getName()))
print("Robot {} is loaded".format(kr5.getName()))

for i in range(100):
if i % 10 == 0:
print('[{}] joint position: {}'.format(world.getSimFrames(), kr5.getPositions()))
print(
"[{}] joint position: {}".format(
world.getSimFrames(), kr5.getPositions()
)
)
world.step()


Expand Down
6 changes: 3 additions & 3 deletions python/examples/hello_world_gui/main.py
Original file line number Diff line number Diff line change
Expand Up @@ -51,9 +51,9 @@ def main():
viewer.addAttachment(grid)

viewer.setUpViewInWindow(0, 0, 640, 480)
viewer.setCameraHomePosition([2.0, 1.0, 2.0],
[0.00, 0.00, 0.00],
[-0.24, 0.94, -0.25])
viewer.setCameraHomePosition(
[2.0, 1.0, 2.0], [0.00, 0.00, 0.00], [-0.24, 0.94, -0.25]
)
viewer.run()


Expand Down
19 changes: 12 additions & 7 deletions python/examples/operational_space_control/main.py
Original file line number Diff line number Diff line change
Expand Up @@ -7,20 +7,22 @@ def __init__(self, world, kr5):
super(HelloWorldNode, self).__init__(world)
self.kr5 = kr5
self.dofs = self.kr5.getNumDofs()
self.ee = kr5.getBodyNode('palm')
self.ee = kr5.getBodyNode("palm")
self.Kp = np.eye(3) * 50.0
self.Kd = np.eye(self.dofs) * 5.0
self.ee_offset = [0.05, 0, 0]

tf = self.ee.getTransform()
tf.pretranslate(self.ee_offset)
self.target = dart.dynamics.SimpleFrame(dart.dynamics.Frame.World(), "target", tf)
self.target = dart.dynamics.SimpleFrame(
dart.dynamics.Frame.World(), "target", tf
)

def customPreStep(self):
M = self.kr5.getMassMatrix()

J = self.ee.getLinearJacobian()
Jt = J.transpose();
Jt = J.transpose()
JJt = np.matmul(J, Jt)
kI = 0.0025 * np.eye(3)
invJ = np.matmul(Jt, np.linalg.inv(JJt + kI))
Expand All @@ -30,7 +32,10 @@ def customPreStep(self):
dJdJt = np.matmul(dJ, dJt)
invdJ = np.matmul(dJt, np.linalg.inv(dJdJt + kI))

e = self.target.getTransform().translation() - self.ee.getTransform().translation()
e = (
self.target.getTransform().translation()
- self.ee.getTransform().translation()
)
de = -self.ee.getLinearVelocity()

cg = self.kr5.getCoriolisAndGravityForces()
Expand Down Expand Up @@ -70,9 +75,9 @@ def main():
viewer.addAttachment(grid)

viewer.setUpViewInWindow(0, 0, 640, 480)
viewer.setCameraHomePosition([2.0, 1.0, 2.0],
[0.00, 0.00, 0.00],
[-0.24, 0.94, -0.25])
viewer.setCameraHomePosition(
[2.0, 1.0, 2.0], [0.00, 0.00, 0.00], [-0.24, 0.94, -0.25]
)
viewer.run()


Expand Down
2 changes: 1 addition & 1 deletion python/examples/rigid_chain/main.py
Original file line number Diff line number Diff line change
Expand Up @@ -45,5 +45,5 @@ def main():
viewer.run()


if __name__== "__main__":
if __name__ == "__main__":
main()
3 changes: 1 addition & 2 deletions python/examples/rigid_cubes/main.py
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,6 @@ def main():
world = dart.utils.SkelParser.readWorld("dart://sample/skel/cubes.skel")
world.setGravity([0, -9.81, 0])


viewer = dart.gui.osg.Viewer()
shadow = dart.gui.osg.WorldNode.createDefaultShadowTechnique(viewer)

Expand All @@ -26,5 +25,5 @@ def main():
viewer.run()


if __name__== "__main__":
if __name__ == "__main__":
main()
8 changes: 4 additions & 4 deletions python/examples/rigid_loop/main.py
Original file line number Diff line number Diff line change
Expand Up @@ -13,9 +13,9 @@ def customPreStep(self):


def main():
world = dart.utils.SkelParser.readWorld('dart://sample/skel/chain.skel')
world = dart.utils.SkelParser.readWorld("dart://sample/skel/chain.skel")
world.setGravity([0, -9.81, 0])
world.setTimeStep(1.0/2000)
world.setTimeStep(1.0 / 2000)

chain = world.getSkeleton(0)
for i in range(chain.getNumJoints()):
Expand All @@ -33,8 +33,8 @@ def main():
chain.setPositions(init_pose)

# Create a ball joint contraint
bd1 = chain.getBodyNode('link 6')
bd2 = chain.getBodyNode('link 10')
bd1 = chain.getBodyNode("link 6")
bd2 = chain.getBodyNode("link 10")
bd1.setColor([0, 1, 0])
bd2.setColor([0, 1, 0])

Expand Down
Loading

0 comments on commit 5f43592

Please sign in to comment.