Debian Codegraph @

python/tests/unit/constraint/test_constraint.py

raw
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()