| 1 |
import platform |
| 2 |
import pytest |
| 3 |
import dartpy as dart |
| 4 |
import numpy as np |
| 5 |
|
| 6 |
|
| 7 |
def test_ball_joint_constraint(): |
| 8 |
world = dart.utils.SkelParser.readWorld('dart://sample/skel/chain.skel') |
| 9 |
world.setGravity([0, -9.81, 0]) |
| 10 |
world.setTimeStep(1.0/2000) |
| 11 |
|
| 12 |
# Set joint damping |
| 13 |
chain = world.getSkeleton(0) |
| 14 |
for i in range(chain.getNumJoints()): |
| 15 |
joint = chain.getJoint(i) |
| 16 |
for j in range(joint.getNumDofs()): |
| 17 |
joint.setDampingCoefficient(j, 0.01) |
| 18 |
|
| 19 |
# Create a ball joint contraint |
| 20 |
bd1 = chain.getBodyNode('link 6') |
| 21 |
bd2 = chain.getBodyNode('link 10') |
| 22 |
offset1 = [0, 0.025, 0] |
| 23 |
joint_pos = bd1.getTransform().multiply(offset1) |
| 24 |
offset2 = bd2.getTransform().inverse().multiply(joint_pos) |
| 25 |
constraint = dart.constraint.BallJointConstraint(bd1, bd2, joint_pos) |
| 26 |
assert constraint.getType() == dart.constraint.BallJointConstraint.getStaticType() |
| 27 |
|
| 28 |
# Add ball joint constraint to the constraint solver |
| 29 |
constraint_solver = world.getConstraintSolver() |
| 30 |
constraint_solver.addConstraint(constraint) |
| 31 |
|
| 32 |
# Check if the ball joint constraint is being satisfied |
| 33 |
for _ in range(100): |
| 34 |
world.step() |
| 35 |
pos1 = bd1.getTransform().multiply(offset1) |
| 36 |
pos2 = bd2.getTransform().multiply(offset2) |
| 37 |
assert np.isclose(pos1, pos2).all() |
| 38 |
|
| 39 |
|
| 40 |
if __name__ == "__main__": |
| 41 |
pytest.main() |