Function test_frame_collision

tests/python/test_frames.py:281–359

()

def test_frame_collision():
    import coal

    fr_name1 = "larm_shoulder2_body"
    fr_id1 = model.getFrameId(fr_name1)
    joint_id = model.frames[fr_id1].parentJoint

    fr_name2 = "rleg_elbow_body"
    fr_id2 = model.getFrameId(fr_name2)
    joint_id2 = model.frames[fr_id2].parentJoint

    frame_SE3 = pin.SE3.Random()
    frame_SE3_bis = pin.SE3.Random()
    alpha = np.random.rand()
    beta = np.random.rand()
    gamma = np.random.rand()
    delta = np.random.rand()

    geometry = pin.buildSampleGeometryModelHumanoid(model)
    ig_frame = geometry.addGeometryObject(
        pin.GeometryObject(
            "frame",
            parent_joint=joint_id,
            parent_frame=fr_id1,
            placement=frame_SE3,
            collision_geometry=coal.Capsule(alpha, gamma),
        )
    )

    ig_frame2 = geometry.addGeometryObject(
        pin.GeometryObject(
            "frame2",
            parent_joint=joint_id2,
            parent_frame=fr_id2,
            placement=frame_SE3_bis,
            collision_geometry=coal.Capsule(beta, delta),
        )
    )
    geometry.addCollisionPair(pin.CollisionPair(ig_frame, ig_frame2))
    gdata = geometry.createData()

    space = manifolds.MultibodyConfiguration(model)
    ndx = space.ndx
    x0 = space.neutral()
    d = np.random.randn(space.ndx) * 0.1
    d[6:] = 0.0
    x0 = space.integrate(x0, d)
    u0 = np.zeros(nu)
    q0 = x0[:nq]

    pin.forwardKinematics(model, rdata, q0)
    pin.updateGeometryPlacements(model, rdata, geometry, gdata, q0)
    pin.computeDistance(geometry, gdata, 0)
    norm = gdata.distanceResults[0].min_distance

    fun = aligator.FrameCollisionResidual(ndx, nu, model, geometry, 0)

    fdata = fun.createData()