Classes
struct	Args
struct	PickPlaceWaypoints
struct	Phase
struct	PhaseArm

Functions
Args	parse_args (int argc, char **argv, const std::string &default_record_path="")
int	verdict (bool ok, const std::string &goal)
double	clamp01 (double v)
double	clamp_abs (double v, double limit)
KDL::JntArray	home_q (unsigned n)
void	lerp_q (const KDL::JntArray &a, const KDL::JntArray &b, double t, KDL::JntArray &out)
void	read_q (const mj_kdl::Robot &robot, KDL::JntArray &q)
void	read_q (const mj_kdl::Robot &robot, KDL::JntArray &q, KDL::JntArray &qd)
double	max_abs_joint_err (const mj_kdl::Robot &robot, const KDL::JntArray &q)
KDL::Frame	tcp_frame (KDL::ChainFkSolverPos_recursive &fk, const mj_kdl::Robot &robot)
void	pd_gravity (mj_kdl::Robot &robot, KDL::ChainDynParam &dyn, const KDL::JntArray &q_des, const double kp=nullptr, const double kd=nullptr)
void	prime_gravity (mj_kdl::Robot &robot, KDL::ChainDynParam &dyn, const KDL::JntArray &q)
mj_kdl::SceneSpec	scene_spec ()
mj_kdl::AttachmentSpec	gripper_attachment (const std::string &path, const std::string &site="pinch_site")
mj_kdl::ToolFrameSpec	gripper_tool (const std::string &prefix="")
mj_kdl::SceneObject	table_object (const std::string &path, double surface_z)
mj_kdl::SceneObject	cube_object (double x, double y, double surface_z)
void	place_cube (mj_kdl::Env &env)
bool	solve_near_seed (KDL::ChainIkSolverVel_wdls &ik_vel, KDL::ChainFkSolverPos_recursive &fk, const mj_kdl::Robot &robot, const KDL::JntArray &seed, const KDL::Frame &target, KDL::JntArray &out)
bool	solve_pick_place (const mj_kdl::Robot &robot, PickPlaceWaypoints &w)
std::vector< Phase >	pick_place_phases (const PickPlaceWaypoints &w)
bool	run_phases (mj_kdl::Env &env, const std::vector< PhaseArm > &arms, const std::vector< Phase > &phases, bool &restart, const std::function< void(std::size_t, const KDL::JntArray &)> &control, const std::function< void(const Phase &, double t_rel)> &after_step={})
std::string	find_asset (const fs::path &relative)
std::string	asset (const fs::path &relative)

Variables
constexpr double	kGripperClosed = 0.82
constexpr double	kHomePose [7] = { 0.0, 0.2618, 3.1416, -2.2689, 0.0, 0.9599, 1.5708 }
constexpr double	kKp [7] = { 100, 200, 100, 200, 100, 200, 100 }
constexpr double	kKd [7] = { 10, 20, 10, 20, 10, 20, 10 }
constexpr double	kTableZ = 0.70
constexpr double	kCubeHS = 0.02
constexpr double	kPickXY [2] = { 0.40, 0.00 }
constexpr double	kPlaceXY [2] = { 0.40, 0.24 }

Function Documentation

◆ asset()

std::string mj_kdl_examples::asset ( const fs::path & relative )

inline

Definition at line 16 of file example_paths.hpp.

References find_asset().

Referenced by main(), and main().

◆ clamp01()

double mj_kdl_examples::clamp01 ( double v )

inline

Definition at line 64 of file common.hpp.

Referenced by main(), and TEST_F().

◆ clamp_abs()

double mj_kdl_examples::clamp_abs	(	double	v,
		double	limit )

inline

Definition at line 66 of file common.hpp.

Referenced by AchdController::control(), main(), main(), and TEST().

◆ cube_object()

mj_kdl::SceneObject mj_kdl_examples::cube_object	(	double	x,
		double	y,
		double	surface_z )

inline

Definition at line 178 of file common.hpp.

References mj_kdl::BOX, mj_kdl::SceneObject::condim, mj_kdl::SceneObject::friction, kCubeHS, mj_kdl::SceneObject::mass, mj_kdl::SceneObject::name, mj_kdl::SceneObject::pos, mj_kdl::SceneObject::rgba, mj_kdl::SceneObject::shape, mj_kdl::SceneObject::size, and mj_kdl::Torsional.

Referenced by main(), and MjcfPickTest::SetUp().

◆ find_asset()

std::string mj_kdl_examples::find_asset ( const fs::path & relative )

inline

Definition at line 10 of file example_paths.hpp.

Referenced by asset(), ArmModesTest::SetUp(), AttachedArmModesTest::SetUp(), DualArmTest::SetUp(), MjcfFtSensorTest::SetUp(), MjcfGripperTest::SetUp(), MjcfLoadTest::SetUp(), MjcfPickTest::SetUp(), MjcfPosCtrlTest::SetUp(), MjcfTrqCtrlTest::SetUp(), RebuildTest::SetUp(), SceneStateTest::SetUp(), TableSceneTest::SetUp(), TEST(), TEST(), TEST(), TEST(), and TEST().

