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 ()
kdl.Frame	tcp_frame (mjk.Robot robot, dict state)
list[float]	jacobian_twist (kdl.Jacobian jac, kdl.JntArray qdot)
None	rnea_track (mjk.Env env, 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.Env env, mjk.Robot robot, dict state)
list[float]	tare_force (mjk.Env env, 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)
float	elbow_height (mjk.Env env)
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
	KP_NULL
	KD_NULL
	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"
str	FT_SITE = "wrist_ft_site"
float	GRIPPER_CLOSED = 0.82
int	SETTLE_STEPS = 300
float	HANDOFF_TARE_TIME = 1.0
float	GUIDE_TIME = 4.7
tuple	SELFCHECK_PUSH = (8.0, 12.0, 6.0)
str	ELBOW_BODY = "forearm_link"
float	MIN_ELBOW_HEIGHT = 0.64
float	TEACH_TIME = 16.0
float	TEACH_RADIUS = 0.04
float	TEACH_RISE = 0.10
float	TEACH_TURNS = 5.0

Detailed Description

F/T admittance around an RNEA task-space inner loop; Python counterpart of ex_admittance_ft.cpp.

Headless, it runs the self-check and exits 1 when a metric is out of its limit; with --gui the
same sequence runs once, the mouse (ctrl + right-drag) pushing the tool after the helix.

Function Documentation

◆ admittance_step()

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

Force -> offset (outer loop) -> RNEA-tracked TCP target, which it returns.

Definition at line 244 of file ex_admittance_ft.py.

References admittance_update(), and rnea_track().

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 219 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 107 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 55 of file ex_admittance_ft.py.

Referenced by rnea_track(), and vclamp().

◆ close_gripper()

None ex_admittance_ft.close_gripper ( mjk.Env env )

Definition at line 184 of file ex_admittance_ft.py.

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

◆ elbow_height()

float ex_admittance_ft.elbow_height ( mjk.Env env )

Definition at line 305 of file ex_admittance_ft.py.

Referenced by run_selfcheck().

◆ frame_point()

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

Definition at line 79 of file ex_admittance_ft.py.

References xyz().

Referenced by run_gui().

◆ ft_attachment()

mjk.AttachmentSpec ex_admittance_ft.ft_attachment ( )

Definition at line 83 of file ex_admittance_ft.py.

Referenced by build_env().

◆ gripper_attachment()

mjk.AttachmentSpec ex_admittance_ft.gripper_attachment ( )

Definition at line 90 of file ex_admittance_ft.py.

Referenced by build_env().

◆ jacobian_twist()

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

Definition at line 142 of file ex_admittance_ft.py.

Referenced by rnea_track().

◆ jnt()

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

Definition at line 48 of file ex_admittance_ft.py.

Referenced by rnea_track(), and tcp_frame().

◆ main()

int ex_admittance_ft.main ( )

Definition at line 406 of file ex_admittance_ft.py.

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

Referenced by main().

◆ measured_force()

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

External force on the tool in the world frame: the tared reading, deadbanded.

Definition at line 203 of file ex_admittance_ft.py.

References vnorm(), and xyz().

Referenced by run_gui(), and run_selfcheck().

◆ rnea_track()

None ex_admittance_ft.rnea_track	(	mjk.Env	env,
		mjk.Robot	robot,
		dict	state,
		kdl.Frame	target )

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

Definition at line 146 of file ex_admittance_ft.py.

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

Referenced by admittance_step(), and settle_and_tare().

◆ run_gui()

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

The same sequence with the viewer; after the helix the mouse pushes the tool.

Definition at line 252 of file ex_admittance_ft.py.

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

Referenced by main().

◆ run_selfcheck()

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

The helix, the tare, then a scripted push on the tool sensed by the F/T; returns metrics.

Definition at line 309 of file ex_admittance_ft.py.

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

Referenced by main().

◆ settle_and_tare()

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

Hold home while the gripper closes, then tare: its ~10 N load only appears once closed.

Definition at line 188 of file ex_admittance_ft.py.

References close_gripper(), rnea_track(), tare_force(), and tcp_frame().

Referenced by main().

◆ spiral_force()

list[float] ex_admittance_ft.spiral_force ( float t )

D_ADM times a helix's velocity: the mass-damper admittance (v = F / D) traces the helix.

Definition at line 232 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 98 of file ex_admittance_ft.py.

Referenced by build_env().

◆ tare_force()

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

Definition at line 215 of file ex_admittance_ft.py.

References xyz().

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

◆ tcp_frame()

kdl.Frame ex_admittance_ft.tcp_frame	(	mjk.Robot	robot,
		dict	state )

Definition at line 136 of file ex_admittance_ft.py.

References jnt().

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

◆ vadd()

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

Definition at line 59 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 67 of file ex_admittance_ft.py.

References clamp().

Referenced by admittance_update().

◆ vnorm()

float ex_admittance_ft.vnorm ( list[float] a )

Definition at line 71 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 63 of file ex_admittance_ft.py.

Referenced by admittance_update().

◆ xyz()

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

Definition at line 75 of file ex_admittance_ft.py.

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

Variable Documentation

◆ BETA_LIN_MAX

ex_admittance_ft.BETA_LIN_MAX

Definition at line 22 of file ex_admittance_ft.py.

◆ BETA_ROT_MAX

ex_admittance_ft.BETA_ROT_MAX

