Robot Initialization and Control

Initialize KDL chains, read sensors, and apply joint commands. More...

Functions
bool	mj_kdl::init_robot_from_mjcf (Robot *r, mjModel *model, mjData *data, const char *base_body, const char *tip_body, const char *prefix="", const ToolFrameSpec *tool=nullptr)
void	mj_kdl::cleanup (Robot *r)
bool	mj_kdl::step (Robot *s)
bool	mj_kdl::step_n (Robot *s, int n)
void	mj_kdl::update (Robot *r)
void	mj_kdl::set_joint_pos (Robot *r, const KDL::JntArray &q, bool call_forward=true)
void	mj_kdl::set_body_pose (mjModel *model, mjData *data, const char *body_name, const double pos[3], const double *quat=nullptr)

Detailed Description

Initialize KDL chains, read sensors, and apply joint commands.

Call init_robot_from_mjcf() once after build_scene(), then call update() each control step to exchange data with MuJoCo.

Function Documentation

cleanup()

void mj_kdl::cleanup

(

Robot *

r

)

Zero all Robot fields. Does not free model or data; call destroy_scene() for that.

Parameters

[in,out]

r

Robot to tear down.

Referenced by main(), main(), DualArmTest::TearDown(), InitTest::TearDown(), MjcfLoadTest::TearDown(), MjcfGripperTest::TearDown(), MjcfPickTest::TearDown(), MjcfPosCtrlTest::TearDown(), MjcfTrqCtrlTest::TearDown(), MjcfVelCtrlTest::TearDown(), TableSceneTest::TearDown(), TEST_F(), TEST_F(), and TEST_F().

init_robot_from_mjcf()

bool mj_kdl::init_robot_from_mjcf	(	Robot *	r,
		mjModel *	model,
		mjData *	data,
		const char *	base_body,
		const char *	tip_body,
		const char *	prefix = "",
		const ToolFrameSpec *	tool = nullptr
	)

Build KDL chain from a compiled MuJoCo model and optional tool/TCP metadata.

If tool->tcp_site is set, that authored site becomes the KDL terminal frame for FK/IK. The joint count and MuJoCo joint/actuator maps still cover only the controllable joints from base_body to tip_body. Pass tool = nullptr (the default) for an arm with no attached tool.

Referenced by main(), main(), DualArmTest::SetUp(), InitTest::SetUp(), MjcfLoadTest::SetUp(), MjcfGripperTest::SetUp(), MjcfPickTest::SetUp(), MjcfPosCtrlTest::SetUp(), MjcfTrqCtrlTest::SetUp(), MjcfVelCtrlTest::SetUp(), TableSceneTest::SetUp(), and TEST_F().

set_body_pose()

void mj_kdl::set_body_pose	(	mjModel *	model,
		mjData *	data,
		const char *	body_name,
		const double	pos[3],
		const double *	quat = nullptr
	)

Teleport a free-floating body to a world-frame position and optionally a world-frame orientation, then zero its velocity. body_name must identify a body that owns a mjJNT_FREE joint. quat is MuJoCo convention [w, x, y, z]; pass nullptr to keep identity orientation.

Parameters

[in,out]	model	MuJoCo model.
[in,out]	data	MuJoCo data.
[in]	body_name	Name of the free-floating body to teleport.
[in]	pos	World-frame position [x, y, z].
[in]	quat	World-frame orientation [w, x, y, z], or nullptr for identity.

Referenced by main().

set_joint_pos()

void mj_kdl::set_joint_pos	(	Robot *	r,
		const KDL::JntArray &	q,
		bool	call_forward = true
	)

Write KDL joint positions into MuJoCo qpos (KDL chain order -> MuJoCo addresses).

Parameters

[in,out]	r	Robot with a valid data pointer.
[in]	q	Joint positions in KDL chain order; size must equal r->n_joints.
[in]	call_forward	If true (default), calls mj_forward() after writing qpos so that body poses and sensor data are updated immediately.

Referenced by main(), main(), MjcfPickTest::reset_scene(), MjcfLoadTest::SetUp(), MjcfPosCtrlTest::SetUp(), MjcfTrqCtrlTest::SetUp(), MjcfVelCtrlTest::SetUp(), TableSceneTest::SetUp(), TEST_F(), TEST_F(), TEST_F(), TEST_F(), and TEST_F().

step()

bool mj_kdl::step

(

Robot *

s

)

Advance one physics timestep.

Headless (no viewer active): calls mj_step() and returns true. GUI (init_window_sim() was called): advances physics, renders, syncs to real time, and polls GLFW events – exactly what tick() used to do. Returns false once the user closes the window.

This replaces the old headless/GUI split:

// Before: if (headless) { mj_kdl::step(&r); } else if (!mj_kdl::tick(&v, m, d)) break;

// After: if (!mj_kdl::step(&r)) break;

Coupling note: in GUI mode the keyboard and mouse perturbation callbacks operate on the Robot registered via init_window_sim(). Always pass the same Robot to both init_window_sim() and step(); passing a different Robot causes perturbation forces to be applied to the wrong model.

Parameters

[in,out]

s

Simulation state; must be the Robot passed to init_window_sim().

Returns

true while the window is open (or always true in headless mode).

Referenced by main(), main(), MjcfPickTest::run_phase(), TEST_F(), TEST_F(), TEST_F(), TEST_F(), TEST_F(), and TEST_F().

step_n()

bool mj_kdl::step_n	(	Robot *	s,
		int	n
	)

Advance the simulation by n timesteps. Returns false immediately if the viewer window is closed mid-sequence.

Parameters

[in,out]	s	Simulation state.
[in]	n	Number of steps.

Referenced by TEST_F(), and TEST_F().

update()

void mj_kdl::update

(

Robot *

r

)

One control cycle: read MuJoCo into *_msr, then apply *_cmd to MuJoCo. Read step: qpos -> jnt_pos_msr, qvel -> jnt_vel_msr, qfrc_actuator -> jnt_trq_msr. Apply step: POSITION -> data->ctrl, TORQUE -> qfrc_applied; also sets ctrl = qpos to neutralize position actuators (zeroes kp*(ctrl-qpos) restoring force). Joints with kdl_to_mj_ctrl[i] == -1 are skipped for ctrl writes.

Referenced by main(), main(), MjcfPickTest::reset_scene(), MjcfPickTest::run_phase(), MjcfPosCtrlTest::SetUp(), MjcfVelCtrlTest::SetUp(), TEST_F(), TEST_F(), TEST_F(), TEST_F(), TEST_F(), TEST_F(), and TEST_F().