◆ gripper_attachment()

mj_kdl::AttachmentSpec mj_kdl_examples::gripper_attachment	(	const std::string &	path,
		const std::string &	site = "pinch_site" )

inline

Definition at line 149 of file common.hpp.

References mj_kdl::AttachmentSpec::attach_to, mj_kdl::AttachmentSpec::mjcf_path, mj_kdl::AttachmentSpec::prefix, and mj_kdl::Site.

Referenced by main(), MjcfFtSensorTest::SetUp(), MjcfGripperTest::SetUp(), MjcfPickTest::SetUp(), MjcfTrqCtrlTest::SetUp(), and TEST().

◆ gripper_tool()

mj_kdl::ToolFrameSpec mj_kdl_examples::gripper_tool ( const std::string & prefix = "" )

inline

Definition at line 159 of file common.hpp.

References mj_kdl::ToolFrameSpec::tcp_site, and mj_kdl::ToolFrameSpec::tool_body.

Referenced by main(), MjcfPickTest::SetUp(), MjcfTrqCtrlTest::SetUp(), TEST_F(), and MjcfFtSensorTest::tool_with().

◆ home_q()

KDL::JntArray mj_kdl_examples::home_q ( unsigned n )

inline

Definition at line 68 of file common.hpp.

References kHomePose.

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

◆ lerp_q()

void mj_kdl_examples::lerp_q	(	const KDL::JntArray &	a,
		const KDL::JntArray &	b,
		double	t,
		KDL::JntArray &	out )

inline

Definition at line 75 of file common.hpp.

References KDL::JntArray::rows().

◆ max_abs_joint_err()

double mj_kdl_examples::max_abs_joint_err	(	const mj_kdl::Robot &	robot,
		const KDL::JntArray &	q )

inline

Definition at line 93 of file common.hpp.

References mj_kdl::RobotPorts::jnt_pos_msr, and KDL::JntArray::rows().

◆ parse_args()

Args mj_kdl_examples::parse_args	(	int	argc,
		char **	argv,
		const std::string &	default_record_path = "" )

inline

Definition at line 41 of file common.hpp.

References mj_kdl_examples::Args::headless, mj_kdl_examples::Args::record, and mj_kdl_examples::Args::record_path.

Referenced by main(), and main().

◆ pd_gravity()

void mj_kdl_examples::pd_gravity	(	mj_kdl::Robot &	robot,
		KDL::ChainDynParam &	dyn,
		const KDL::JntArray &	q_des,
		const double *	kp = nullptr,
		const double *	kd = nullptr )

inline

Definition at line 112 of file common.hpp.

References mj_kdl::Robot::chain, KDL::Chain::getNrOfJoints(), mj_kdl::RobotPorts::jnt_trq_cmd, mj_kdl::RobotPorts::jnt_vel_msr, KDL::ChainDynParam::JntToGravity(), and read_q().

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

◆ pick_place_phases()

std::vector< Phase > mj_kdl_examples::pick_place_phases ( const PickPlaceWaypoints & w )

inline

Definition at line 292 of file common.hpp.

References mj_kdl_examples::PickPlaceWaypoints::home, kGripperClosed, mj_kdl_examples::PickPlaceWaypoints::lift, mj_kdl_examples::PickPlaceWaypoints::pick, mj_kdl_examples::PickPlaceWaypoints::pick_above, mj_kdl_examples::PickPlaceWaypoints::place, and mj_kdl_examples::PickPlaceWaypoints::place_above.

Referenced by main(), and TEST_F().

◆ place_cube()

void mj_kdl_examples::place_cube ( mj_kdl::Env & env )

inline

Definition at line 197 of file common.hpp.

References kCubeHS, kPickXY, kTableZ, and mj_kdl::set_body_pose().

Referenced by main().

◆ prime_gravity()

void mj_kdl_examples::prime_gravity	(	mj_kdl::Robot &	robot,
		KDL::ChainDynParam &	dyn,
		const KDL::JntArray &	q )

inline

Definition at line 131 of file common.hpp.

References mj_kdl::RobotPorts::jnt_trq_cmd, KDL::ChainDynParam::JntToGravity(), and KDL::JntArray::rows().

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

◆ read_q() [1/2]

void mj_kdl_examples::read_q	(	const mj_kdl::Robot &	robot,
		KDL::JntArray &	q )

inline

Definition at line 80 of file common.hpp.

References mj_kdl::RobotPorts::jnt_pos_msr, and KDL::JntArray::rows().

Referenced by RneaArm::control(), main(), main(), AchdController::measure(), pd_gravity(), and tcp_frame().

◆ read_q() [2/2]

void mj_kdl_examples::read_q	(	const mj_kdl::Robot &	robot,
		KDL::JntArray &	q,
		KDL::JntArray &	qd )

inline

Definition at line 85 of file common.hpp.

References mj_kdl::RobotPorts::jnt_pos_msr, mj_kdl::RobotPorts::jnt_vel_msr, and KDL::JntArray::rows().

◆ run_phases()

