Variables
	a

float	alpha = 1.0

int	base_joint_id = 0

	collision_model = pin.GeometryModel()

	constraint1_joint1_placement = pin.SE3.Identity()

	constraint1_joint2_placement = pin.SE3.Identity()

	constraint_data = constraint_model.createData()

	constraint_dim = constraint_model.size()

	constraint_model

	constraint_value = constraint_data.c1Mc2.translation

	data = model.createData()

	dq = dz[constraint_dim:]

int	dt = 5e-3

	dual_feas = np.linalg.norm(J.T.dot(constraint_value + y), np.inf)

	dy = dz[:constraint_dim]

	dz = kkt_constraint.solve(rhs)

int	eps = 1e-10

	geom_obj0

	geom_obj1 = pin.GeometryObject("link_B1", joint1_id, placement_shape_B, shape_link_B)

	geom_obj2 = pin.GeometryObject("link_A2", joint2_id, placement_shape_A, shape_link_A)

	geom_obj3 = pin.GeometryObject("link_B2", joint3_id, placement_shape_B, shape_link_B)

float	height = 0.1

	inertia_link_A = pin.Inertia.FromBox(mass_link_A, length_link_A, width, height)

	inertia_link_B = pin.Inertia.FromBox(mass_link_B, length_link_B, width, height)

	J

	joint1_id

	joint1_placement = pin.SE3.Identity()

	joint2_id = model.addJoint(joint1_id, pin.JointModelRY(), joint2_placement, "link_A2")

	joint2_placement = pin.SE3.Identity()

	joint3_id = model.addJoint(joint2_id, pin.JointModelRY(), joint3_placement, "link_B2")

	joint3_placement = pin.SE3.Identity()

	Kd

	kkt_constraint = pin.ContactCholeskyDecomposition(model, [constraint_model])

float	length_link_A = 1.0

float	length_link_B = 0.6

	M

float	mass_link_A = 10.0

float	mass_link_B = 5.0

	meshColor

	model = pin.Model()

int	mu = 1e-4

int	mu_sim = 1e-10

int	N = 100

	open

	placement_center_link_A = pin.SE3.Identity()

	placement_center_link_B = pin.SE3.Identity()

	placement_shape_A = placement_center_link_A.copy()

	placement_shape_B = placement_center_link_B.copy()

	primal_feas = np.linalg.norm(constraint_value, np.inf)

	prox_settings = pin.ProximalSettings(1e-8, mu_sim, 10)

	q = q0.copy()

	q0 = pin.neutral(model)

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

float	radius = 0.05

	RED_COLOR = np.array([1.0, 0.0, 0.0, 1.0])

int	rho = 1e-10

	rhs = np.concatenate([-constraint_value - y * mu, np.zeros(model.nv)])

	rotation

	shape_link_A = fcl.Capsule(radius, length_link_A)

	shape_link_B = fcl.Capsule(radius, length_link_B)

int	t = 0

int	T_sim = 100000

	tau = np.zeros((model.nv))

	translation

	v = np.zeros((model.nv))

	visual_model = collision_model

	viz = MeshcatVisualizer(model, collision_model, visual_model)

	WHITE_COLOR = np.array([1.0, 1.0, 1.0, 1.0])

float	width = 0.01

	y = np.ones((constraint_dim))

Variable Documentation

◆ a

simulation-closed-kinematic-chains.a

Initial value:

 =  pin.constraintDynamics(
         model, data, q, v, tau, [constraint_model], [constraint_data], prox_settings
     )

Definition at line 167 of file simulation-closed-kinematic-chains.py.

◆ alpha

float simulation-closed-kinematic-chains.alpha = 1.0

Definition at line 144 of file simulation-closed-kinematic-chains.py.

◆ base_joint_id

int simulation-closed-kinematic-chains.base_joint_id = 0

Definition at line 42 of file simulation-closed-kinematic-chains.py.

◆ collision_model

simulation-closed-kinematic-chains.collision_model = pin.GeometryModel()

Definition at line 37 of file simulation-closed-kinematic-chains.py.

◆ constraint1_joint1_placement

simulation-closed-kinematic-chains.constraint1_joint1_placement = pin.SE3.Identity()

Definition at line 92 of file simulation-closed-kinematic-chains.py.

◆ constraint1_joint2_placement

simulation-closed-kinematic-chains.constraint1_joint2_placement = pin.SE3.Identity()

Definition at line 95 of file simulation-closed-kinematic-chains.py.

◆ constraint_data

