Initialize KDL chains and command robots through their ports. More...

Functions
Status	mj_kdl::init_robot_from_mjcf (Robot r, Env env, const char base_body, const char tip_body, const char prefix="", const ToolFrameSpec tool=nullptr)
Status	mj_kdl::init_robot_from_chain (Robot r, Env env, const KDL::Chain &chain, const std::vector< std::string > &joint_names, const char prefix="", const ToolFrameSpec tool=nullptr)
std::vector< double >	mj_kdl::joint_force_limits (const Robot *r, double fallback=1e6)
void	mj_kdl::cleanup (Robot *r)
Status	mj_kdl::set_control_mode (Robot *r, CtrlMode mode)
Status	mj_kdl::set_control_mode (Env *env, int robot, CtrlMode mode)
void	mj_kdl::set_joint_pos (Robot *r, const KDL::JntArray &q)

Detailed Description

Initialize KDL chains and command robots through their ports.

Call init_robot_from_mjcf() once after init_env(); it registers the robot with the Env, whose update() then reads and commands it every control step.

Function Documentation

◆ cleanup()

void mj_kdl::cleanup ( Robot * r )

Unregister r from its Env and clear it. The Env keeps running without it.

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

◆ init_robot_from_chain()

Status mj_kdl::init_robot_from_chain	(	Robot *	r,
		Env *	env,
		const KDL::Chain &	chain,
		const std::vector< std::string > &	joint_names,
		const char *	prefix = "",
		const ToolFrameSpec *	tool = nullptr )

Adopt an externally built KDL chain and wire it to a compiled MuJoCo model.

Same as init_robot_from_mjcf(), except the chain is supplied rather than derived from the model: use this when the chain comes from the scene description that also produced the MJCF, so the solvers compute with the authored dynamics. The chain is taken as given - no tool inertia is lumped onto it, since such a chain already carries its tool as explicit segments.

joint_names lists the MuJoCo joint names in KDL chain order, one per chain joint; they drive the same qpos/dof/ctrl index maps init_robot_from_mjcf() builds. prefix is prepended to each joint name and F/T sensor name. tool is used only to resolve FT sensors; tool->tool_body and tool->tcp_site are ignored. Registers r with env, as init_robot_from_mjcf().

Referenced by TEST_F(), and TEST_F().

◆ init_robot_from_mjcf()

Status mj_kdl::init_robot_from_mjcf	(	Robot *	r,
		Env *	env,
		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. prefix is prepended to every name the call resolves: base_body, tip_body, the tool body, TCP site and F/T sensor names, e.g. ("base_link", "bracelet_link", "r2_") is the second arm. Registers r with env, which reads, commands and resets it from then on; on failure r is left as it was.

Referenced by main(), main(), ArmModesTest::SetUp(), AttachedArmModesTest::SetUp(), DualArmTest::SetUp(), InitTest::SetUp(), MjcfLoadTest::SetUp(), MjcfPickTest::SetUp(), MjcfPosCtrlTest::SetUp(), MjcfTrqCtrlTest::SetUp(), RebuildTest::SetUp(), SceneStateTest::SetUp(), TableSceneTest::SetUp(), TEST(), TEST(), TEST(), TEST(), TEST(), TEST_F(), TEST_F(), TEST_F(), TEST_F(), TEST_F(), TEST_F(), TEST_F(), TEST_F(), TEST_F(), TEST_F(), TEST_F(), TEST_F(), TEST_P(), and TEST_P().

◆ joint_force_limits()

std::vector< double > mj_kdl::joint_force_limits	(	const Robot *	r,
		double	fallback = 1e6 )

Parameters

r	Initialized robot (init_robot_from_mjcf() already called).
fallback	Bound used for joints without a force-limited actuator; default is a large, effectively non-limiting value.

Returns: One entry per KDL joint, same order as r->joint_names.

Referenced by TEST_F(), and TEST_P().

◆ set_control_mode() [1/2]

Status mj_kdl::set_control_mode	(	Env *	env,
		int	robot,
		CtrlMode	mode )

The same switch for a robot driven outside a Robot chain (e.g. through env.scene): robot is its index in SceneSpec::robots.

Returns: an error if it has no actuators for mode.

◆ set_control_mode() [2/2]

Status mj_kdl::set_control_mode	(	Robot *	r,
		CtrlMode	mode )

Switch the robot to mode: seed the new actuators so nothing jumps, enable their group, disable the robot's other mode groups.

Returns: an error if the robot has no actuator for mode.

Referenced by main(), main(), MjcfPosCtrlTest::SetUp(), TEST(), TEST(), TEST_F(), TEST_F(), TEST_F(), TEST_F(), TEST_F(), TEST_F(), TEST_F(), TEST_F(), TEST_F(), TEST_F(), TEST_F(), TEST_P(), TEST_P(), TEST_P(), TEST_P(), TEST_P(), and TEST_P().

◆ set_joint_pos()

void mj_kdl::set_joint_pos	(	Robot *	r,
		const KDL::JntArray &	q )

Write KDL joint positions into MuJoCo qpos (KDL chain order -> MuJoCo addresses). Frames read afterwards follow the new positions.