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]	jacobian_twist (kdl.Jacobian jac, kdl.JntArray qdot)
None	rnea_track (mjk.Robot robot, dict state, kdl.Frame target)
None	close_gripper (mjk.Env env)
list[float]	settle_and_tare (mjk.Env env, mjk.Robot robot, dict state)
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
	KP_LIN
	KD_LIN
	KP_ROT
	KD_ROT
	BETA_LIN_MAX
	BETA_ROT_MAX
float	TAU_MAX = 141.6
	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

Admittance control with an RNEA computed-torque inner loop, FT-driven.

Same outer admittance loop as ex_admittance_ft.py, but a different inner loop.
Admittance control is an outer force->position loop wrapped around an inner
motion controller. The sibling example uses an (ideal) POSITION inner loop; here
the inner loop is torque-based COMPUTED TORQUE in task space:

    beta = Cartesian PD on TCP pose error             (desired TCP accel)
    qddot_des = WDLS(beta)                            (resolved acceleration)
    tau = RNEA(q, qdot, qddot_des)                    (KDL ChainIdSolver_RNE)
    apply tau in TORQUE mode

RNEA inverse dynamics maps the resolved joint acceleration to torques through
the full arm dynamics (gravity, Coriolis, inertia). Keeping the servo in
Cartesian space avoids the unstable joint-IK target chasing that makes FT
hand-guiding wobble after release.

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_rnea.admittance_step	(	env,
		robot,
		nominal,
		state,
		force )

One admittance tick: force -> offset (outer loop) -> RNEA-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 284 of file ex_admittance_ft_rnea.py.

References admittance_update(), and rnea_track().

Referenced by run_gui(), and run_selfcheck().

◆ admittance_update()

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

Definition at line 252 of file ex_admittance_ft_rnea.py.

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

Referenced by admittance_step().

◆ build_env()

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

Definition at line 134 of file ex_admittance_ft_rnea.py.

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

Referenced by main().

◆ clamp()

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

Definition at line 82 of file ex_admittance_ft_rnea.py.

Referenced by rnea_track(), and vclamp().

◆ close_gripper()

None ex_admittance_ft_rnea.close_gripper ( mjk.Env env )

Definition at line 201 of file ex_admittance_ft_rnea.py.

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

◆ frame_point()

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

Definition at line 106 of file ex_admittance_ft_rnea.py.

References xyz().

Referenced by run_gui().

◆ ft_attachment()

mjk.AttachmentSpec ex_admittance_ft_rnea.ft_attachment ( )

Definition at line 110 of file ex_admittance_ft_rnea.py.

Referenced by build_env().

◆ gripper_attachment()

mjk.AttachmentSpec ex_admittance_ft_rnea.gripper_attachment ( )

Definition at line 117 of file ex_admittance_ft_rnea.py.

Referenced by build_env().

◆ jacobian_twist()

list[float] ex_admittance_ft_rnea.jacobian_twist	(	kdl.Jacobian	jac,
		kdl.JntArray	qdot )

Definition at line 165 of file ex_admittance_ft_rnea.py.

Referenced by rnea_track().

◆ jnt()

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

Definition at line 75 of file ex_admittance_ft_rnea.py.

Referenced by rnea_track().

◆ main()

int ex_admittance_ft_rnea.main ( )

Definition at line 451 of file ex_admittance_ft_rnea.py.

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

Referenced by main().

◆ measured_force()

list[float] ex_admittance_ft_rnea.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 227 of file ex_admittance_ft_rnea.py.

References vnorm(), and xyz().

Referenced by run_gui(), and run_selfcheck().

◆ rnea_track()

None ex_admittance_ft_rnea.rnea_track	(	mjk.Robot	robot,
		dict	state,
		kdl.Frame	target )

Task-space computed torque: Cartesian PD -> qddot -> RNEA torque.

Definition at line 169 of file ex_admittance_ft_rnea.py.

References clamp(), jacobian_twist(), and jnt().

Referenced by admittance_step(), and settle_and_tare().

◆ run_gui()

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

Admittance control for the whole run (RNEA computed-torque inner loop).

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 296 of file ex_admittance_ft_rnea.py.

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

Referenced by main().

◆ run_selfcheck()

dict ex_admittance_ft_rnea.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 361 of file ex_admittance_ft_rnea.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_rnea.settle_and_tare	(	mjk.Env	env,
		mjk.Robot	robot,
		dict	state )

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 206 of file ex_admittance_ft_rnea.py.

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

Referenced by main().

◆ spiral_force()

list[float] ex_admittance_ft_rnea.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. Fed into
the admittance, this drives the intro helix.

Definition at line 267 of file ex_admittance_ft_rnea.py.

Referenced by run_gui(), and run_selfcheck().

◆ table_object()

mjk.SceneObject ex_admittance_ft_rnea.table_object ( )

Definition at line 125 of file ex_admittance_ft_rnea.py.

Referenced by build_env().

◆ tare_force()

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

Definition at line 248 of file ex_admittance_ft_rnea.py.

References xyz().

Referenced by run_gui(), and run_selfcheck().

◆ vadd()

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

Definition at line 86 of file ex_admittance_ft_rnea.py.

Referenced by admittance_update().

◆ vclamp()

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

Definition at line 94 of file ex_admittance_ft_rnea.py.

References clamp().

Referenced by admittance_update().

◆ vnorm()

float ex_admittance_ft_rnea.vnorm ( list[float] a )

Definition at line 98 of file ex_admittance_ft_rnea.py.

