admittance_ft Namespace Reference

Classes
struct	AdmState
struct	SceneHandles
class	Controller
struct	Metrics

Functions
double	clamp (double v, double lo, double hi)
KDL::JntArray	home_q (unsigned n)
KDL::Vector	vclamp (const KDL::Vector &v, double limit)
double	norm3 (const KDL::Vector &v)
KDL::Rotation	mj_xmat_to_kdl_rot (const double *m)
KDL::Frame	site_frame_by_id (const mjModel *model, const mjData *data, int id)
KDL::Frame	current_tcp (KDL::ChainFkSolverPos_recursive &fk, const mj_kdl::Robot &robot)
KDL::Vector	spiral_force (double t)
void	admittance_update (AdmState &s, const KDL::Vector &force, double dt)
void	set_body_wrench (mjModel *model, mjData *data, const char *body, const KDL::Vector &force)
void	close_gripper (mjModel *model, mjData *data)
KDL::Vector	tare_force (const mj_kdl::Robot &robot)
KDL::Vector	measured_force (const mj_kdl::Robot &robot, const AdmState &s)
bool	build_scene (SceneHandles &h)
KDL::Frame	admittance_step (mj_kdl::Robot &robot, Controller &ctrl, AdmState &state, const KDL::Frame &nominal, const KDL::Vector &force, double dt)
void	settle_and_tare (SceneHandles &h, Controller &ctrl, AdmState &state)
Metrics	run_selfcheck (SceneHandles &h, Controller &ctrl, AdmState &state, const KDL::Frame &nominal)
int	finish_headless (const Metrics &m)
void	run_gui (SceneHandles &h, Controller &ctrl, AdmState &state, const KDL::Frame &nominal)
int	run (int argc, char **argv, std::unique_ptr< Controller >(*make_controller)(SceneHandles &))

Function Documentation

admittance_step()

KDL::Frame admittance_ft::admittance_step ( mj_kdl::Robot & robot, Controller & ctrl, AdmState & state, const KDL::Frame & nominal, const KDL::Vector & force, double dt )

inline

Definition at line 237 of file admittance_ft_common.hpp.

References admittance_update(), admittance_ft::AdmState::offset, and admittance_ft::Controller::track().

Referenced by run_gui(), and run_selfcheck().

admittance_update()

void admittance_ft::admittance_update ( AdmState & s, const KDL::Vector & force, double dt )

inline

Definition at line 103 of file admittance_ft_common.hpp.

References norm3(), admittance_ft::AdmState::offset, vclamp(), and admittance_ft::AdmState::vel.

Referenced by admittance_step().

build_scene()

bool admittance_ft::build_scene ( SceneHandles & h )

inline

Definition at line 170 of file admittance_ft_common.hpp.

References mj_kdl::SceneSpec::add_floor, mj_kdl::SceneSpec::add_skybox, mj_kdl_examples::asset(), mj_kdl::AttachmentSpec::attach_to, mj_kdl::RobotSpec::attach_to, mj_kdl::RobotSpec::attachments, mj_kdl::build_scene(), admittance_ft::SceneHandles::data, mj_kdl::Env::data, admittance_ft::SceneHandles::env, mj_kdl::env_add_robot(), mj_kdl::SceneObject::fixed, mj_kdl::ForceTorqueSensorSpec::frame_site, mj_kdl::ToolFrameSpec::ft_sensors, mj_kdl::init_robot_from_mjcf(), mj_kdl_examples::menagerie_model(), mj_kdl::AttachmentSpec::mjcf_path, mj_kdl::SceneObject::mjcf_path, admittance_ft::SceneHandles::model, mj_kdl::Env::model, mj_kdl::ForceTorqueSensorSpec::name, mj_kdl::SceneObject::name, mj_kdl::SceneSpec::objects, mj_kdl::RobotSpec::path, mj_kdl::SceneObject::pos, mj_kdl::AttachmentSpec::prefix, admittance_ft::SceneHandles::robot, mj_kdl::SceneSpec::robots, admittance_ft::SceneHandles::scene, mj_kdl::scene_object_site_name(), mj_kdl::Site, mj_kdl::Env::spec, mj_kdl::ToolFrameSpec::tcp_site, mj_kdl::SceneSpec::timestep, mj_kdl::ToolFrameSpec::tool_body, and admittance_ft::SceneHandles::tool_body_id.

