(length=[0.2], mass=[1.0])
| 20 | |
| 21 | |
| 22 | def setBallsAndHalfSpace(length=[0.2], mass=[1.0]): |
| 23 | assert len(length) == len(mass) or len(length) == 1 or len(mass) == 1 |
| 24 | N = max(len(length), len(mass)) |
| 25 | if len(length) == 1: |
| 26 | length = length * N |
| 27 | if len(mass) == 1: |
| 28 | mass = mass * N |
| 29 | model = pin.Model() |
| 30 | geom_model = pin.GeometryModel() |
| 31 | |
| 32 | # create plane for floor |
| 33 | n = np.array([0.0, 0.0, 1]) |
| 34 | plane_shape = Halfspace(n, 0) |
| 35 | T = pin.SE3(np.eye(3), np.zeros(3)) |
| 36 | plane = pin.GeometryObject("plane", 0, 0, T, plane_shape) |
| 37 | plane.meshColor = np.array([0.5, 0.5, 0.5, 1.0]) |
| 38 | plane_id = geom_model.addGeometryObject(plane) |
| 39 | ball_ids = [] |
| 40 | for n_ball in range(N): |
| 41 | a = length[n_ball] |
| 42 | m = mass[n_ball] |
| 43 | freeflyer = pin.JointModelFreeFlyer() |
| 44 | joint = model.addJoint(0, freeflyer, pin.SE3.Identity(), "ball_" + str(n_ball)) |
| 45 | model.appendBodyToJoint( |
| 46 | joint, pin.Inertia.FromSphere(m, a / 2), pin.SE3.Identity() |
| 47 | ) |
| 48 | ball_shape = Sphere(a / 2) |
| 49 | geom_ball = pin.GeometryObject( |
| 50 | "ball_" + str(n_ball), joint, joint, pin.SE3.Identity(), ball_shape |
| 51 | ) |
| 52 | geom_ball.meshColor = np.array([1.0, 0.2, 0.2, 1.0]) |
| 53 | ball_id = geom_model.addGeometryObject(geom_ball) |
| 54 | for id in ball_ids: |
| 55 | col_pair = pin.CollisionPair(id, ball_id) |
| 56 | geom_model.addCollisionPair(col_pair) |
| 57 | ball_ids += [ball_id] |
| 58 | col_pair = pin.CollisionPair(plane_id, ball_id) |
| 59 | geom_model.addCollisionPair(col_pair) |
| 60 | |
| 61 | model.qref = pin.neutral(model) |
| 62 | model.qinit = model.qref.copy() |
| 63 | for n_ball in range(N): |
| 64 | model.qinit[7 * n_ball] += a |
| 65 | model.qinit[7 * n_ball + 2] += 0.1 |
| 66 | |
| 67 | data = model.createData() |
| 68 | geom_data = geom_model.createData() |
| 69 | for req in geom_data.collisionRequests: |
| 70 | req.security_margin = 1e-3 |
| 71 | return model, data, geom_model, geom_data |
| 72 | |
| 73 | |
| 74 | def test_init(): |
no outgoing calls
no test coverage detected