simulation-closed-kinematic-chains.constraint_data = constraint_model.createData()

Definition at line 106 of file simulation-closed-kinematic-chains.py.

◆ constraint_dim

simulation-closed-kinematic-chains.constraint_dim = constraint_model.size()

Definition at line 107 of file simulation-closed-kinematic-chains.py.

◆ constraint_model

simulation-closed-kinematic-chains.constraint_model

Initial value:

 =  pin.RigidConstraintModel(
     pin.ContactType.CONTACT_3D,
     model,
     joint3_id,
     constraint1_joint1_placement,
     base_joint_id,
     constraint1_joint2_placement,
 )

Definition at line 98 of file simulation-closed-kinematic-chains.py.

◆ constraint_value

simulation-closed-kinematic-chains.constraint_value = constraint_data.c1Mc2.translation

Definition at line 123 of file simulation-closed-kinematic-chains.py.

◆ data

simulation-closed-kinematic-chains.data = model.createData()

Definition at line 88 of file simulation-closed-kinematic-chains.py.

◆ dq

simulation-closed-kinematic-chains.dq = dz[constraint_dim:]

Definition at line 142 of file simulation-closed-kinematic-chains.py.

◆ dt

int simulation-closed-kinematic-chains.dt = 5e-3

Definition at line 155 of file simulation-closed-kinematic-chains.py.

◆ dual_feas

simulation-closed-kinematic-chains.dual_feas = np.linalg.norm(J.T.dot(constraint_value + y), np.inf)

Definition at line 133 of file simulation-closed-kinematic-chains.py.

◆ dy

simulation-closed-kinematic-chains.dy = dz[:constraint_dim]

Definition at line 141 of file simulation-closed-kinematic-chains.py.

◆ dz

simulation-closed-kinematic-chains.dz = kkt_constraint.solve(rhs)

Definition at line 140 of file simulation-closed-kinematic-chains.py.

◆ eps

int simulation-closed-kinematic-chains.eps = 1e-10

Definition at line 118 of file simulation-closed-kinematic-chains.py.

◆ geom_obj0

simulation-closed-kinematic-chains.geom_obj0

Initial value:

 =  pin.GeometryObject(
     "link_A1",
     base_joint_id,
     pin.SE3(
         pin.Quaternion.FromTwoVectors(pin.ZAxis, pin.XAxis).matrix(), np.zeros((3))
     ),
     shape_link_A,
 )

Definition at line 43 of file simulation-closed-kinematic-chains.py.

◆ geom_obj1

simulation-closed-kinematic-chains.geom_obj1 = pin.GeometryObject("link_B1", joint1_id, placement_shape_B, shape_link_B)

Definition at line 60 of file simulation-closed-kinematic-chains.py.

◆ geom_obj2

simulation-closed-kinematic-chains.geom_obj2 = pin.GeometryObject("link_A2", joint2_id, placement_shape_A, shape_link_A)

Definition at line 68 of file simulation-closed-kinematic-chains.py.

◆ geom_obj3

simulation-closed-kinematic-chains.geom_obj3 = pin.GeometryObject("link_B2", joint3_id, placement_shape_B, shape_link_B)

Definition at line 76 of file simulation-closed-kinematic-chains.py.

◆ height

float simulation-closed-kinematic-chains.height = 0.1

Definition at line 8 of file simulation-closed-kinematic-chains.py.

◆ inertia_link_A

simulation-closed-kinematic-chains.inertia_link_A = pin.Inertia.FromBox(mass_link_A, length_link_A, width, height)

Definition at line 20 of file simulation-closed-kinematic-chains.py.

◆ inertia_link_B

simulation-closed-kinematic-chains.inertia_link_B = pin.Inertia.FromBox(mass_link_B, length_link_B, width, height)

Definition at line 28 of file simulation-closed-kinematic-chains.py.

◆ J

simulation-closed-kinematic-chains.J

Initial value:

 =  pin.getFrameJacobian(
         model,
         data,
         constraint_model.joint1_id,
         constraint_model.joint1_placement,
         constraint_model.reference_frame,
     )[:3, :]

Definition at line 125 of file simulation-closed-kinematic-chains.py.

◆ joint1_id

simulation-closed-kinematic-chains.joint1_id

Initial value:

 =  model.addJoint(
     base_joint_id, pin.JointModelRY(), joint1_placement, "link_B1"
 )

Definition at line 56 of file simulation-closed-kinematic-chains.py.

◆ joint1_placement

