Classes
class	ResetRequested
class	Phase

Functions
kdl.Frame	world_T_base (int arm)
kdl.Vector	cube_spot (int arm, xy)
mjk.SceneObject	cube_object (str name, kdl.Vector spot)
mjk.SceneObject	free_object (name, shape, x, y, half, rgb)
mjk.CameraSpec	camera (name, pos, quat, fovy)
tuple[mjk.Env, list[mjk.Robot]]	build_env ()
kdl.JntArray	jnt (values)
list[float]	gravity (kdl.ChainDynParam dyn, q_values)
list[float]	solve_near_seed (chain, limits, list[float] seed, kdl.Frame target)
list[Phase]	pick_place_phases (mjk.Robot robot)
list[float]	rnea_torque (robot, rnea, n_segments, q_des)
None	measure (env, dict state)
bool	run_phase (env, robots, fingers, solvers, Phase phase, dict state)
dict	new_metrics ()
int	main ()

Variables
list	HOME = [0.0, 0.2618, 3.1416, -2.2689, 0.0, 0.9599, 1.5708]
float	SURFACE_Z = 0.70
float	CUBE_HS = 0.02
tuple	PICK = (0.40, 0.00)
tuple	PLACE = (0.40, 0.24)
list	KP = [100.0, 200.0, 100.0, 200.0, 100.0, 200.0, 100.0]
list	KD = [20.0, 28.0, 20.0, 28.0, 20.0, 28.0, 20.0]
float	GRIPPER_CLOSED = 0.82
float	MAX_PLACE_ERR = 0.005
float	MIN_ELBOW_HEIGHT = 0.45
float	MAX_DISTURB = 0.001
list	BASES = [(-0.70, -0.12, 0.0), (0.70, 0.12, math.pi)]
list	PREFIXES = ["", "r2_"]
list	FREE_OBJECTS
list	CAMERAS

Detailed Description

Two arms facing each other across a table each pick and place a cube with RNEA computed
torque; Python counterpart of ex_rnea_pick_place.cpp. Headless it exits 1 unless both cubes
are placed, both elbows stay up, the free objects stay put, the arms never touch and RNEA
never fails.

Function Documentation

◆ build_env()

tuple[mjk.Env, list[mjk.Robot]] ex_rnea_pick_place.build_env ( )

Definition at line 106 of file ex_rnea_pick_place.py.

References camera(), cube_object(), cube_spot(), and free_object().

Referenced by main().

◆ camera()

mjk.CameraSpec ex_rnea_pick_place.camera	(	name,
		pos,
		quat,
		fovy )

Definition at line 97 of file ex_rnea_pick_place.py.

Referenced by build_env().

◆ cube_object()

mjk.SceneObject ex_rnea_pick_place.cube_object	(	str	name,
		kdl.Vector	spot )

Definition at line 71 of file ex_rnea_pick_place.py.

Referenced by build_env().

◆ cube_spot()

kdl.Vector ex_rnea_pick_place.cube_spot	(	int	arm,
			xy )

A cube's centre resting on the table at xy in the arm's base frame, in the world frame.

Definition at line 66 of file ex_rnea_pick_place.py.

References world_T_base().

Referenced by build_env(), and main().

◆ free_object()

mjk.SceneObject ex_rnea_pick_place.free_object	(	name,
		shape,
		x,
		y,
		half,
		rgb )

Definition at line 84 of file ex_rnea_pick_place.py.

Referenced by build_env().

◆ gravity()

list[float] ex_rnea_pick_place.gravity	(	kdl.ChainDynParam	dyn,
			q_values )

Definition at line 152 of file ex_rnea_pick_place.py.

References jnt().

Referenced by main().

◆ jnt()

kdl.JntArray ex_rnea_pick_place.jnt ( values )

Definition at line 145 of file ex_rnea_pick_place.py.

Referenced by gravity(), rnea_torque(), and solve_near_seed().

◆ main()

int ex_rnea_pick_place.main ( )

Definition at line 281 of file ex_rnea_pick_place.py.

References build_env(), cube_spot(), gravity(), main(), new_metrics(), pick_place_phases(), and run_phase().

Referenced by main().

◆ measure()

None ex_rnea_pick_place.measure	(		env,
		dict	state )

What a viewer sees: the elbows, the free objects and any arm-to-arm contact.

Definition at line 234 of file ex_rnea_pick_place.py.

Referenced by run_phase().

◆ new_metrics()

dict ex_rnea_pick_place.new_metrics ( )

Definition at line 277 of file ex_rnea_pick_place.py.

Referenced by main().

◆ pick_place_phases()

list[Phase] ex_rnea_pick_place.pick_place_phases ( mjk.Robot robot )

Joint waypoints in the arm's base frame, so they serve every arm placed like this one.

Definition at line 189 of file ex_rnea_pick_place.py.

References solve_near_seed().

Referenced by main().

◆ rnea_torque()

list[float] ex_rnea_pick_place.rnea_torque	(	robot,
		rnea,
		n_segments,
		q_des )

Definition at line 221 of file ex_rnea_pick_place.py.

References jnt().

Referenced by run_phase().

◆ run_phase()

