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)
kdl.Jacobian	alpha_identity ()
mjk.AttachmentSpec	ft_attachment ()
mjk.AttachmentSpec	gripper_attachment ()
mjk.SceneObject	table_object ()
tuple[mjk.Env, mjk.Robot]	build_env ()
None	achd_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
	TAU_MAX
	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 ACHD (Vereshchagin) task-space inner loop, FT-driven.

Same outer admittance loop as ex_admittance_ft.py / ex_admittance_ft_rnea.py,
but a different inner loop. Admittance is an outer force->position loop wrapped
around an inner motion controller. The siblings use an ideal POSITION loop and a
joint-space RNEA computed-torque loop; here the inner loop is TASK SPACE:

    beta = Cartesian PD on the TCP pose error (desired Cartesian acceleration)
    qddot = ACHD(q, qdot, alpha, beta)   (constrained hybrid dynamics, KDL
                                          ChainHdSolver_Vereshchagin)
    tau = RNEA(q, qdot, qddot)           (inverse dynamics for the torque)
    apply tau in TORQUE mode

The acceleration-constrained hybrid-dynamics (ACHD) solver consumes the desired
Cartesian acceleration directly, so the admittance's Cartesian target feeds it
without an IK step (alpha = identity constrains all 6 TCP DOF). It resolves the
joint accelerations through the full arm dynamics, so tracking bandwidth is
uniform across directions (a plain PD+gravity law lags/inverts the soft axis).

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

◆ achd_track()

None ex_admittance_ft_achd.achd_track	(	mjk.Robot	robot,
		dict	state,
		kdl.Frame	target )

Inner loop: task-space control via ACHD constrained hybrid dynamics.

A Cartesian PD on the TCP pose error is the desired acceleration (beta) for
all 6 constrained DOF (alpha = identity); ACHD resolves it into joint
accelerations through the arm dynamics, then RNEA maps those to torques.
No IK is needed -- the Cartesian target feeds the solver directly.
robot.update() must have refreshed jnt_pos_msr/jnt_vel_msr this step.

Definition at line 174 of file ex_admittance_ft_achd.py.

References clamp(), and jnt().

Referenced by admittance_step(), and settle_and_tare().

◆ admittance_step()

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

One admittance tick: force -> offset (outer loop) -> ACHD-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 296 of file ex_admittance_ft_achd.py.

References achd_track(), and admittance_update().

Referenced by run_gui(), and run_selfcheck().

◆ admittance_update()

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

Definition at line 264 of file ex_admittance_ft_achd.py.

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

Referenced by admittance_step().

◆ alpha_identity()

kdl.Jacobian ex_admittance_ft_achd.alpha_identity ( )

Constraint matrix: all 6 TCP DOF are controlled (one beta per row).

Definition at line 111 of file ex_admittance_ft_achd.py.

Referenced by main().

◆ build_env()

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

Definition at line 143 of file ex_admittance_ft_achd.py.

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

Referenced by main().

◆ clamp()

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

Definition at line 83 of file ex_admittance_ft_achd.py.

Referenced by achd_track(), and vclamp().

◆ close_gripper()

None ex_admittance_ft_achd.close_gripper ( mjk.Env env )

Definition at line 214 of file ex_admittance_ft_achd.py.

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

◆ frame_point()

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

Definition at line 107 of file ex_admittance_ft_achd.py.

References xyz().

Referenced by run_gui().

◆ ft_attachment()

mjk.AttachmentSpec ex_admittance_ft_achd.ft_attachment ( )

Definition at line 119 of file ex_admittance_ft_achd.py.

Referenced by build_env().

◆ gripper_attachment()

mjk.AttachmentSpec ex_admittance_ft_achd.gripper_attachment ( )

Definition at line 126 of file ex_admittance_ft_achd.py.

Referenced by build_env().

◆ jnt()

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

Definition at line 76 of file ex_admittance_ft_achd.py.

Referenced by achd_track().

◆ main()

int ex_admittance_ft_achd.main ( )

Definition at line 465 of file ex_admittance_ft_achd.py.

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

Referenced by main().

◆ measured_force()

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

References vnorm(), and xyz().

Referenced by run_gui(), and run_selfcheck().

◆ run_gui()

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

