Functions
kdl.JntArray	jnt (list[float] values)
float	clamp (float value, float low, float high)
list[float]	vadd (list[float] a, list[float] b)
list[float]	vscale (list[float] a, float s)
list[float]	vclamp (list[float] a, float limit)
float	vnorm (list[float] a)
list[float]	xyz (kdl.Vector v)
list[float]	frame_point (kdl.Frame frame, kdl.Vector point)
mjk.AttachmentSpec	ft_attachment ()
mjk.AttachmentSpec	gripper_attachment ()
mjk.SceneObject	table_object ()
tuple[mjk.Env, mjk.Robot]	build_env ()
list[float]	ik_step (kdl.ChainIkSolverVel_wdls ik, kdl.ChainFkSolverPos_recursive fk, list[float] q_seed, kdl.Frame target, list[tuple[float, float]] limits)
None	hold (mjk.Robot robot, list[float] q)
None	close_gripper (mjk.Env env)
list[float]	settle_and_tare (mjk.Env env, mjk.Robot robot)
list[float]	measured_force (mjk.Robot robot, dict state)
list[float]	tare_force (mjk.Robot robot)
None	admittance_update (dict state, list[float] force, float dt)
list[float]	spiral_force (float t)
	admittance_step (env, robot, nominal, state, force)
None	run_gui (mjk.Env env, mjk.Robot robot, kdl.Frame nominal, dict state)
dict	run_selfcheck (mjk.Env env, mjk.Robot robot, kdl.Frame nominal, dict state)
int	main ()

Variables
list	HOME = [0.0, 0.2618, 3.1416, -2.2689, 0.0, 0.9599, 1.5708]
float	TABLE_Z = 0.70
	M_ADM
	D_ADM
	K_ADM
float	FORCE_DEADBAND = 2.5
float	MAX_OFFSET = 0.20
float	MAX_VEL = 0.25
str	TOOL_BODY = "g_base"
str	GRIPPER_ACTUATOR = "g_fingers_actuator"
int	SETTLE_STEPS = 300
float	HANDOFF_TARE_TIME = 1.0
tuple	SELFCHECK_PUSH = (8.0, 12.0, 6.0)
float	TEACH_TIME = 16.0
float	TEACH_RADIUS = 0.04
float	TEACH_RISE = 0.10
float	TEACH_TURNS = 5.0

Detailed Description

Kinova + Robotiq tabletop admittance control driven by a named FT sensor.

The robot stays in ONE control law for the whole run: admittance. Admittance
control is an outer force->position loop wrapped around a stiff inner position
controller (it is the position-controlled dual of impedance control, which is
torque-based). The outer loop maps external force to a TCP position offset; the
inner loop (POSITION mode) tracks that offset exactly.

Outer admittance law per Cartesian axis (no position stiffness):

    M * a = F_ext - D * v
    v += a * dt          (clamped to MAX_VEL)
    offset += v * dt     (clamped to MAX_OFFSET)

