Skip to content

Commit

Permalink
examples: fix closed loop example
Browse files Browse the repository at this point in the history
  • Loading branch information
jcarpent committed Dec 23, 2024
1 parent e5556e9 commit e5c1480
Showing 1 changed file with 10 additions and 6 deletions.
16 changes: 10 additions & 6 deletions examples/simulation-closed-kinematic-chains.py
Original file line number Diff line number Diff line change
@@ -1,3 +1,4 @@
import sys
import time

import hppfcl as fcl
Expand Down Expand Up @@ -76,14 +77,9 @@
geom_obj3 = pin.GeometryObject("link_B2", joint3_id, placement_shape_B, shape_link_B)
geom_obj3.meshColor = RED_COLOR
collision_model.addGeometryObject(geom_obj3)

visual_model = collision_model
viz = MeshcatVisualizer(model, collision_model, visual_model)
viz.initViewer(open=True)
viz.loadViewerModel()

q0 = pin.neutral(model)
viz.display(q0)

data = model.createData()
pin.forwardKinematics(model, data, q0)
Expand Down Expand Up @@ -146,7 +142,6 @@
y -= alpha * (-dy + y)

q_sol = (q[:] + np.pi) % np.pi - np.pi
viz.display(q_sol)

# Perform the simulation
q = q_sol.copy()
Expand All @@ -162,6 +157,15 @@
pin.initConstraintDynamics(model, data, [constraint_model])
prox_settings = pin.ProximalSettings(1e-8, mu_sim, 10)

try:
viz = MeshcatVisualizer(model, collision_model, visual_model)
viz.initViewer(open=True)
except ImportError as error:
print(error)
sys.exit(0)
viz.loadViewerModel()
viz.display(q_sol)

while t <= T_sim:
a = pin.constraintDynamics(
model, data, q, v, tau, [constraint_model], [constraint_data], prox_settings
Expand Down

0 comments on commit e5c1480

Please sign in to comment.