simulation-closed-kinematic-chains.joint1_placement = pin.SE3.Identity()

Definition at line 54 of file simulation-closed-kinematic-chains.py.

◆ joint2_id

simulation-closed-kinematic-chains.joint2_id = model.addJoint(joint1_id, pin.JointModelRY(), joint2_placement, "link_A2")

Definition at line 66 of file simulation-closed-kinematic-chains.py.

◆ joint2_placement

simulation-closed-kinematic-chains.joint2_placement = pin.SE3.Identity()

Definition at line 64 of file simulation-closed-kinematic-chains.py.

◆ joint3_id

simulation-closed-kinematic-chains.joint3_id = model.addJoint(joint2_id, pin.JointModelRY(), joint3_placement, "link_B2")

Definition at line 74 of file simulation-closed-kinematic-chains.py.

◆ joint3_placement

simulation-closed-kinematic-chains.joint3_placement = pin.SE3.Identity()

Definition at line 72 of file simulation-closed-kinematic-chains.py.

◆ Kd

simulation-closed-kinematic-chains.Kd

Definition at line 161 of file simulation-closed-kinematic-chains.py.

◆ kkt_constraint

simulation-closed-kinematic-chains.kkt_constraint = pin.ContactCholeskyDecomposition(model, [constraint_model])

Definition at line 117 of file simulation-closed-kinematic-chains.py.

◆ length_link_A

float simulation-closed-kinematic-chains.length_link_A = 1.0

Definition at line 13 of file simulation-closed-kinematic-chains.py.

◆ length_link_B

float simulation-closed-kinematic-chains.length_link_B = 0.6

Definition at line 17 of file simulation-closed-kinematic-chains.py.

◆ M

simulation-closed-kinematic-chains.M

Definition at line 116 of file simulation-closed-kinematic-chains.py.

◆ mass_link_A

float simulation-closed-kinematic-chains.mass_link_A = 10.0

Definition at line 12 of file simulation-closed-kinematic-chains.py.

◆ mass_link_B

float simulation-closed-kinematic-chains.mass_link_B = 5.0

Definition at line 16 of file simulation-closed-kinematic-chains.py.

◆ meshColor

simulation-closed-kinematic-chains.meshColor

Definition at line 51 of file simulation-closed-kinematic-chains.py.

◆ model

simulation-closed-kinematic-chains.model = pin.Model()

Definition at line 36 of file simulation-closed-kinematic-chains.py.

◆ mu

int simulation-closed-kinematic-chains.mu = 1e-4

Definition at line 111 of file simulation-closed-kinematic-chains.py.

◆ mu_sim

int simulation-closed-kinematic-chains.mu_sim = 1e-10

Definition at line 159 of file simulation-closed-kinematic-chains.py.

◆ N

int simulation-closed-kinematic-chains.N = 100

Definition at line 119 of file simulation-closed-kinematic-chains.py.

◆ open

simulation-closed-kinematic-chains.open

Definition at line 82 of file simulation-closed-kinematic-chains.py.

◆ placement_center_link_A

simulation-closed-kinematic-chains.placement_center_link_A = pin.SE3.Identity()

Definition at line 21 of file simulation-closed-kinematic-chains.py.

◆ placement_center_link_B

simulation-closed-kinematic-chains.placement_center_link_B = pin.SE3.Identity()

Definition at line 29 of file simulation-closed-kinematic-chains.py.

◆ placement_shape_A

simulation-closed-kinematic-chains.placement_shape_A = placement_center_link_A.copy()

Definition at line 23 of file simulation-closed-kinematic-chains.py.

◆ placement_shape_B

simulation-closed-kinematic-chains.placement_shape_B = placement_center_link_B.copy()

Definition at line 31 of file simulation-closed-kinematic-chains.py.

◆ primal_feas

simulation-closed-kinematic-chains.primal_feas = np.linalg.norm(constraint_value, np.inf)

Definition at line 132 of file simulation-closed-kinematic-chains.py.

◆ prox_settings

simulation-closed-kinematic-chains.prox_settings = pin.ProximalSettings(1e-8, mu_sim, 10)

Definition at line 163 of file simulation-closed-kinematic-chains.py.

◆ q

tuple simulation-closed-kinematic-chains.q = q0.copy()

Definition at line 113 of file simulation-closed-kinematic-chains.py.

◆ q0

simulation-closed-kinematic-chains.q0 = pin.neutral(model)

Definition at line 85 of file simulation-closed-kinematic-chains.py.

◆ q_sol