Referenced by run().

clamp()

double admittance_ft::clamp ( double v, double lo, double hi )

inline

Definition at line 42 of file admittance_ft_common.hpp.

Referenced by vclamp().

close_gripper()

void admittance_ft::close_gripper ( mjModel * model, mjData * data )

inline

Definition at line 130 of file admittance_ft_common.hpp.

Referenced by run_gui(), run_selfcheck(), and settle_and_tare().

current_tcp()

KDL::Frame admittance_ft::current_tcp ( KDL::ChainFkSolverPos_recursive & fk, const mj_kdl::Robot & robot )

inline

Definition at line 75 of file admittance_ft_common.hpp.

References mj_kdl::Robot::jnt_pos_msr, and mj_kdl::Robot::n_joints.

Referenced by run(), run_gui(), run_selfcheck(), and settle_and_tare().

finish_headless()

int admittance_ft::finish_headless ( const Metrics & m )

inline

Definition at line 358 of file admittance_ft_common.hpp.

References admittance_ft::Metrics::handoff_force, admittance_ft::Metrics::helix_react, admittance_ft::Metrics::helix_settle_err, admittance_ft::Metrics::helix_track_err, admittance_ft::Metrics::hold_drift, admittance_ft::Metrics::push_dy, admittance_ft::Metrics::push_recovery_err, and admittance_ft::Metrics::push_response.

Referenced by run().

home_q()

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

inline

Definition at line 44 of file admittance_ft_common.hpp.

Referenced by run().

measured_force()

KDL::Vector admittance_ft::measured_force ( const mj_kdl::Robot & robot, const AdmState & s )

inline

Definition at line 143 of file admittance_ft_common.hpp.

References admittance_ft::AdmState::bias, mj_kdl::Robot::data, mj_kdl::find_ft_sensor(), mj_kdl::ForceTorqueSensor::frame_site_id, mj_kdl::Robot::model, norm3(), site_frame_by_id(), and mj_kdl::ForceTorqueSensor::wrench.

Referenced by run_gui(), and run_selfcheck().

mj_xmat_to_kdl_rot()

KDL::Rotation admittance_ft::mj_xmat_to_kdl_rot ( const double * m )

inline

Definition at line 62 of file admittance_ft_common.hpp.

Referenced by site_frame_by_id().

norm3()

double admittance_ft::norm3 ( const KDL::Vector & v )

inline

Definition at line 60 of file admittance_ft_common.hpp.

Referenced by admittance_update(), measured_force(), and run_selfcheck().

run()

int admittance_ft::run ( int argc, char ** argv, std::unique_ptr< Controller >(* make_controller )(SceneHandles &) )

inline

Definition at line 439 of file admittance_ft_common.hpp.

References admittance_ft::AdmState::bias, build_scene(), mj_kdl::Robot::chain, admittance_ft::SceneHandles::cleanup(), mj_kdl::Robot::ctrl_mode, current_tcp(), admittance_ft::SceneHandles::data, admittance_ft::SceneHandles::env, finish_headless(), home_q(), admittance_ft::SceneHandles::model, mj_kdl::Robot::n_joints, admittance_ft::AdmState::offset, mj_kdl::Env::on_reset, mj_kdl::reset(), admittance_ft::SceneHandles::robot, run_gui(), run_selfcheck(), set_body_wrench(), mj_kdl::set_joint_pos(), and settle_and_tare().

Referenced by main().

run_gui()

void admittance_ft::run_gui ( SceneHandles & h, Controller & ctrl, AdmState & state, const KDL::Frame & nominal )

inline

Definition at line 378 of file admittance_ft_common.hpp.

