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 char *tool_body=nullptr)
void	mj_kdl::cleanup (Robot *r)
void	mj_kdl::step (Robot *s)
void	mj_kdl::step_n (Robot *s, int n)
void	mj_kdl::reset (Robot *s)
void	mj_kdl::update (Robot *r)
void	mj_kdl::set_joint_pos (Robot *r, const KDL::JntArray &q, bool call_forward=true)

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(), DualArmTest::TearDown(), InitTest::TearDown(), MjcfLoadTest::TearDown(), MjcfGripperTest::TearDown(), MjcfPickTest::TearDown(), MjcfPosCtrlTest::TearDown(), MjcfTrqCtrlTest::TearDown(), MjcfVelCtrlTest::TearDown(), TableSceneTest::TearDown(), 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 char *	tool_body = nullptr
	)

Build KDL chain from a compiled MuJoCo model (no URDF required). Traverses the body tree from base_body to tip_body.

When tool_body is provided, all bodies in the subtree rooted at tool_body are collected and their mass/inertia is lumped into a single fixed KDL segment appended to the chain. This makes KDL dynamics (ChainDynParam::JntToGravity, JntToMass, etc.) account for the tool's full inertia without adding any controllable joints. n_joints remains equal to the number of hinge/slide joints between base_body and tip_body.

Parameters

[out]	r	Robot populated with chain, joint_names, joint_limits, index maps.
[in]	model	Compiled MuJoCo model.
[in]	data	MuJoCo data pointer (mj_forward is called internally if tool_body is set).
[in]	base_body	Name of the chain root body (not included as a segment).
[in]	tip_body	Name of the chain end body (last controllable link).
[in]	prefix	Optional body/joint name prefix for multi-robot disambiguation.
[in]	tool_body	Optional root of the tool subtree whose inertia is appended as a fixed segment. Pass the gripper base body (e.g. "g_base") so that KDL dynamics include the full gripper mass.

Returns

true on success.

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

reset()

void mj_kdl::reset

(

Robot *

s

)

Reset simulation to the model's keyframe 0 (or default pose).

Parameters

[in,out]

s

Simulation state.

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(), MjcfPickTest::reset_scene(), MjcfLoadTest::SetUp(), MjcfPosCtrlTest::SetUp(), MjcfTrqCtrlTest::SetUp(), MjcfVelCtrlTest::SetUp(), TableSceneTest::SetUp(), TEST_F(), TEST_F(), TEST_F(), and TEST_F().

step()

void mj_kdl::step

(

Robot *

s

)

Advance the simulation by one timestep and call mj_forward().

Parameters

[in,out]

s

Simulation state.

Referenced by main(), TEST_F(), TEST_F(), TEST_F(), TEST_F(), and TEST_F().

step_n()

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

Advance the simulation by n timesteps.

Parameters

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

Referenced by 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_bias -> 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(), MjcfPickTest::reset_scene(), MjcfPickTest::run_phase(), MjcfPosCtrlTest::SetUp(), MjcfVelCtrlTest::SetUp(), TEST_F(), TEST_F(), TEST_F(), and TEST_F().