Admittance control for the whole run (ACHD task-space 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 308 of file ex_admittance_ft_achd.py.

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

Referenced by main().

◆ run_selfcheck()

dict ex_admittance_ft_achd.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 375 of file ex_admittance_ft_achd.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_achd.settle_and_tare	(	mjk.Env	env,
		mjk.Robot	robot,
		dict	state )

Close the gripper, hold the home pose until the wrist load settles, 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 219 of file ex_admittance_ft_achd.py.

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

Referenced by main().

◆ spiral_force()

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

Referenced by run_gui(), and run_selfcheck().

◆ table_object()

mjk.SceneObject ex_admittance_ft_achd.table_object ( )

Definition at line 134 of file ex_admittance_ft_achd.py.

Referenced by build_env().

◆ tare_force()

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

Definition at line 260 of file ex_admittance_ft_achd.py.

References xyz().

Referenced by run_gui(), and run_selfcheck().

◆ vadd()

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

Definition at line 87 of file ex_admittance_ft_achd.py.

Referenced by admittance_update().

◆ vclamp()

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

Definition at line 95 of file ex_admittance_ft_achd.py.

References clamp().

Referenced by admittance_update().

◆ vnorm()

float ex_admittance_ft_achd.vnorm ( list[float] a )

Definition at line 99 of file ex_admittance_ft_achd.py.

Referenced by measured_force(), and run_selfcheck().

◆ vscale()

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

Definition at line 91 of file ex_admittance_ft_achd.py.

Referenced by admittance_update().

◆ xyz()

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

Definition at line 103 of file ex_admittance_ft_achd.py.

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

Variable Documentation

◆ BETA_LIN_MAX

ex_admittance_ft_achd.BETA_LIN_MAX

Definition at line 55 of file ex_admittance_ft_achd.py.

◆ BETA_ROT_MAX

ex_admittance_ft_achd.BETA_ROT_MAX

Definition at line 55 of file ex_admittance_ft_achd.py.

◆ D_ADM

ex_admittance_ft_achd.D_ADM

Definition at line 59 of file ex_admittance_ft_achd.py.

◆ FORCE_DEADBAND

float ex_admittance_ft_achd.FORCE_DEADBAND = 2.5

Definition at line 60 of file ex_admittance_ft_achd.py.

◆ GRIPPER_ACTUATOR

str ex_admittance_ft_achd.GRIPPER_ACTUATOR = "g_fingers_actuator"

Definition at line 64 of file ex_admittance_ft_achd.py.

◆ HANDOFF_TARE_TIME

float ex_admittance_ft_achd.HANDOFF_TARE_TIME = 1.0

Definition at line 66 of file ex_admittance_ft_achd.py.

◆ HOME

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

Definition at line 48 of file ex_admittance_ft_achd.py.

◆ K_ADM

ex_admittance_ft_achd.K_ADM

Definition at line 59 of file ex_admittance_ft_achd.py.

◆ KD_LIN

ex_admittance_ft_achd.KD_LIN

Definition at line 53 of file ex_admittance_ft_achd.py.

◆ KD_ROT

ex_admittance_ft_achd.KD_ROT

Definition at line 54 of file ex_admittance_ft_achd.py.

◆ KP_LIN

ex_admittance_ft_achd.KP_LIN

Definition at line 53 of file ex_admittance_ft_achd.py.

◆ KP_ROT

ex_admittance_ft_achd.KP_ROT

Definition at line 54 of file ex_admittance_ft_achd.py.

◆ M_ADM

ex_admittance_ft_achd.M_ADM

Definition at line 59 of file ex_admittance_ft_achd.py.

◆ MAX_OFFSET

float ex_admittance_ft_achd.MAX_OFFSET = 0.20

Definition at line 61 of file ex_admittance_ft_achd.py.

◆ MAX_VEL

float ex_admittance_ft_achd.MAX_VEL = 0.25

Definition at line 62 of file ex_admittance_ft_achd.py.

◆ SELFCHECK_PUSH

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

Definition at line 67 of file ex_admittance_ft_achd.py.

◆ SETTLE_STEPS

int ex_admittance_ft_achd.SETTLE_STEPS = 300

Definition at line 65 of file ex_admittance_ft_achd.py.

◆ TABLE_Z

float ex_admittance_ft_achd.TABLE_Z = 0.70

Definition at line 49 of file ex_admittance_ft_achd.py.

◆ TAU_MAX

ex_admittance_ft_achd.TAU_MAX

Definition at line 55 of file ex_admittance_ft_achd.py.

◆ TEACH_RADIUS

float ex_admittance_ft_achd.TEACH_RADIUS = 0.04

Definition at line 71 of file ex_admittance_ft_achd.py.

◆ TEACH_RISE

float ex_admittance_ft_achd.TEACH_RISE = 0.10

Definition at line 72 of file ex_admittance_ft_achd.py.

◆ TEACH_TIME

float ex_admittance_ft_achd.TEACH_TIME = 16.0

Definition at line 70 of file ex_admittance_ft_achd.py.

◆ TEACH_TURNS

float ex_admittance_ft_achd.TEACH_TURNS = 5.0

Definition at line 73 of file ex_admittance_ft_achd.py.

◆ TOOL_BODY

str ex_admittance_ft_achd.TOOL_BODY = "g_base"

Definition at line 63 of file ex_admittance_ft_achd.py.

Functions

Variables

Detailed Description

Function Documentation

◆ achd_track()

◆ admittance_step()

◆ admittance_update()

◆ alpha_identity()

◆ build_env()

◆ clamp()

◆ close_gripper()

◆ frame_point()

◆ ft_attachment()

◆ gripper_attachment()

◆ jnt()

◆ main()

◆ measured_force()

◆ 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