The logical FT sensor sits between the Kinova wrist and the Robotiq gripper.
After closing the gripper and letting the wrist load settle, the controller
tares it (the gripper's ~10 N static load only appears once it has closed).

The run has two sources of external force, both handled by the same law:
  - Intro: a scripted force whose direction sweeps a helix (spiral_force) drives
    the admittance, so the TCP traces a helix.
  - After the helix: the scripted force stops; the controller stays in
    admittance and responds to the FT-measured force, so in the GUI you can
    ctrl + right-drag the gripper. With K = 0 there is no equilibrium to spring
    back to: when force stops, damping bleeds v -> 0 and the pose holds.

Function Documentation

◆ admittance_step()

ex_admittance_ft.admittance_step	(	env,
		robot,
		nominal,
		state,
		force )

One admittance tick: force -> offset (outer loop) -> position-tracked TCP.

robot.update() must have run this step so the FT read behind `force` is
current. Returns the commanded target frame (for tracing).

Definition at line 270 of file ex_admittance_ft.py.

References admittance_update(), hold(), and ik_step().

Referenced by run_gui(), and run_selfcheck().

◆ admittance_update()

None ex_admittance_ft.admittance_update	(	dict	state,
		list[float]	force,
		float	dt )

Definition at line 238 of file ex_admittance_ft.py.

References vadd(), vclamp(), and vscale().

Referenced by admittance_step().

◆ build_env()

tuple[mjk.Env, mjk.Robot] ex_admittance_ft.build_env ( )

Definition at line 118 of file ex_admittance_ft.py.

References ft_attachment(), gripper_attachment(), and table_object().

Referenced by main().

◆ clamp()

float ex_admittance_ft.clamp	(	float	value,
		float	low,
		float	high )

Definition at line 66 of file ex_admittance_ft.py.

Referenced by ik_step(), and vclamp().

◆ close_gripper()

None ex_admittance_ft.close_gripper ( mjk.Env env )

Definition at line 189 of file ex_admittance_ft.py.

Referenced by run_gui(), run_selfcheck(), and settle_and_tare().

◆ frame_point()

list[float] ex_admittance_ft.frame_point	(	kdl.Frame	frame,
		kdl.Vector	point )

Definition at line 90 of file ex_admittance_ft.py.

References xyz().

Referenced by run_gui().

◆ ft_attachment()

mjk.AttachmentSpec ex_admittance_ft.ft_attachment ( )

Definition at line 94 of file ex_admittance_ft.py.

Referenced by build_env().

◆ gripper_attachment()

mjk.AttachmentSpec ex_admittance_ft.gripper_attachment ( )

Definition at line 101 of file ex_admittance_ft.py.

Referenced by build_env().

◆ hold()

None ex_admittance_ft.hold	(	mjk.Robot	robot,
		list[float]	q )

Inner position loop: command and pin the arm to q (exact tracking).

Admittance control is an outer force->position loop wrapped around a stiff
inner position controller. The Kinova's position actuators are too soft to
track a moving Cartesian target, so we make the inner loop ideal by also
setting the joint state directly (call_forward refreshes kinematics/sensors).

Definition at line 177 of file ex_admittance_ft.py.

Referenced by admittance_step(), and settle_and_tare().

◆ ik_step()

list[float] ex_admittance_ft.ik_step	(	kdl.ChainIkSolverVel_wdls	ik,
		kdl.ChainFkSolverPos_recursive	fk,
		list[float]	q_seed,
		kdl.Frame	target,
		list[tuple[float, float]]	limits )

Definition at line 149 of file ex_admittance_ft.py.

References clamp(), and jnt().

Referenced by admittance_step().

◆ jnt()

kdl.JntArray ex_admittance_ft.jnt ( list[float] values )

Definition at line 59 of file ex_admittance_ft.py.

Referenced by ik_step().

◆ main()

int ex_admittance_ft.main ( )

Definition at line 439 of file ex_admittance_ft.py.

References build_env(), main(), run_gui(), run_selfcheck(), and settle_and_tare().

Referenced by main().

◆ measured_force()

list[float] ex_admittance_ft.measured_force	(	mjk.Robot	robot,
		dict	state )

External force on the tool in world frame, gravity-tared, deadbanded.

The MuJoCo force sensor reports the reaction wrench at the site, so the
external push the user applies is the negated, bias-removed reading. The
bias is the gripper's static gravity load captured after the gripper closes
and the wrist load settles (see settle_and_tare); expressed in
the world frame this is just the distal weight (mg, downward) and is
invariant to the arm configuration, so a single tare stays valid as the TCP
translates around home. Sub-deadband residue (noise, settling transients)
is rejected to zero.

Definition at line 213 of file ex_admittance_ft.py.

References vnorm(), and xyz().

Referenced by run_gui(), and run_selfcheck().

◆ run_gui()

None ex_admittance_ft.run_gui	(	mjk.Env	env,
		mjk.Robot	robot,
		kdl.Frame	nominal,
		dict	state )

Admittance control for the whole run (POSITION inner loop throughout).

For the first TEACH_TIME seconds a scripted helical force drives the
admittance, so the TCP traces a helix. After that the scripted force stops
and you can ctrl + right-drag the gripper to apply your own force, which the
FT senses; the same admittance responds and holds on release.

Definition at line 285 of file ex_admittance_ft.py.

References admittance_step(), close_gripper(), frame_point(), measured_force(), spiral_force(), and tare_force().

Referenced by main().

◆ run_selfcheck()

dict ex_admittance_ft.run_selfcheck	(	mjk.Env	env,
		mjk.Robot	robot,
		kdl.Frame	nominal,
		dict	state )

Headless exercise of the same admittance law the GUI uses. Returns metrics.

Phase A: the scripted helical force drives the admittance (intro behaviour).
Phase B: a physical +Y wrench is sensed by the FT and yielded to, then
released. Verifies the admittance reacts to both force sources and holds
when force stops.

Definition at line 351 of file ex_admittance_ft.py.

References admittance_step(), close_gripper(), measured_force(), spiral_force(), tare_force(), and vnorm().

Referenced by main().

◆ settle_and_tare()

list[float] ex_admittance_ft.settle_and_tare	(	mjk.Env	env,
		mjk.Robot	robot )

Close the gripper, hold home until the wrist load settles, then tare.

The gripper's static load shows up at the FT site only once it has closed
and settled (~10 N here). Taring before that (right after reset, gripper
open) leaves a large constant bias error that an integrating (K=0)
admittance turns into permanent drift. So we hold the closed-gripper home
pose for a moment first, then capture the bias.

Definition at line 194 of file ex_admittance_ft.py.

References close_gripper(), hold(), and xyz().

Referenced by main().

◆ spiral_force()

list[float] ex_admittance_ft.spiral_force ( float t )

Scripted external force whose direction sweeps a helix over TEACH_TIME.

The force is D_ADM times the velocity of a helical path, so a mass-damper
admittance (steady state v = F / D) turns it into helical motion. Applied to
the tool and sensed by the FT sensor, this drives the intro helix.

Definition at line 253 of file ex_admittance_ft.py.

Referenced by run_gui(), and run_selfcheck().

◆ table_object()

mjk.SceneObject ex_admittance_ft.table_object ( )

Definition at line 109 of file ex_admittance_ft.py.

Referenced by build_env().

◆ tare_force()

list[float] ex_admittance_ft.tare_force ( mjk.Robot robot )

Definition at line 234 of file ex_admittance_ft.py.

References xyz().

Referenced by run_gui(), and run_selfcheck().

◆ vadd()

list[float] ex_admittance_ft.vadd	(	list[float]	a,
		list[float]	b )

Definition at line 70 of file ex_admittance_ft.py.

Referenced by admittance_update().

◆ vclamp()

list[float] ex_admittance_ft.vclamp	(	list[float]	a,
		float	limit )

Definition at line 78 of file ex_admittance_ft.py.

References clamp().

Referenced by admittance_update().

◆ vnorm()

float ex_admittance_ft.vnorm ( list[float] a )

Definition at line 82 of file ex_admittance_ft.py.

Referenced by measured_force(), and run_selfcheck().

◆ vscale()

list[float] ex_admittance_ft.vscale	(	list[float]	a,
		float	s )

Definition at line 74 of file ex_admittance_ft.py.

Referenced by admittance_update().

◆ xyz()

list[float] ex_admittance_ft.xyz ( kdl.Vector v )

Definition at line 86 of file ex_admittance_ft.py.

Referenced by frame_point(), measured_force(), settle_and_tare(), and tare_force().

Variable Documentation

◆ D_ADM

ex_admittance_ft.D_ADM

Definition at line 42 of file ex_admittance_ft.py.

◆ FORCE_DEADBAND

float ex_admittance_ft.FORCE_DEADBAND = 2.5

Definition at line 43 of file ex_admittance_ft.py.

◆ GRIPPER_ACTUATOR

str ex_admittance_ft.GRIPPER_ACTUATOR = "g_fingers_actuator"

Definition at line 47 of file ex_admittance_ft.py.

◆ HANDOFF_TARE_TIME

float ex_admittance_ft.HANDOFF_TARE_TIME = 1.0

Definition at line 49 of file ex_admittance_ft.py.

◆ HOME

list ex_admittance_ft.HOME = [0.0, 0.2618, 3.1416, -2.2689, 0.0, 0.9599, 1.5708]

Definition at line 37 of file ex_admittance_ft.py.

◆ K_ADM

ex_admittance_ft.K_ADM

Definition at line 42 of file ex_admittance_ft.py.

◆ M_ADM

ex_admittance_ft.M_ADM

Definition at line 42 of file ex_admittance_ft.py.

◆ MAX_OFFSET

float ex_admittance_ft.MAX_OFFSET = 0.20

Definition at line 44 of file ex_admittance_ft.py.

◆ MAX_VEL

float ex_admittance_ft.MAX_VEL = 0.25

Definition at line 45 of file ex_admittance_ft.py.

◆ SELFCHECK_PUSH

tuple ex_admittance_ft.SELFCHECK_PUSH = (8.0, 12.0, 6.0)

Definition at line 50 of file ex_admittance_ft.py.

◆ SETTLE_STEPS

int ex_admittance_ft.SETTLE_STEPS = 300

Definition at line 48 of file ex_admittance_ft.py.

◆ TABLE_Z

float ex_admittance_ft.TABLE_Z = 0.70

Definition at line 38 of file ex_admittance_ft.py.

◆ TEACH_RADIUS

float ex_admittance_ft.TEACH_RADIUS = 0.04

Definition at line 54 of file ex_admittance_ft.py.

◆ TEACH_RISE

float ex_admittance_ft.TEACH_RISE = 0.10

Definition at line 55 of file ex_admittance_ft.py.

◆ TEACH_TIME

float ex_admittance_ft.TEACH_TIME = 16.0

Definition at line 53 of file ex_admittance_ft.py.

◆ TEACH_TURNS

float ex_admittance_ft.TEACH_TURNS = 5.0

Definition at line 56 of file ex_admittance_ft.py.

◆ TOOL_BODY

str ex_admittance_ft.TOOL_BODY = "g_base"

Definition at line 46 of file ex_admittance_ft.py.

Functions

Variables

Detailed Description

Function Documentation

◆ admittance_step()

◆ admittance_update()

◆ build_env()

◆ clamp()

◆ close_gripper()

◆ frame_point()

◆ ft_attachment()

◆ gripper_attachment()

◆ hold()

◆ ik_step()

◆ jnt()

◆ main()

◆ measured_force()

◆ run_gui()

◆ run_selfcheck()

◆ settle_and_tare()

◆ spiral_force()

◆ table_object()

◆ tare_force()

◆ vadd()

◆ vclamp()

◆ vnorm()

◆ vscale()

◆ xyz()

Variable Documentation

◆ D_ADM

◆ FORCE_DEADBAND

◆ GRIPPER_ACTUATOR

◆ HANDOFF_TARE_TIME

◆ HOME

◆ K_ADM

◆ M_ADM

◆ MAX_OFFSET

◆ MAX_VEL

◆ SELFCHECK_PUSH

◆ SETTLE_STEPS

◆ TABLE_Z

◆ TEACH_RADIUS

◆ TEACH_RISE

◆ TEACH_TIME

◆ TEACH_TURNS

◆ TOOL_BODY