bool ex_rnea_pick_place.run_phase	(		env,
			robots,
			fingers,
			solvers,
		Phase	phase,
		dict	state )

One update() per step for both arms: it reads the state and applies the previous
cycle's command.

Definition at line 249 of file ex_rnea_pick_place.py.

References measure(), and rnea_torque().

Referenced by main().

◆ solve_near_seed()

list[float] ex_rnea_pick_place.solve_near_seed	(		chain,
			limits,
		list[float]	seed,
		kdl.Frame	target )

Damped-least-squares IK stepped from seed, so the answer stays on the seed's branch.

Definition at line 158 of file ex_rnea_pick_place.py.

References jnt().

Referenced by pick_place_phases().

◆ world_T_base()

kdl.Frame ex_rnea_pick_place.world_T_base ( int arm )

Definition at line 61 of file ex_rnea_pick_place.py.

Referenced by cube_spot().

Variable Documentation

◆ BASES

list ex_rnea_pick_place.BASES = [(-0.70, -0.12, 0.0), (0.70, 0.12, math.pi)]

Definition at line 31 of file ex_rnea_pick_place.py.

◆ CAMERAS

list ex_rnea_pick_place.CAMERAS

Initial value:

=  [
    ("overview", [0.0, -1.3, 1.9], [0.410747, 0.0, 0.0, 0.911749], 55.0),
    ("side", [0.0, 1.5, 1.15], [0.0, 0.633989, 0.773342, 0.0], 50.0),
]

Definition at line 41 of file ex_rnea_pick_place.py.

◆ CUBE_HS

float ex_rnea_pick_place.CUBE_HS = 0.02

Definition at line 19 of file ex_rnea_pick_place.py.

◆ FREE_OBJECTS

list ex_rnea_pick_place.FREE_OBJECTS

Initial value:

=  [
    ("red_box", mjk.Shape.BOX, 0.0, 0.42, 0.03, (1.0, 0.2, 0.2)),
    ("green_box", mjk.Shape.BOX, 0.0, -0.42, 0.03, (0.2, 1.0, 0.2)),
    ("yellow_box", mjk.Shape.BOX, -0.45, 0.42, 0.04, (1.0, 0.85, 0.1)),
    ("orange_sphere", mjk.Shape.SPHERE, 0.45, -0.42, 0.035, (1.0, 0.55, 0.0)),
    ("purple_sphere", mjk.Shape.SPHERE, 0.0, 0.0, 0.025, (0.7, 0.0, 0.9)),
]

Definition at line 33 of file ex_rnea_pick_place.py.

◆ GRIPPER_CLOSED

float ex_rnea_pick_place.GRIPPER_CLOSED = 0.82

Definition at line 25 of file ex_rnea_pick_place.py.

◆ HOME

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

Definition at line 17 of file ex_rnea_pick_place.py.

◆ KD

list ex_rnea_pick_place.KD = [20.0, 28.0, 20.0, 28.0, 20.0, 28.0, 20.0]

Definition at line 24 of file ex_rnea_pick_place.py.

◆ KP

list ex_rnea_pick_place.KP = [100.0, 200.0, 100.0, 200.0, 100.0, 200.0, 100.0]

Definition at line 23 of file ex_rnea_pick_place.py.

◆ MAX_DISTURB

float ex_rnea_pick_place.MAX_DISTURB = 0.001

Definition at line 28 of file ex_rnea_pick_place.py.

◆ MAX_PLACE_ERR

float ex_rnea_pick_place.MAX_PLACE_ERR = 0.005

Definition at line 26 of file ex_rnea_pick_place.py.

◆ MIN_ELBOW_HEIGHT

float ex_rnea_pick_place.MIN_ELBOW_HEIGHT = 0.45

Definition at line 27 of file ex_rnea_pick_place.py.

◆ PICK

tuple ex_rnea_pick_place.PICK = (0.40, 0.00)

Definition at line 20 of file ex_rnea_pick_place.py.

◆ PLACE

tuple ex_rnea_pick_place.PLACE = (0.40, 0.24)

Definition at line 21 of file ex_rnea_pick_place.py.

◆ PREFIXES

list ex_rnea_pick_place.PREFIXES = ["", "r2_"]

Definition at line 32 of file ex_rnea_pick_place.py.

◆ SURFACE_Z

float ex_rnea_pick_place.SURFACE_Z = 0.70

Definition at line 18 of file ex_rnea_pick_place.py.

Classes

Functions

Variables

Detailed Description

Function Documentation

◆ build_env()

◆ camera()

◆ cube_object()

◆ cube_spot()

◆ free_object()

◆ gravity()

◆ jnt()

◆ main()

◆ measure()

◆ new_metrics()

◆ pick_place_phases()

◆ rnea_torque()

◆ run_phase()

◆ solve_near_seed()

◆ world_T_base()

Variable Documentation

◆ BASES

◆ CAMERAS

◆ CUBE_HS

◆ FREE_OBJECTS

◆ GRIPPER_CLOSED

◆ HOME

◆ KD

◆ KP

◆ MAX_DISTURB

◆ MAX_PLACE_ERR

◆ MIN_ELBOW_HEIGHT

◆ PICK

◆ PLACE

◆ PREFIXES

◆ SURFACE_Z