oxmpl-js 0.6.0

JavaScript/WASM Bindings for the OxMPL Library
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73

import oxmpl from 'oxmpl-js';
import { describe, expect, test } from 'vitest';

class CompoundGoal {
  constructor(space, target, radius) {
    this.space = space;
    this.target = target;
    this.radius = radius;
  }
  isSatisfied(state) {
    return this.space.distance(state, this.target) <= this.radius;
  }
  distanceGoal(state) {
    return Math.max(0, this.space.distance(state, this.target) - this.radius);
  }
  sampleGoal() {
    const tRV = this.target.getComponent(0);
    const tSO2 = this.target.getComponent(1);
    const rv = new oxmpl.base.RealVectorState(Array.from(tRV.values));
    const so2 = new oxmpl.base.SO2State(tSO2.value);
    const builder = new oxmpl.base.CompoundStateBuilder();
    builder.addRealVectorState(rv);
    builder.addSO2State(so2);
    return builder.build();
  }
}

describe('RRT Compound Integration Tests', () => {
  test('RRT problem in Compound Space', () => {
    const r2Space = new oxmpl.base.RealVectorStateSpace(2, [-5, 5, -5, 5]);
    const so2Space = new oxmpl.base.SO2StateSpace();
    const spaceBuilder = new oxmpl.base.CompoundStateSpaceBuilder();
    spaceBuilder.addRealVectorStateSpace(r2Space, 1.0);
    spaceBuilder.addSO2StateSpace(so2Space, 0.5);
    const space = spaceBuilder.build();

    const startBuilder = new oxmpl.base.CompoundStateBuilder();
    startBuilder.addRealVectorState(new oxmpl.base.RealVectorState([-2, 0]));
    startBuilder.addSO2State(new oxmpl.base.SO2State(0));
    const startState = startBuilder.build();

    const targetBuilder = new oxmpl.base.CompoundStateBuilder();
    targetBuilder.addRealVectorState(new oxmpl.base.RealVectorState([2, 0]));
    targetBuilder.addSO2State(new oxmpl.base.SO2State(Math.PI));
    const targetState = targetBuilder.build();

    const goalRegion = new CompoundGoal(space, targetState, 0.5);
    const goal = new oxmpl.base.Goal(goalRegion);

    const problemDef = oxmpl.base.ProblemDefinition.fromCompoundState(space, startState, goal);

    const validityCheckerFn = (state) => {
      const r2 = state.getComponent(0);
      const v = Array.from(r2.values);
      if (v[0] >= -0.5 && v[0] <= 0.5 && v[1] >= -2 && v[1] <= 2) return false;
      return true;
    };
    const validityChecker = new oxmpl.base.StateValidityChecker(validityCheckerFn);

    const planner_config = new oxmpl.base.PlannerConfig(0);
    const planner = new oxmpl.geometric.RRT(0.5, 0.1, problemDef, planner_config);
    planner.setup(validityChecker);

    const path = planner.solve(5.0);
    const states = path.getStates();
    expect(states.length).toBeGreaterThan(1);
    expect(space.distance(states[0], startState)).toBeLessThan(1e-9);
    expect(goalRegion.isSatisfied(states[states.length - 1])).toBe(true);
    for (const state of states) {
      expect(validityCheckerFn(state)).toBe(true);
    }
  });
});