Definition at line 22 of file ex_admittance_ft.py.

◆ D_ADM

ex_admittance_ft.D_ADM

Definition at line 26 of file ex_admittance_ft.py.

◆ ELBOW_BODY

str ex_admittance_ft.ELBOW_BODY = "forearm_link"

Definition at line 38 of file ex_admittance_ft.py.

◆ FORCE_DEADBAND

float ex_admittance_ft.FORCE_DEADBAND = 2.5

Definition at line 27 of file ex_admittance_ft.py.

◆ FT_SITE

str ex_admittance_ft.FT_SITE = "wrist_ft_site"

Definition at line 32 of file ex_admittance_ft.py.

◆ GRIPPER_ACTUATOR

str ex_admittance_ft.GRIPPER_ACTUATOR = "g_fingers_actuator"

Definition at line 31 of file ex_admittance_ft.py.

◆ GRIPPER_CLOSED

float ex_admittance_ft.GRIPPER_CLOSED = 0.82

Definition at line 33 of file ex_admittance_ft.py.

◆ GUIDE_TIME

float ex_admittance_ft.GUIDE_TIME = 4.7

Definition at line 36 of file ex_admittance_ft.py.

◆ HANDOFF_TARE_TIME

float ex_admittance_ft.HANDOFF_TARE_TIME = 1.0

Definition at line 35 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 16 of file ex_admittance_ft.py.

◆ K_ADM

ex_admittance_ft.K_ADM

Definition at line 26 of file ex_admittance_ft.py.

◆ KD_LIN

ex_admittance_ft.KD_LIN

Definition at line 20 of file ex_admittance_ft.py.

◆ KD_NULL

ex_admittance_ft.KD_NULL

Definition at line 23 of file ex_admittance_ft.py.

◆ KD_ROT

ex_admittance_ft.KD_ROT

Definition at line 21 of file ex_admittance_ft.py.

◆ KP_LIN

ex_admittance_ft.KP_LIN

Definition at line 20 of file ex_admittance_ft.py.

◆ KP_NULL

ex_admittance_ft.KP_NULL

Definition at line 23 of file ex_admittance_ft.py.

◆ KP_ROT

ex_admittance_ft.KP_ROT

Definition at line 21 of file ex_admittance_ft.py.

◆ M_ADM

ex_admittance_ft.M_ADM

Definition at line 26 of file ex_admittance_ft.py.

◆ MAX_OFFSET

float ex_admittance_ft.MAX_OFFSET = 0.20

Definition at line 28 of file ex_admittance_ft.py.

◆ MAX_VEL

float ex_admittance_ft.MAX_VEL = 0.25

Definition at line 29 of file ex_admittance_ft.py.

◆ MIN_ELBOW_HEIGHT

float ex_admittance_ft.MIN_ELBOW_HEIGHT = 0.64

Definition at line 39 of file ex_admittance_ft.py.

◆ SELFCHECK_PUSH

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

Definition at line 37 of file ex_admittance_ft.py.

◆ SETTLE_STEPS

int ex_admittance_ft.SETTLE_STEPS = 300

Definition at line 34 of file ex_admittance_ft.py.

◆ TABLE_Z

float ex_admittance_ft.TABLE_Z = 0.70

Definition at line 17 of file ex_admittance_ft.py.

◆ TEACH_RADIUS

float ex_admittance_ft.TEACH_RADIUS = 0.04

Definition at line 43 of file ex_admittance_ft.py.

◆ TEACH_RISE

float ex_admittance_ft.TEACH_RISE = 0.10

Definition at line 44 of file ex_admittance_ft.py.

◆ TEACH_TIME

float ex_admittance_ft.TEACH_TIME = 16.0

Definition at line 42 of file ex_admittance_ft.py.

◆ TEACH_TURNS

float ex_admittance_ft.TEACH_TURNS = 5.0

Definition at line 45 of file ex_admittance_ft.py.

◆ TOOL_BODY

str ex_admittance_ft.TOOL_BODY = "g_base"

Definition at line 30 of file ex_admittance_ft.py.

Functions

Variables

Detailed Description

Function Documentation

◆ admittance_step()

◆ admittance_update()

◆ build_env()

◆ clamp()

◆ close_gripper()

◆ elbow_height()

◆ 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()

◆ tcp_frame()

◆ vadd()

◆ vclamp()

◆ vnorm()

◆ vscale()

◆ xyz()

Variable Documentation

◆ BETA_LIN_MAX

◆ BETA_ROT_MAX

◆ D_ADM

◆ ELBOW_BODY

◆ FORCE_DEADBAND

◆ FT_SITE

◆ GRIPPER_ACTUATOR

◆ GRIPPER_CLOSED

◆ GUIDE_TIME

◆ HANDOFF_TARE_TIME

◆ HOME

◆ K_ADM

◆ KD_LIN

◆ KD_NULL

◆ KD_ROT

◆ KP_LIN

◆ KP_NULL

◆ KP_ROT

◆ M_ADM

◆ MAX_OFFSET

◆ MAX_VEL

◆ MIN_ELBOW_HEIGHT

◆ SELFCHECK_PUSH

◆ SETTLE_STEPS

◆ TABLE_Z

◆ TEACH_RADIUS

◆ TEACH_RISE

◆ TEACH_TIME

◆ TEACH_TURNS

◆ TOOL_BODY