Python constraints¶
Seven constraints written in Python, switched on one after another in a live loop.
python_constraints_example, tesseract_robotics 0.35.0.9.dev692
An ABB IRB 2400 rides a rail, with a grinder on its flange. The tool tip rests on a mark on a
workpiece. One TrajOpt problem is solved per frame: a single waypoint, warm-started from the
frame before, with a squared stay-close cost and whichever rules of
trajopt_ifopt.python_constraints
are on. Each rule is a few lines of Python; the SQP that satisfies them is TrajOpt's, in C++.
What you are watching¶
| Act | Rules on | What happens |
|---|---|---|
| 1 | none | The rail moves and the tool goes with it. |
| 2 | WorldHold |
The tip stays on the mark while the rail moves. The tool axis wanders. |
| 3 | + HoldAxis 0.5° |
The tool axis locks onto vertical. |
| 4 | + WatchTheTool 3° |
A camera on a mast on the carriage turns to keep the tip in its cone. It stays on from here. |
| 5 | PointsDown 20° + ElbowUp |
The rail becomes the solver's seventh axis. A plane rises under the elbow, and the carriage drives to where the elbow clears it. |
| 6 | + StayDexterous |
Manipulability stays above a rising bound. |
| 7 | + Clearance |
A ball drives in. The forearm keeps 15 cm from it, and the tip stays put. |
| 8 | all seven | The mark moves, and the tip traces a circle on the workpiece. |
In acts 1 to 4 something else moves the rail: its position is measured, and the arm and the camera solve in the carriage's frame. From act 5 the rail is a variable like any joint.
Every bound that moves ramps from the live value, so nothing jumps when a rule comes on.
Reading the scene¶
- The panel, on the plane behind the robot, has a ruler per rule. The ball on a ruler is the live value. The bar is the bound, and it is there only while the rule is on.
- The last ruler is the solve. Its ball is what the frame's solve took, and its bar is the frame the loop has at 30 Hz, 33 ms.
- The glyphs show each rule in the cell: the mark, the
HoldAxisline, thePointsDowncone, theElbowUpplane, the camera's cone and optical axis, and the shell around the ball that the forearm stays out of. - The caption names the act, and says how many rules are on and what a frame's solve took, as the act's median.
The captions, the gauges' markers, the mark and the glyphs are links of the environment on prismatic joints, posed by the same trajectory as the robot. That is how a recording, which can animate link poses and nothing else, shows a bound appear, move and disappear.
The rules of one frame¶
def rules(
inputs: Inputs, var: ti.Var, group: JointGroup, robot: Robot, base_pose: Isometry3d
) -> list[PyConstraint]:
"""The constraint sets of one frame: the rules `inputs` has on, at its bounds."""
sets: list[PyConstraint] = []
if inputs.hold:
sets.append(WorldHold(var, group, TCP, inputs.mark, base_pose, tol=0.0, coeff=RULE_COEFF))
if inputs.axis_deg is not None:
sets.append(HoldAxis(var, group, TCP, DOWN, np.radians(inputs.axis_deg), coeff=RULE_COEFF))
if inputs.cone_deg is not None:
sets.append(PointsDown(var, group, TCP, np.radians(inputs.cone_deg), coeff=RULE_COEFF))
if inputs.elbow_m is not None:
sets.append(ElbowUp(var, group, ELBOW, WORLD, inputs.elbow_m, coeff=RULE_COEFF))
if inputs.view_deg is not None:
sets.append(
WatchTheTool(var, group, CAMERA, TCP, np.radians(inputs.view_deg), coeff=RULE_COEFF)
)
if inputs.dexterity is not None:
sets.append(StayDexterous(var, group, TCP, inputs.dexterity, coeff=RULE_COEFF))
if inputs.clearance:
sets.append(
Clearance(
var,
robot.env,
[joint_id.name() for joint_id in group.getJointIds()],
FOREARM,
CLEARANCE_MIN_M,
CLEARANCE_MAX_M,
margin=CLEARANCE_MARGIN_M,
coeff=RULE_COEFF,
)
)
if inputs.follow_m is not None and inputs.rail_m is None:
sets.append(CarriageFollows(var, group, TCP, inputs.follow_m, coeff=RULE_COEFF))
return sets
The loop¶
LiveLoop.step solves one frame. run plays the acts through it.
class LiveLoop:
"""The cell and its state between frames: `step` solves one frame from the last."""
def __init__(self) -> None:
self.robot = build_cell()
env = self.robot.env
arm_joints = [joint_id.name() for joint_id in env.getGroupJointIds("manipulator")]
# A joint group keeps the other joints where they are when it is made. With the rail
# at zero, the first group works in the carriage's frame: the rail is someone else's.
env.setState({RAIL_JOINT: 0.0, OBSTACLE_JOINT: BALL_PARKED_M})
self._arm_names = [*arm_joints, *CAMERA_JOINTS]
self._arm = env.getJointGroup("arm_and_camera", self._arm_names)
# The second has the rail too: the solver's seventh axis.
self._cell_names = [RAIL_JOINT, *self._arm_names]
self._cell = env.getJointGroup("rail_arm_and_camera", self._cell_names)
self._arm_limits = ti.toBounds(self._arm.getLimits().joint_limits)
self._cell_limits = ti.toBounds(self._cell.getLimits().joint_limits)
# Where the tip rests at the start [m]: the mark, until an act moves it.
start = np.array([*START, 0.0, 0.0])
self.target = np.array(self._arm.calcFwdKin(start)[TCP].translation)
self.joint_names = [*self._cell_names, OBSTACLE_JOINT, *INSTRUMENT_JOINTS]
# Measures the forearm's clearance for the gauge, whether or not the rule is on.
self._probe_nodes = ti.createNodesVariables(
"probe", self._cell_names, [np.zeros(len(self._cell_names))], self._cell_limits
)
self._probe = Clearance(
self._probe_nodes.getNodes()[0].getVar("joints"),
env,
self._cell_names,
FOREARM,
0.0,
CLEARANCE_MAX_M,
margin=CLEARANCE_MARGIN_M,
)
self.q = np.array([0.0, *start]) # the rail, the arm, the camera's pan and tilt
self.live = self._measure(self.target, 0.0)
def _measure(self, mark: np.ndarray, solve_ms: float) -> dict[str, float]:
measured = measure(self._cell, self.q, mark, float(self._probe.f(self.q)[0]))
return {**measured, "solve_ms": solve_ms}
def step(self, inputs: Inputs) -> Frame:
"""Solve one frame: a single waypoint, warm-started from the frame before."""
env = self.robot.env
if inputs.rail_m is not None:
# The rail is measured, not commanded: the arm solves in its own base frame.
env.setState({RAIL_JOINT: inputs.rail_m, OBSTACLE_JOINT: inputs.ball_m})
group, names, limits = self._arm, self._arm_names, self._arm_limits
seed = self.q[1:]
base_pose = _isometry((0.0, inputs.rail_m, 0.0))
else:
env.setState({OBSTACLE_JOINT: inputs.ball_m})
group, names, limits = self._cell, self._cell_names, self._cell_limits
seed = self.q
base_pose = Isometry3d.Identity()
nodes = ti.createNodesVariables("frame", names, [seed], limits)
var = nodes.getNodes()[0].getVar("joints")
problem = tsqp.TrajOptQPProblem(nodes)
problem.addCostSet(
ti.JointPosConstraint(seed, var, np.ones(len(seed)), "stay_close"),
tsqp.CostPenaltyType.SQUARED,
)
sets = rules(inputs, var, group, self.robot, base_pose)
for rule in sets:
problem.addConstraintSet(rule)
problem.setup()
started = time.perf_counter()
if sets:
tsqp.TrustRegionSQPSolver(tsqp.OSQPEigenSolver()).solve(problem)
solve_ms = (time.perf_counter() - started) * MS_PER_S
solved = np.asarray(var.value())
self.q = solved.copy() if inputs.rail_m is None else np.array([inputs.rail_m, *solved])
self.live = self._measure(inputs.mark, solve_ms)
return Frame(self.q, self.live, solve_ms, _violation(sets))
def run(acts: tuple[Act, ...] = ACTS) -> dict:
"""Play the acts: one solve per frame.
Returns:
`robot`, the recorded `joint_names`, an `ActResult` per act in `acts`, the `target` the
tip holds [m], and `success`: no rule was violated by more than 1e-3 of its unit.
"""
loop = LiveLoop()
results = []
for index, act in enumerate(acts):
result = ActResult(act)
began = dict(loop.live)
frames = int(round(act.seconds * FRAME_HZ))
for frame in range(frames):
u = frame / (frames - 1)
ball = BALL_PARKED_M
if act.ball_m is not None:
ball += (act.ball_m - BALL_PARKED_M) * np.sin(np.pi * u) ** 2
mark = loop.target
if act.trace_m is not None:
turn = 2.0 * np.pi * u
mark = mark + act.trace_m * np.array([np.cos(turn) - 1.0, np.sin(turn), 0.0])
inputs = Inputs(
rail_m=None if act.rail is None else float(act.rail(u)),
ball_m=float(ball),
mark=mark,
hold=act.hold,
axis_deg=act.axis_deg,
cone_deg=act.cone_deg,
elbow_m=_ramped(act.elbow_m, began["elbow_m"], u, lower_bound=True),
view_deg=_ramped(act.view_deg, began["view_deg"], u, lower_bound=False),
dexterity=_ramped(act.dexterity, began["dexterity"], u, lower_bound=True),
clearance=act.clearance,
)
solved = loop.step(inputs)
result.solve_ms.append(solved.solve_ms)
result.live.append(solved.live)
result.worst_violation = max(result.worst_violation, solved.violation)
result.rows.append(
[
*solved.q,
inputs.ball_m,
*instrument_values(
inputs, solved.live, loop.target, caption=index, tracing=act.trace_m
),
frame / FRAME_HZ,
]
)
results.append(result)
return {
"robot": loop.robot,
"joint_names": loop.joint_names,
"acts": results,
"target": loop.target,
"success": all(result.worst_violation < VIOLATION_TOLERANCE for result in results),
}
The acts¶
@dataclass(frozen=True)
class Act:
"""One stretch of the loop: which rules are on, and what moves the rail and the ball."""
caption: str
seconds: float
# The rail position over the act's 0..1 [m] while something else drives it; None once the
# rail is the solver's seventh axis.
rail: Callable[[float], float] | None = None
# The ball's closest approach along its line [m]; it drives in from the far side and back.
ball_m: float | None = None
hold: bool = True
axis_deg: float | None = None
cone_deg: float | None = None
# Bounds that ramp from the live value to these targets.
elbow_m: float | None = None
view_deg: float | None = None
dexterity: float | None = None
clearance: bool = False
# Radius of the circle the mark moves on [m], once round in the act; None: it stays put.
trace_m: float | None = None
def _swing(cycles: float, phase: float = 0.0) -> Callable[[float], float]:
return lambda u: RAIL_SWING_M * np.sin(2.0 * np.pi * cycles * u + phase)
ACTS = (
Act("No rule\nThe rail moves and the tool goes with it", 3.0, rail=_swing(0.5), hold=False),
Act(
"WorldHold\nThe tip stays on the mark; the tool axis wanders",
4.5,
rail=_swing(0.75, np.pi),
),
Act(
"+ HoldAxis 0.5°\nThe tool locks vertical",
4.0,
rail=_swing(0.5, 0.5 * np.pi),
axis_deg=0.5,
),
Act(
"+ WatchTheTool 3°\nThe camera on the mast turns to keep the tip in its cone",
5.0,
rail=_swing(1.0, -0.5 * np.pi),
axis_deg=0.5,
view_deg=VIEW_DEG,
),
Act(
"PointsDown 20° + ElbowUp, and the rail is now the solver's\n"
"The carriage drives to where the elbow clears the plane",
4.0,
cone_deg=20.0,
view_deg=VIEW_DEG,
elbow_m=1.25,
),
Act(
"+ StayDexterous\nManipulability stays above its bound",
4.0,
cone_deg=20.0,
view_deg=VIEW_DEG,
dexterity=1.25,
),
Act(
"+ Clearance\nThe forearm keeps 15 cm from the ball",
6.0,
cone_deg=20.0,
view_deg=VIEW_DEG,
ball_m=-0.10,
clearance=True,
),
Act(
"All seven at once\nThe mark moves, and the tip traces the circle",
7.0,
ball_m=-0.45,
axis_deg=20.0,
cone_deg=20.0,
elbow_m=1.22,
view_deg=VIEW_DEG,
dexterity=0.8,
clearance=True,
trace_m=0.12,
),
)
The cell¶
The tool, the rail, the workpiece, the camera and the ball are added to tesseract's
abb_irb2400_on_positioner scene. Their meshes carry a MeshMaterial with metallic and
roughness values, which the viewer shades with.
def build_cell() -> Robot:
"""The IRB 2400 on its rail, with a tool, a workpiece, a forearm camera and a ball."""
locator = GeneralResourceLocator(paths=[get_tesseract_resource_path()])
robot = Robot.from_tesseract_support("abb_irb2400_on_positioner", locator)
# The tool: an adapter plate, then the grinder with its base on the plate and its axis
# along the flange normal. `tcp` is its tip.
grinder_resource = locator.locateResource(GRINDER_MESH)
if grinder_resource is None:
raise ResourceNotFoundError(f"Could not locate the grinder mesh: {GRINDER_MESH}")
grinder = createMeshFromResource(grinder_resource, np.full(3, GRINDER_SCALE), True, True)
_add_link(
robot,
"adapter",
"tool0",
_isometry((0, 0, ADAPTER_M / 2)),
(_visual(_cylinder(ADAPTER.radius_m, ADAPTER.height_m, ANODISED)),),
)
_add_link(
robot,
"grinder",
"tool0",
_isometry((0, 0, ADAPTER_M + GRINDER_BASE_M * GRINDER_SCALE)),
tuple(
_visual(
_finished(
np.asarray(part.getVertices()),
np.asarray(part.getFaces()).reshape(-1, 4)[:, 1:],
TOOL_STEEL,
)
)
for part in grinder
),
)
tool_length = ADAPTER_M + (GRINDER_BASE_M + GRINDER_TIP_M) * GRINDER_SCALE
_add_link(robot, TCP, "tool0", _isometry((0, 0, tool_length)))
# The rail and the carriage the robot stands on.
_add_link(
robot,
"rail",
WORLD,
_isometry(),
(
_visual(
_box(*RAIL_BEAM_M, RAIL_STEEL), _isometry((RAIL_BEAM_X_M, 0, RAIL_BEAM_M[2] / 2))
),
_visual(
_box(*RAIL_BEAM_M, RAIL_STEEL), _isometry((-RAIL_BEAM_X_M, 0, RAIL_BEAM_M[2] / 2))
),
),
)
_add_link(
robot,
"carriage",
"positioner_tool0",
_isometry(),
(_visual(_box(*CARRIAGE_M, ALUMINIUM), _isometry((0, 0, CARRIAGE_M[2] / 2))),),
)
# The workpiece: a plate on a pedestal, its top where the tool tip is at the start.
tip = robot.env.getJointGroup("manipulator").calcFwdKin(START)[TCP].translation
top = float(tip[2])
_add_link(
robot,
"workpiece",
WORLD,
_isometry((tip[0], tip[1], 0)),
(
_visual(
_box(PEDESTAL_SIDE_M, PEDESTAL_SIDE_M, top - PLATE.height_m, CAST_IRON),
_isometry((0, 0, (top - PLATE.height_m) / 2)),
),
_visual(
_cylinder(PLATE.radius_m, PLATE.height_m, POLISHED),
_isometry((0, 0, top - PLATE.height_m / 2)),
),
),
)
# The camera: a mast on the carriage, a pan joint about the vertical, a tilt joint about
# the horizontal. The pan frame's x axis heads for the mark and the tilt frame's x axis
# points at it, so at zero pan and tilt the camera looks at the mark. `camera` is the
# optical frame: z forward, x right, y down.
mast_x, mast_y = CAMERA_MAST_XY_M
to_mark = tip - np.array([mast_x, mast_y, CAMERA_HEIGHT_M])
heading = np.arctan2(to_mark[1], to_mark[0])
pitch = np.arctan2(-to_mark[2], np.hypot(to_mark[0], to_mark[1]))
yaw = np.array(
[[np.cos(heading), -np.sin(heading), 0], [np.sin(heading), np.cos(heading), 0], [0, 0, 1]]
)
nod = np.array(
[[np.cos(pitch), 0, np.sin(pitch)], [0, 1, 0], [-np.sin(pitch), 0, np.cos(pitch)]]
)
optical = np.array(OPTICAL_FROM_TILT)
_add_link(
robot,
"camera_mast",
"positioner_tool0",
_isometry((mast_x, mast_y, 0)),
(
_visual(
_cylinder(MAST_RADIUS_M, CAMERA_HEIGHT_M - MAST_HEAD_CLEARANCE_M, RAIL_STEEL),
_isometry((0, 0, (CAMERA_HEIGHT_M - MAST_HEAD_CLEARANCE_M) / 2)),
),
),
)
_add_link(
robot,
"camera_pan",
"camera_mast",
_isometry((0, 0, CAMERA_HEIGHT_M), yaw),
(
_visual(
_cylinder(PAN_HOUSING.radius_m, PAN_HOUSING.height_m, ANODISED),
_isometry((0, 0, -PAN_HOUSING.height_m / 2)),
),
),
axis=(0, 0, 1),
revolute=True,
)
_add_link(
robot,
CAMERA,
"camera_pan",
_isometry(rotation=nod @ optical),
(
_visual(_box(*CAMERA_BODY_M, ANODISED)),
_visual(
_cylinder(LENS.radius_m, LENS.height_m, TOOL_STEEL), _isometry((0, 0, LENS_Z_M))
),
),
# The tilt axis, the pan frame's y, in the optical frame: its -x.
axis=(-1, 0, 0),
revolute=True,
)
# The ball, on a slide parallel to the rail. The only body with collision geometry
# besides the robot, so it is what `Clearance` keeps the forearm away from.
_add_link(
robot,
"obstacle",
WORLD,
_isometry((BALL_LINE_XZ_M[0], 0, BALL_LINE_XZ_M[1])),
(_visual(_ball(BALL_RADIUS_M, RED_LACQUER)),),
collision=Sphere(BALL_RADIUS_M),
axis=(0, 1, 0),
)
_add_instruments(robot)
return robot
Drive it yourself¶
tesseract_python_constraints_example # the acts play by themselves
tesseract_python_constraints_example --sliders # you drive
Without the flag the acts play, and the viewer's Playback controls start, stop and scrub them.
With --sliders the loop solves from the viewer's control panel instead:
- Rail swings by itself starts and stops the rail's own motion; unticked, the Rail slider is yours.
- Rail is the solver's 7th axis hands the carriage to the solver. It drives to abeam of the tip and stays within the band of Carriage follows the tip within. Widen the band, and the other rules move the carriage instead.
- The mark has a handle. Drag it: along the rail the carriage follows, and across it the arm reaches.
- Ball on its slide drives the ball into the arm.
- Every rule has a checkbox, and a slider where it has a bound. A bound that comes on closes in from the live value, as the acts' bounds ramp.
- Solve shows the time of the last frame, and says when the rules you ticked conflict.
Either way, Constraint opacity sets how solid the rules' glyphs are drawn.
def drive_with_sliders(viewer, loop: LiveLoop, stop: threading.Event) -> None:
"""Solve a frame per tick from the GUI's state, until `stop` is set.
The rail swings by itself or follows its slider, or is handed to the solver, which then
keeps the carriage abeam of the tip; the ball follows its slider; the mark follows the
handle dragged in the scene; every rule has a checkbox, and a slider where it has a bound.
A bound that comes on closes in from the live value, as the acts' bounds ramp.
"""
gui = viewer.server.gui
handle = viewer.server.scene.add_transform_controls(
"/mark_handle",
scale=MARK_HANDLE_M,
position=tuple(loop.target),
disable_rotations=True,
depth_test=False,
)
with gui.add_folder("Inputs"):
swings = gui.add_checkbox("Rail swings by itself", True)
rail = _slider(gui, "Rail [m]", SLIDER_RAIL)
ours = gui.add_checkbox("Rail is the solver's 7th axis", False)
follow = _slider(gui, "Carriage follows the tip within [m]", SLIDER_FOLLOW)
gui.add_markdown(
"Ticked, the carriage drives to abeam of the tip and stays there: drag the mark by "
"its handle. Widen the band for the other rules to move the carriage."
)
ball = _slider(gui, "Ball on its slide [m]", SLIDER_BALL)
with gui.add_folder("Rules"):
hold = gui.add_checkbox("WorldHold", True)
axis_on = gui.add_checkbox("HoldAxis", False)
axis = _slider(gui, "HoldAxis tolerance [°]", SLIDER_AXIS_DEG)
cone_on = gui.add_checkbox("PointsDown", False)
cone = _slider(gui, "PointsDown cone [°]", SLIDER_CONE_DEG)
elbow_on = gui.add_checkbox("ElbowUp", False)
elbow = _slider(gui, "Elbow at least [m]", SLIDER_ELBOW_M)
view_on = gui.add_checkbox("WatchTheTool", True)
view = _slider(gui, "Tip off the camera axis at most [°]", SLIDER_VIEW_DEG)
dexterous_on = gui.add_checkbox("StayDexterous", False)
dexterous = _slider(gui, "Manipulability at least", SLIDER_DEXTERITY)
clearance = gui.add_checkbox("Clearance", False)
status = gui.add_text("Solve", initial_value="", disabled=True)
began = time.perf_counter()
recent: list[float] = []
while not stop.is_set():
tick = time.perf_counter()
if swings.value and not ours.value:
rail.value = float(RAIL_SWING_M * np.sin(2.0 * np.pi * (tick - began) / RAIL_PERIOD_S))
live = loop.live
inputs = Inputs(
rail_m=None if ours.value else float(rail.value),
ball_m=float(ball.value),
mark=np.array(handle.position, dtype=np.float64),
hold=hold.value,
axis_deg=_closing("tilt_deg", axis.value, live) if axis_on.value else None,
cone_deg=_closing("tilt_deg", cone.value, live) if cone_on.value else None,
elbow_m=_closing("elbow_m", elbow.value, live) if elbow_on.value else None,
view_deg=_closing("view_deg", view.value, live) if view_on.value else None,
dexterity=_closing("dexterity", dexterous.value, live) if dexterous_on.value else None,
clearance=clearance.value,
follow_m=_closing("lead_m", follow.value, live) if ours.value else None,
)
frame = loop.step(inputs)
if ours.value:
# Show where the solver put the carriage; the slider takes over from there.
rail.value = float(np.clip(frame.q[0], -RAIL_SWING_M, RAIL_SWING_M))
viewer.update_joint_positions(
loop.joint_names,
[
*frame.q,
inputs.ball_m,
*instrument_values(inputs, frame.live, loop.target, caption=None),
],
)
feasible = frame.violation < VIOLATION_TOLERANCE
status.value = f"{frame.solve_ms:.1f} ms" + ("" if feasible else " · rules conflict")
recent.append(frame.solve_ms)
if len(recent) == READOUT_FRAMES:
# Where an act's caption would be: what the solves of the last frames took.
on = rule_count(inputs) + (inputs.follow_m is not None)
_add_text(
viewer.server.scene,
"/readout",
solve_line(on, float(np.median(recent))),
caption_anchor(),
panel_frame()[1],
CAPTION_LINE_M,
valign="top",
)
recent.clear()
stop.wait(max(0.0, 1.0 / FRAME_HZ - (time.perf_counter() - tick)))
A rule of your own¶
The carriage follows the tip because of an eighth rule, written in the example. With the rail
handed over and no rule asking, the solver leaves the carriage where it is: the only cost is
staying close to the frame before. CarriageFollows asks.
class CarriageFollows(PyConstraint):
"""A rule of this example's own: the carriage stays abeam of the tip, within `band` [m].
Along the rail, the tip may lead or trail the carriage by `band` at most: the carriage does
the travelling and the arm the reaching. The rail is the first joint of `q`, and runs along
the world's y.
"""
def __init__(self, var: ti.Var, cell: JointGroup, tcp: str, band: float, coeff: float = 1.0):
super().__init__("carriage_follows", var, [-band], [band], coeff=coeff)
self._cell, self._tcp = cell, tcp
def f(self, q: np.ndarray) -> np.ndarray:
return np.array([self._cell.calcFwdKin(q)[self._tcp].translation[1] - q[0]])
Solve times¶
Each act's caption carries the median solve of its frames, and the last ruler of the panel shows every frame's solve against the 33 ms a 30 Hz loop has. The numbers are those of the machine the recording was made on, a shared 2-vCPU cloud VM (Intel Xeon, 2.1 GHz). Run the example for local numbers: it prints the median and the slowest frame per act.
StayDexterous, WatchTheTool, ElbowUp and Clearance use the default forward-difference
Jacobian; Clearance queries the contact manager for each column.
See also¶
PyConstraint: a rule fromf(q)- Online Planning — replanning a whole trajectory per step
python_constraints_example.py