References mj_kdl::add_trace_segment(), admittance_step(), admittance_ft::AdmState::bias, mj_kdl::Robot::chain, mj_kdl::cleanup(), close_gripper(), current_tcp(), admittance_ft::SceneHandles::data, admittance_ft::SceneHandles::env, mj_kdl::get_body_frame(), mj_kdl::init_window_sim(), mj_kdl::is_running(), measured_force(), admittance_ft::SceneHandles::model, admittance_ft::Controller::name(), admittance_ft::Controller::reset(), mj_kdl::reset(), admittance_ft::SceneHandles::robot, admittance_ft::SceneHandles::scene, set_body_wrench(), mj_kdl::set_free_camera(), spiral_force(), mj_kdl::step(), tare_force(), mj_kdl::SceneSpec::timestep, and mj_kdl::update().

Referenced by run().

run_selfcheck()

Metrics admittance_ft::run_selfcheck ( SceneHandles & h, Controller & ctrl, AdmState & state, const KDL::Frame & nominal )

inline

Definition at line 280 of file admittance_ft_common.hpp.

References admittance_step(), admittance_ft::AdmState::bias, mj_kdl::Robot::chain, close_gripper(), current_tcp(), admittance_ft::SceneHandles::data, admittance_ft::Metrics::handoff_force, admittance_ft::Metrics::helix_react, admittance_ft::Metrics::helix_settle_err, admittance_ft::Metrics::helix_track_err, admittance_ft::Metrics::hold_drift, measured_force(), admittance_ft::SceneHandles::model, norm3(), admittance_ft::AdmState::offset, admittance_ft::Metrics::push_dy, admittance_ft::Metrics::push_recovery_err, admittance_ft::Metrics::push_response, admittance_ft::SceneHandles::robot, admittance_ft::SceneHandles::scene, set_body_wrench(), spiral_force(), mj_kdl::step(), tare_force(), mj_kdl::SceneSpec::timestep, and mj_kdl::update().

Referenced by run().

set_body_wrench()

void admittance_ft::set_body_wrench ( mjModel * model, mjData * data, const char * body, const KDL::Vector & force )

inline

Definition at line 118 of file admittance_ft_common.hpp.

Referenced by run(), run_gui(), and run_selfcheck().

settle_and_tare()

void admittance_ft::settle_and_tare ( SceneHandles & h, Controller & ctrl, AdmState & state )

inline

Definition at line 253 of file admittance_ft_common.hpp.

References admittance_ft::AdmState::bias, mj_kdl::Robot::chain, close_gripper(), current_tcp(), admittance_ft::SceneHandles::data, admittance_ft::SceneHandles::model, admittance_ft::SceneHandles::robot, mj_kdl::step(), tare_force(), admittance_ft::Controller::track(), and mj_kdl::update().

Referenced by run().

site_frame_by_id()

KDL::Frame admittance_ft::site_frame_by_id ( const mjModel * model, const mjData * data, int id )

inline

Definition at line 67 of file admittance_ft_common.hpp.

References mj_xmat_to_kdl_rot().

Referenced by measured_force(), and tare_force().

spiral_force()

KDL::Vector admittance_ft::spiral_force ( double t )

inline

Definition at line 91 of file admittance_ft_common.hpp.

Referenced by run_gui(), and run_selfcheck().

tare_force()

KDL::Vector admittance_ft::tare_force ( const mj_kdl::Robot & robot )

inline

Definition at line 136 of file admittance_ft_common.hpp.

References mj_kdl::Robot::data, mj_kdl::find_ft_sensor(), mj_kdl::ForceTorqueSensor::frame_site_id, mj_kdl::Robot::model, site_frame_by_id(), and mj_kdl::ForceTorqueSensor::wrench.

Referenced by run_gui(), run_selfcheck(), and settle_and_tare().

vclamp()

KDL::Vector admittance_ft::vclamp ( const KDL::Vector & v, double limit )

inline

Definition at line 51 of file admittance_ft_common.hpp.

References clamp().

Referenced by admittance_update().