bool mj_kdl_examples::run_phases	(	mj_kdl::Env &	env,
		const std::vector< PhaseArm > &	arms,
		const std::vector< Phase > &	phases,
		bool &	restart,
		const std::function< void(std::size_t, const KDL::JntArray &)> &	control,
		const std::function< void(const Phase &, double t_rel)> &	after_step = {} )

inline

Definition at line 319 of file common.hpp.

Referenced by main(), and TEST_F().

◆ scene_spec()

mj_kdl::SceneSpec mj_kdl_examples::scene_spec ( )

inline

Definition at line 138 of file common.hpp.

References mj_kdl::SceneSpec::add_floor, mj_kdl::SceneSpec::add_skybox, and mj_kdl::SceneSpec::timestep.

Referenced by main(), and main().

◆ solve_near_seed()

bool mj_kdl_examples::solve_near_seed	(	KDL::ChainIkSolverVel_wdls &	ik_vel,
		KDL::ChainFkSolverPos_recursive &	fk,
		const mj_kdl::Robot &	robot,
		const KDL::JntArray &	seed,
		const KDL::Frame &	target,
		KDL::JntArray &	out )

inline

Definition at line 204 of file common.hpp.

References KDL::ChainIkSolverVel_wdls::CartToJnt(), KDL::diff(), KDL::ChainFkSolverPos_recursive::JntToCart(), mj_kdl::Robot::joint_limits, KDL::Vector::Norm(), KDL::Twist::rot, KDL::JntArray::rows(), and KDL::Twist::vel.

Referenced by main(), and solve_pick_place().

◆ solve_pick_place()

bool mj_kdl_examples::solve_pick_place	(	const mj_kdl::Robot &	robot,
		PickPlaceWaypoints &	w )

inline

◆ table_object()

mj_kdl::SceneObject mj_kdl_examples::table_object	(	const std::string &	path,
		double	surface_z )

inline

Definition at line 168 of file common.hpp.

References mj_kdl::SceneObject::name.

Referenced by main(), and main().

◆ tcp_frame()

KDL::Frame mj_kdl_examples::tcp_frame	(	KDL::ChainFkSolverPos_recursive &	fk,
		const mj_kdl::Robot &	robot )

inline

Definition at line 102 of file common.hpp.

References mj_kdl::Robot::chain, KDL::Chain::getNrOfJoints(), KDL::ChainFkSolverPos_recursive::JntToCart(), and read_q().

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

◆ verdict()

int mj_kdl_examples::verdict	(	bool	ok,
		const std::string &	goal )

inline

Definition at line 58 of file common.hpp.

Referenced by main(), and main().

Variable Documentation

◆ kCubeHS

double mj_kdl_examples::kCubeHS = 0.02

constexpr

Definition at line 29 of file common.hpp.

Referenced by cube_object(), main(), place_cube(), and solve_pick_place().

◆ kGripperClosed

double mj_kdl_examples::kGripperClosed = 0.82

constexpr

Definition at line 19 of file common.hpp.

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

◆ kHomePose

double mj_kdl_examples::kHomePose[7] = { 0.0, 0.2618, 3.1416, -2.2689, 0.0, 0.9599, 1.5708 }

constexpr

Definition at line 22 of file common.hpp.

Referenced by home_q(), and TEST_F().

◆ kKd

double mj_kdl_examples::kKd[7] = { 10, 20, 10, 20, 10, 20, 10 }

constexpr

Definition at line 26 of file common.hpp.

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

◆ kKp

double mj_kdl_examples::kKp[7] = { 100, 200, 100, 200, 100, 200, 100 }

constexpr

Definition at line 25 of file common.hpp.

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

◆ kPickXY

double mj_kdl_examples::kPickXY[2] = { 0.40, 0.00 }

constexpr

Definition at line 30 of file common.hpp.

Referenced by main(), place_cube(), MjcfPickTest::SetUp(), and solve_pick_place().

◆ kPlaceXY

double mj_kdl_examples::kPlaceXY[2] = { 0.40, 0.24 }

constexpr

Definition at line 31 of file common.hpp.

Referenced by main(), and solve_pick_place().

◆ kTableZ

double mj_kdl_examples::kTableZ = 0.70

constexpr

Definition at line 28 of file common.hpp.

Referenced by main(), and place_cube().

Classes

Functions

Variables

Function Documentation

◆ asset()

◆ clamp01()

◆ clamp_abs()

◆ cube_object()

◆ find_asset()

◆ gripper_attachment()

◆ gripper_tool()

◆ home_q()

◆ lerp_q()

◆ max_abs_joint_err()

◆ parse_args()

◆ pd_gravity()

◆ pick_place_phases()

◆ place_cube()

◆ prime_gravity()

◆ read_q() [1/2]

◆ read_q() [2/2]

◆ run_phases()

◆ scene_spec()

◆ solve_near_seed()

◆ solve_pick_place()

◆ table_object()

◆ tcp_frame()

◆ verdict()

Variable Documentation

◆ kCubeHS

◆ kGripperClosed

◆ kHomePose

◆ kKd

◆ kKp

◆ kPickXY

◆ kPlaceXY

◆ kTableZ