Referenced by measured_force(), and run_selfcheck().

◆ vscale()

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

Definition at line 90 of file ex_admittance_ft_rnea.py.

Referenced by admittance_update().

◆ xyz()

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

Definition at line 102 of file ex_admittance_ft_rnea.py.

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

Variable Documentation

◆ BETA_LIN_MAX

ex_admittance_ft_rnea.BETA_LIN_MAX

Definition at line 53 of file ex_admittance_ft_rnea.py.

◆ BETA_ROT_MAX

ex_admittance_ft_rnea.BETA_ROT_MAX

Definition at line 53 of file ex_admittance_ft_rnea.py.

◆ D_ADM

ex_admittance_ft_rnea.D_ADM

Definition at line 58 of file ex_admittance_ft_rnea.py.

◆ FORCE_DEADBAND

float ex_admittance_ft_rnea.FORCE_DEADBAND = 2.5

Definition at line 59 of file ex_admittance_ft_rnea.py.

◆ GRIPPER_ACTUATOR

str ex_admittance_ft_rnea.GRIPPER_ACTUATOR = "g_fingers_actuator"

Definition at line 63 of file ex_admittance_ft_rnea.py.

◆ HANDOFF_TARE_TIME

float ex_admittance_ft_rnea.HANDOFF_TARE_TIME = 1.0

Definition at line 65 of file ex_admittance_ft_rnea.py.

◆ HOME

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

Definition at line 46 of file ex_admittance_ft_rnea.py.

◆ K_ADM

ex_admittance_ft_rnea.K_ADM

Definition at line 58 of file ex_admittance_ft_rnea.py.

◆ KD_LIN

ex_admittance_ft_rnea.KD_LIN

Definition at line 51 of file ex_admittance_ft_rnea.py.

◆ KD_ROT

ex_admittance_ft_rnea.KD_ROT

Definition at line 52 of file ex_admittance_ft_rnea.py.

◆ KP_LIN

ex_admittance_ft_rnea.KP_LIN

Definition at line 51 of file ex_admittance_ft_rnea.py.

◆ KP_ROT

ex_admittance_ft_rnea.KP_ROT

Definition at line 52 of file ex_admittance_ft_rnea.py.

◆ M_ADM

ex_admittance_ft_rnea.M_ADM

Definition at line 58 of file ex_admittance_ft_rnea.py.

◆ MAX_OFFSET

float ex_admittance_ft_rnea.MAX_OFFSET = 0.20

Definition at line 60 of file ex_admittance_ft_rnea.py.

◆ MAX_VEL

float ex_admittance_ft_rnea.MAX_VEL = 0.25

Definition at line 61 of file ex_admittance_ft_rnea.py.

◆ SELFCHECK_PUSH

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

Definition at line 66 of file ex_admittance_ft_rnea.py.

◆ SETTLE_STEPS

int ex_admittance_ft_rnea.SETTLE_STEPS = 300

Definition at line 64 of file ex_admittance_ft_rnea.py.

◆ TABLE_Z

float ex_admittance_ft_rnea.TABLE_Z = 0.70

Definition at line 47 of file ex_admittance_ft_rnea.py.

◆ TAU_MAX

float ex_admittance_ft_rnea.TAU_MAX = 141.6

Definition at line 54 of file ex_admittance_ft_rnea.py.

◆ TEACH_RADIUS

float ex_admittance_ft_rnea.TEACH_RADIUS = 0.04

Definition at line 70 of file ex_admittance_ft_rnea.py.

◆ TEACH_RISE

float ex_admittance_ft_rnea.TEACH_RISE = 0.10

Definition at line 71 of file ex_admittance_ft_rnea.py.

◆ TEACH_TIME

float ex_admittance_ft_rnea.TEACH_TIME = 16.0

Definition at line 69 of file ex_admittance_ft_rnea.py.

◆ TEACH_TURNS

float ex_admittance_ft_rnea.TEACH_TURNS = 5.0

Definition at line 72 of file ex_admittance_ft_rnea.py.

◆ TOOL_BODY

str ex_admittance_ft_rnea.TOOL_BODY = "g_base"

Definition at line 62 of file ex_admittance_ft_rnea.py.

Functions

Variables

Detailed Description

Function Documentation

◆ admittance_step()

◆ admittance_update()

◆ build_env()

◆ clamp()

◆ close_gripper()

◆ frame_point()

◆ ft_attachment()

◆ gripper_attachment()

◆ jacobian_twist()

◆ jnt()

◆ main()

◆ measured_force()

◆ rnea_track()

◆ run_gui()

◆ run_selfcheck()

◆ settle_and_tare()

◆ spiral_force()

◆ table_object()

◆ tare_force()

◆ vadd()

◆ vclamp()

◆ vnorm()

◆ vscale()

◆ xyz()

Variable Documentation

◆ BETA_LIN_MAX

◆ BETA_ROT_MAX

◆ D_ADM

◆ FORCE_DEADBAND

◆ GRIPPER_ACTUATOR

◆ HANDOFF_TARE_TIME

◆ HOME

◆ K_ADM

◆ KD_LIN

◆ KD_ROT

◆ KP_LIN

◆ KP_ROT

◆ M_ADM

◆ MAX_OFFSET

◆ MAX_VEL

◆ SELFCHECK_PUSH

◆ SETTLE_STEPS

◆ TABLE_Z

◆ TAU_MAX

◆ TEACH_RADIUS

◆ TEACH_RISE

◆ TEACH_TIME

◆ TEACH_TURNS

◆ TOOL_BODY