tuple simulation-closed-kinematic-chains.q_sol = (q[:] + np.pi) % np.pi - np.pi

Definition at line 148 of file simulation-closed-kinematic-chains.py.

◆ radius

float simulation-closed-kinematic-chains.radius = 0.05

Definition at line 10 of file simulation-closed-kinematic-chains.py.

◆ RED_COLOR

simulation-closed-kinematic-chains.RED_COLOR = np.array([1.0, 0.0, 0.0, 1.0])

Definition at line 39 of file simulation-closed-kinematic-chains.py.

◆ rho

int simulation-closed-kinematic-chains.rho = 1e-10

Definition at line 110 of file simulation-closed-kinematic-chains.py.

◆ rhs

simulation-closed-kinematic-chains.rhs = np.concatenate([-constraint_value - y * mu, np.zeros(model.nv)])

Definition at line 138 of file simulation-closed-kinematic-chains.py.

◆ rotation

simulation-closed-kinematic-chains.rotation

Definition at line 24 of file simulation-closed-kinematic-chains.py.

◆ shape_link_A

simulation-closed-kinematic-chains.shape_link_A = fcl.Capsule(radius, length_link_A)

Definition at line 14 of file simulation-closed-kinematic-chains.py.

◆ shape_link_B

simulation-closed-kinematic-chains.shape_link_B = fcl.Capsule(radius, length_link_B)

Definition at line 18 of file simulation-closed-kinematic-chains.py.

◆ t

int simulation-closed-kinematic-chains.t = 0

Definition at line 158 of file simulation-closed-kinematic-chains.py.

◆ T_sim

int simulation-closed-kinematic-chains.T_sim = 100000

Definition at line 157 of file simulation-closed-kinematic-chains.py.

◆ tau

simulation-closed-kinematic-chains.tau = np.zeros((model.nv))

Definition at line 154 of file simulation-closed-kinematic-chains.py.

◆ translation

simulation-closed-kinematic-chains.translation

Definition at line 22 of file simulation-closed-kinematic-chains.py.

◆ v

simulation-closed-kinematic-chains.v = np.zeros((model.nv))

Definition at line 153 of file simulation-closed-kinematic-chains.py.

◆ visual_model

simulation-closed-kinematic-chains.visual_model = collision_model

Definition at line 80 of file simulation-closed-kinematic-chains.py.

◆ viz

simulation-closed-kinematic-chains.viz = MeshcatVisualizer(model, collision_model, visual_model)

Definition at line 81 of file simulation-closed-kinematic-chains.py.

◆ WHITE_COLOR

simulation-closed-kinematic-chains.WHITE_COLOR = np.array([1.0, 1.0, 1.0, 1.0])

Definition at line 40 of file simulation-closed-kinematic-chains.py.

◆ width

float simulation-closed-kinematic-chains.width = 0.01

Definition at line 9 of file simulation-closed-kinematic-chains.py.

◆ y

simulation-closed-kinematic-chains.y = np.ones((constraint_dim))

Definition at line 115 of file simulation-closed-kinematic-chains.py.

Variables

Variable Documentation

◆ a

◆ alpha

◆ base_joint_id

◆ collision_model

◆ constraint1_joint1_placement

◆ constraint1_joint2_placement

◆ constraint_data

◆ constraint_dim

◆ constraint_model

◆ constraint_value

◆ data

◆ dq

◆ dt

◆ dual_feas

◆ dy

◆ dz

◆ eps

◆ geom_obj0

◆ geom_obj1

◆ geom_obj2

◆ geom_obj3

◆ height

◆ inertia_link_A

◆ inertia_link_B

◆ J

◆ joint1_id

◆ joint1_placement

◆ joint2_id

◆ joint2_placement

◆ joint3_id

◆ joint3_placement

◆ Kd

◆ kkt_constraint

◆ length_link_A

◆ length_link_B

◆ M

◆ mass_link_A

◆ mass_link_B

◆ meshColor

◆ model

◆ mu

◆ mu_sim

◆ N

◆ open

◆ placement_center_link_A

◆ placement_center_link_B

◆ placement_shape_A

◆ placement_shape_B

◆ primal_feas

◆ prox_settings

◆ q

◆ q0

◆ q_sol

◆ radius

◆ RED_COLOR

◆ rho

◆ rhs

◆ rotation

◆ shape_link_A

◆ shape_link_B

◆ t

◆ T_sim

◆ tau

◆ translation

◆ v

◆ visual_model

◆ viz

◆ WHITE_COLOR

◆ width

◆ y