#include <AutoBalancer.h>

Inheritance diagram for AutoBalancer:

Classes
struct	ABCIKparam

Public Member Functions
bool	adjustFootSteps (const OpenHRP::AutoBalancerService::Footstep &rfootstep, const OpenHRP::AutoBalancerService::Footstep &lfootstep)

	AutoBalancer (RTC::Manager *manager)

void	distributeReferenceZMPToWrenches (const hrp::Vector3 &_ref_zmp)

bool	emergencyStop ()

bool	getAutoBalancerParam (OpenHRP::AutoBalancerService::AutoBalancerParam &i_param)

bool	getFootstepParam (OpenHRP::AutoBalancerService::FootstepParam &i_param)

bool	getGaitGeneratorParam (OpenHRP::AutoBalancerService::GaitGeneratorParam &i_param)

bool	getGoPosFootstepsSequence (const double &x, const double &y, const double &th, OpenHRP::AutoBalancerService::FootstepsSequence_out o_footstep)

bool	getRemainingFootstepSequence (OpenHRP::AutoBalancerService::FootstepSequence_out o_footstep, CORBA::Long &o_current_fs_idx)

bool	goPos (const double &x, const double &y, const double &th)

bool	goStop ()

bool	goVelocity (const double &vx, const double &vy, const double &vth)

virtual RTC::ReturnCode_t	onActivated (RTC::UniqueId ec_id)

virtual RTC::ReturnCode_t	onDeactivated (RTC::UniqueId ec_id)

virtual RTC::ReturnCode_t	onExecute (RTC::UniqueId ec_id)

virtual RTC::ReturnCode_t	onFinalize ()

virtual RTC::ReturnCode_t	onInitialize ()

bool	releaseEmergencyStop ()

bool	setAutoBalancerParam (const OpenHRP::AutoBalancerService::AutoBalancerParam &i_param)

bool	setFootSteps (const OpenHRP::AutoBalancerService::FootstepSequence &fs, CORBA::Long overwrite_fs_idx)

bool	setFootSteps (const OpenHRP::AutoBalancerService::FootstepsSequence &fss, CORBA::Long overwrite_fs_idx)

bool	setFootStepsWithParam (const OpenHRP::AutoBalancerService::FootstepSequence &fs, const OpenHRP::AutoBalancerService::StepParamSequence &sps, CORBA::Long overwrite_fs_idx)

bool	setFootStepsWithParam (const OpenHRP::AutoBalancerService::FootstepsSequence &fss, const OpenHRP::AutoBalancerService::StepParamsSequence &spss, CORBA::Long overwrite_fs_idx)

bool	setGaitGeneratorParam (const OpenHRP::AutoBalancerService::GaitGeneratorParam &i_param)

bool	startAutoBalancer (const ::OpenHRP::AutoBalancerService::StrSequence &limbs)

bool	stopAutoBalancer ()

void	waitFootSteps ()

void	waitFootStepsEarly (const double tm)

virtual	~AutoBalancer ()

Public Member Functions inherited from RTC::DataFlowComponentBase
	DataFlowComponentBase (Manager *manager)

void	init ()

virtual	~DataFlowComponentBase (void)

Public Member Functions inherited from RTC::RTObject_impl
ReturnCode_t	activate (RTC::UniqueId ec_id)

void	addConfigurationParamListener (ConfigurationParamListenerType type, ConfigurationParamListener *listener, bool autoclean=true)

ConfigurationParamListener *	addConfigurationParamListener (ConfigurationParamListenerType listener_type, Listener &obj, void(Listener::memfunc)(const char , const char *))

void	addConfigurationSetListener (ConfigurationSetListenerType type, ConfigurationSetListener *listener, bool autoclean=true)

ConfigurationSetListener *	addConfigurationSetListener (ConfigurationSetListenerType listener_type, Listener &obj, void(Listener::*memfunc)(const coil::Properties &config_set))

void	addConfigurationSetNameListener (ConfigurationSetNameListenerType type, ConfigurationSetNameListener *listener, bool autoclean=true)

ConfigurationSetNameListener *	addConfigurationSetNameListener (ConfigurationSetNameListenerType type, Listener &obj, void(Listener::memfunc)(const char ))

void	addExecutionContextActionListener (ECActionListenerType listener_type, ECActionListener *listener, bool autoclean=true)

ECActionListener *	addExecutionContextActionListener (ECActionListenerType listener_type, Listener &obj, void(Listener::*memfunc)(UniqueId))

bool	addInPort (const char *name, InPortBase &inport)

bool	addOutPort (const char *name, OutPortBase &outport)

bool	addPort (PortBase &port)

bool	addPort (PortService_ptr port)

bool	addPort (CorbaPort &port)

PortActionListener *	addPortActionListener (PortActionListenerType listener_type, Listener &obj, void(Listener::*memfunc)(const RTC::PortProfile &))

void	addPortActionListener (PortActionListenerType listener_type, PortActionListener *listener, bool autoclean=true)

void	addPortConnectListener (PortConnectListenerType listener_type, PortConnectListener *listener, bool autoclean=true)

PortConnectListener *	addPortConnectListener (PortConnectListenerType listener_type, Listener &obj, void(Listener::memfunc)(const char , ConnectorProfile &))

void	addPortConnectRetListener (PortConnectRetListenerType listener_type, PortConnectRetListener *listener, bool autoclean=true)

PortConnectRetListener *	addPortConnectRetListener (PortConnectRetListenerType listener_type, Listener &obj, void(Listener::memfunc)(const char , ConnectorProfile &, ReturnCode_t))

void	addPostComponentActionListener (PostComponentActionListenerType listener_type, PostComponentActionListener *listener, bool autoclean=true)

PostComponentActionListener *	addPostComponentActionListener (PostCompActionListenerType listener_type, Listener &obj, void(Listener::*memfunc)(UniqueId ec_id, ReturnCode_t ret))

void	addPreComponentActionListener (PreComponentActionListenerType listener_type, PreComponentActionListener *listener, bool autoclean=true)

PreComponentActionListener *	addPreComponentActionListener (PreCompActionListenerType listener_type, Listener &obj, void(Listener::*memfunc)(UniqueId ec_id))

bool	addSdoServiceConsumer (const SDOPackage::ServiceProfile &prof)

bool	addSdoServiceProvider (const SDOPackage::ServiceProfile &prof, SdoServiceProviderBase *provider)

UniqueId	attach_context (ExecutionContext_ptr exec_context)

UniqueId	bindContext (ExecutionContext_ptr exec_context)

bool	bindParameter (const char param_name, VarType &var, const char def_val, bool(trans)(VarType &, const char )=coil::stringTo)

ReturnCode_t	deactivate (RTC::UniqueId ec_id)

void	deletePort (PortService_ptr port)

void	deletePort (CorbaPort &port)

void	deletePort (PortBase &port)

void	deletePortByName (const char *port_name)

ReturnCode_t	detach_context (UniqueId exec_handle)

virtual ReturnCode_t	exit ()

virtual ReturnCode_t	finalize ()

void	finalizeContexts ()

void	finalizePorts ()

virtual ComponentProfile *	get_component_profile ()

virtual SDOPackage::Configuration_ptr	get_configuration ()

virtual ExecutionContext_ptr	get_context (UniqueId exec_handle)

virtual ExecutionContextHandle_t	get_context_handle (ExecutionContext_ptr cxt)

virtual SDOPackage::DeviceProfile *	get_device_profile ()

virtual SDOPackage::Monitoring_ptr	get_monitoring ()

virtual SDOPackage::OrganizationList *	get_organizations ()

virtual ExecutionContextList *	get_owned_contexts ()

virtual SDOPackage::OrganizationList *	get_owned_organizations ()

virtual ExecutionContextList *	get_participating_contexts ()

virtual PortServiceList *	get_ports ()

virtual char *	get_sdo_id ()

virtual SDOPackage::SDOService_ptr	get_sdo_service (const char *id)

virtual char *	get_sdo_type ()

virtual SDOPackage::ServiceProfile *	get_service_profile (const char *id)

virtual SDOPackage::ServiceProfileList *	get_service_profiles ()

virtual CORBA::Any *	get_status (const char *name)

virtual SDOPackage::NVList *	get_status_list ()

const char *	getCategory ()

const char *	getDescription ()

ExecutionContext_ptr	getExecutionContext (RTC::UniqueId ec_id)

double	getExecutionRate (RTC::UniqueId ec_id)

const char *	getInstanceName ()

std::vector< std::string >	getNamingNames ()

RTObject_ptr	getObjRef () const

coil::Properties &	getProperties ()

const char *	getTypeName ()

const char *	getVendor ()

const char *	getVersion ()

virtual ReturnCode_t	initialize ()

virtual CORBA::Boolean	is_alive (ExecutionContext_ptr exec_context)

bool	isOwnExecutionContext (RTC::UniqueId ec_id)

virtual ReturnCode_t	on_aborting (UniqueId exec_handle)

virtual ReturnCode_t	on_activated (UniqueId exec_handle)

virtual ReturnCode_t	on_deactivated (UniqueId exec_handle)

virtual ReturnCode_t	on_error (UniqueId exec_handle)

virtual ReturnCode_t	on_execute (UniqueId exec_handle)

virtual ReturnCode_t	on_finalize ()

virtual ReturnCode_t	on_initialize ()

virtual ReturnCode_t	on_rate_changed (UniqueId exec_handle)

virtual ReturnCode_t	on_reset (UniqueId exec_handle)

virtual ReturnCode_t	on_shutdown (UniqueId exec_handle)

virtual ReturnCode_t	on_startup (UniqueId exec_handle)

virtual ReturnCode_t	on_state_update (UniqueId exec_handle)

bool	readAll ()

void	registerInPort (const char *name, InPortBase &inport)

void	registerOutPort (const char *name, OutPortBase &outport)

void	registerPort (PortBase &port)

void	registerPort (PortService_ptr port)

void	registerPort (CorbaPort &port)

void	removeConfigurationParamListener (ConfigurationParamListenerType type, ConfigurationParamListener *listener)

void	removeConfigurationSetListener (ConfigurationSetListenerType type, ConfigurationSetListener *listener)

void	removeConfigurationSetNameListener (ConfigurationSetNameListenerType type, ConfigurationSetNameListener *listener)

void	removeExecutionContextActionListener (ECActionListenerType listener_type, ECActionListener *listener)

bool	removeInPort (InPortBase &port)

bool	removeOutPort (OutPortBase &port)

bool	removePort (PortService_ptr port)

bool	removePort (CorbaPort &port)

bool	removePort (PortBase &port)

void	removePortActionListener (PortActionListenerType listener_type, PortActionListener *listener)

void	removePortConnectListener (PortConnectListenerType listener_type, PortConnectListener *listener)

void	removePortConnectRetListener (PortConnectRetListenerType listener_type, PortConnectRetListener *listener)

void	removePostComponentActionListener (PostComponentActionListenerType listener_type, PostComponentActionListener *listener)

void	removePreComponentActionListener (PreComponentActionListenerType listener_type, PreComponentActionListener *listener)

bool	removeSdoServiceConsumer (const char *id)

bool	removeSdoServiceProvider (const char *id)

ReturnCode_t	reset (RTC::UniqueId ec_id)

	RTObject_impl (CORBA::ORB_ptr orb, PortableServer::POA_ptr poa)

	RTObject_impl (Manager *manager)

ReturnCode_t	setExecutionRate (RTC::UniqueId ec_id, double rate)

void	setInstanceName (const char *instance_name)

void	setObjRef (const RTObject_ptr rtobj)

void	setProperties (const coil::Properties &prop)

void	setReadAll (bool read=true, bool completion=false)

void	setWriteAll (bool write=true, bool completion=false)

void	updateParameters (const char *config_set)

bool	writeAll ()

virtual	~RTObject_impl (void)

Protected Attributes
TimedAcceleration3D	m_accRef

OutPort< TimedAcceleration3D >	m_accRefOut

TimedBooleanSeq	m_actContactStates

InPort< TimedBooleanSeq >	m_actContactStatesIn

RTC::CorbaPort	m_AutoBalancerServicePort

TimedPoint3D	m_basePos

TimedPose3D	m_basePose

OutPort< TimedPose3D >	m_basePoseOut

InPort< TimedPoint3D >	m_basePosIn

OutPort< TimedPoint3D >	m_basePosOut

TimedOrientation3D	m_baseRpy

InPort< TimedOrientation3D >	m_baseRpyIn

OutPort< TimedOrientation3D >	m_baseRpyOut

TimedDoubleSeq	m_baseTform

OutPort< TimedDoubleSeq >	m_baseTformOut

TimedPoint3D	m_cog

OutPort< TimedPoint3D >	m_cogOut

TimedBooleanSeq	m_contactStates

OutPort< TimedBooleanSeq >	m_contactStatesOut

TimedDoubleSeq	m_controlSwingSupportTime

OutPort< TimedDoubleSeq >	m_controlSwingSupportTimeOut

TimedPoint3D	m_diffCP

InPort< TimedPoint3D >	m_diffCPIn

TimedLong	m_emergencySignal

InPort< TimedLong >	m_emergencySignalIn

std::vector< TimedDoubleSeq >	m_force

std::vector< TimedPoint3D >	m_limbCOPOffset

std::vector< OutPort< TimedPoint3D > * >	m_limbCOPOffsetOut

TimedDoubleSeq	m_optionalData

InPort< TimedDoubleSeq >	m_optionalDataIn

OutPort< TimedDoubleSeq >	m_qOut

TimedDoubleSeq	m_qRef

InPort< TimedDoubleSeq >	m_qRefIn

std::vector< TimedDoubleSeq >	m_ref_force

std::vector< InPort< TimedDoubleSeq > * >	m_ref_forceIn

std::vector< OutPort< TimedDoubleSeq > * >	m_ref_forceOut

TimedPoint3D	m_refFootOriginExtMoment

InPort< TimedPoint3D >	m_refFootOriginExtMomentIn

TimedBoolean	m_refFootOriginExtMomentIsHoldValue

InPort< TimedBoolean >	m_refFootOriginExtMomentIsHoldValueIn

TimedPoint3D	m_sbpCogOffset

OutPort< TimedPoint3D >	m_sbpCogOffsetOut

AutoBalancerService_impl	m_service0

TimedDoubleSeq	m_toeheelRatio

OutPort< TimedDoubleSeq >	m_toeheelRatioOut

TimedBoolean	m_walkingStates

OutPort< TimedBoolean >	m_walkingStatesOut

TimedPoint3D	m_zmp

InPort< TimedPoint3D >	m_zmpIn

RTC::OutPort< RTC::TimedPoint3D >	m_zmpOut

Protected Attributes inherited from RTC::RTObject_impl
ComponentActionListeners	m_actionListeners

ConfigAdmin	m_configsets

bool	m_created

std::vector< ExecutionContextBase * >	m_eclist

ExecutionContextServiceList	m_ecMine

ExecutionContextServiceList	m_ecOther

bool	m_exiting

std::vector< InPortBase * >	m_inports

RTObject_var	m_objref

std::vector< OutPortBase * >	m_outports

Manager *	m_pManager

CORBA::ORB_var	m_pORB

PortAdmin	m_portAdmin

PortConnectListeners	m_portconnListeners

PortableServer::POA_var	m_pPOA

ComponentProfile	m_profile

coil::Properties	m_properties

SDOPackage::Configuration_var	m_pSdoConfig

SDOPackage::Configuration_impl *	m_pSdoConfigImpl

bool	m_readAll

bool	m_readAllCompletion

SDOPackage::OrganizationList	m_sdoOrganizations

SDOPackage::OrganizationList	m_sdoOwnedOrganizations

SdoServiceAdmin	m_sdoservice

SDOPackage::NVList	m_sdoStatus

bool	m_writeAll

bool	m_writeAllCompletion

Logger	rtclog

Private Types
enum	{ BIPED, TROT, PACE, CRAWL, GALLOP }

enum	{ MODE_IDLE, MODE_ABC, MODE_SYNC_TO_IDLE, MODE_SYNC_TO_ABC }

enum	{ MODE_NO_FORCE, MODE_REF_FORCE, MODE_REF_FORCE_WITH_FOOT, MODE_REF_FORCE_RFU_EXT_MOMENT }

typedef boost::shared_ptr< SimpleFullbodyInverseKinematicsSolver >	fikPtr

typedef boost::shared_ptr< rats::gait_generator >	ggPtr

Private Member Functions
bool	calc_inital_support_legs (const double &y, std::vector< rats::coordinates > &initial_support_legs_coords, std::vector< rats::leg_type > &initial_support_legs, rats::coordinates &start_ref_coords)

void	calc_static_balance_point_from_forces (hrp::Vector3 &sb_point, const hrp::Vector3 &tmpcog, const double ref_com_height)

hrp::Vector3	calc_vel_from_hand_error (const rats::coordinates &tmp_fix_coords)

void	calcFixCoordsForAdjustFootstep (rats::coordinates &tmp_fix_coords)

hrp::Vector3	calcFootMidPosUsingZMPWeightMap ()

void	calcReferenceJointAnglesForIK ()

void	calculateOutputRefForces ()

void	copyRatscoords2Footstep (OpenHRP::AutoBalancerService::Footstep &out_fs, const rats::coordinates &in_fs)

void	fixLegToCoords (const hrp::Vector3 &fix_pos, const hrp::Matrix33 &fix_rot)

void	fixLegToCoords2 (rats::coordinates &tmp_fix_coords)

void	getOutputParametersForABC ()

void	getOutputParametersForIDLE ()

void	getOutputParametersForWalking ()

void	getTargetParameters ()

std::string	getUseForceModeString ()

void	interpolateLegNamesAndZMPOffsets ()

bool	isOptionalDataContact (const std::string &ee_name)

hrp::Matrix33	OrientRotationMatrix (const hrp::Matrix33 &rot, const hrp::Vector3 &axis1, const hrp::Vector3 &axis2)

void	rotateRefForcesForFixCoords (rats::coordinates &tmp_fix_coords)

void	solveFullbodyIK ()

void	startABCparam (const ::OpenHRP::AutoBalancerService::StrSequence &limbs)

bool	startWalking ()

void	static_balance_point_proc_one (hrp::Vector3 &tmp_input_sbp, const double ref_com_height)

void	stopABCparam ()

void	stopWalking ()

void	updateTargetCoordsForHandFixMode (rats::coordinates &tmp_fix_coords)

void	updateWalkingVelocityFromHandError (rats::coordinates &tmp_fix_coords)

void	waitABCTransition ()

Private Attributes
hrp::Link *	additional_force_applied_link

hrp::Vector3	additional_force_applied_point_offset

interpolator *	adjust_footstep_interpolator

double	adjust_footstep_transition_time

std::map< std::string, size_t >	contact_states_index_map

enum AutoBalancer:: { ... }	control_mode

double	d_pos_z_root

std::vector< hrp::Vector3 >	default_zmp_offsets

std::vector< std::string >	ee_vec

fikPtr	fik

rats::coordinates	fix_leg_coords

rats::coordinates	fix_leg_coords2

enum AutoBalancer:: { ... }	gait_type

ggPtr	gg

bool	gg_is_walking

bool	gg_solved

std::string	graspless_manip_arm

bool	graspless_manip_mode

hrp::Vector3	graspless_manip_p_gain

rats::coordinates	graspless_manip_reference_trans_coords

hrp::Vector3	hand_fix_initial_offset

hrp::InvDynStateBuffer	idsb

std::map< std::string, ABCIKparam >	ikp

hrp::Vector3	input_basePos

hrp::Matrix33	input_baseRot

hrp::Vector3	input_zmp

std::vector< IIRFilter >	invdyn_zmp_filters

bool	is_hand_fix_initial

bool	is_hand_fix_mode

bool	is_legged_robot

bool	is_stop_mode

std::vector< std::string >	leg_names

interpolator *	leg_names_interpolator

double	leg_names_interpolator_ratio

double	limb_stretch_avoidance_time_const

double	limb_stretch_avoidance_vlimit [2]

int	loop

unsigned int	m_debugLevel

double	m_dt

coil::Mutex	m_mutex

hrp::BodyPtr	m_robot

std::map< std::string, hrp::VirtualForceSensorParam >	m_vfs

hrp::Vector3	prev_imu_sensor_pos

hrp::Vector3	prev_imu_sensor_vel

hrp::Vector3	prev_ref_zmp

hrp::Vector3	ref_cog

std::vector< hrp::Vector3 >	ref_forces

std::vector< hrp::Vector3 >	ref_moments

hrp::Vector3	ref_zmp

hrp::Vector3	sbp_cog_offset

hrp::Vector3	sbp_offset

std::vector< std::string >	sensor_names

hrp::Vector3	target_root_p

hrp::Matrix33	target_root_R

interpolator *	transition_interpolator

double	transition_interpolator_ratio

double	transition_time

enum AutoBalancer:: { ... }	use_force

bool	use_limb_stretch_avoidance

interpolator *	zmp_offset_interpolator

double	zmp_transition_time

Additional Inherited Members
Public Types inherited from RTC::RTObject_impl
typedef ExecutionContextActionListener	ECActionListener

typedef ExecutionContextActionListenerType	ECActionListenerType

typedef PostComponentActionListener	PostCompActionListener

typedef PostComponentActionListenerType	PostCompActionListenerType

typedef PreComponentActionListener	PreCompActionListener

typedef PreComponentActionListenerType	PreCompActionListenerType

Protected Member Functions inherited from RTC::RTObject_impl
virtual ReturnCode_t	onAborting (RTC::UniqueId exec_handle)

void	onAddPort (const PortProfile &pprof)

void	onAttachExecutionContext (UniqueId ec_id)

void	onDetachExecutionContext (UniqueId ec_id)

virtual ReturnCode_t	onError (RTC::UniqueId exec_handle)

virtual ReturnCode_t	onRateChanged (RTC::UniqueId exec_handle)

void	onRemovePort (const PortProfile &pprof)

virtual ReturnCode_t	onReset (RTC::UniqueId exec_handle)

virtual ReturnCode_t	onShutdown (RTC::UniqueId exec_handle)

virtual ReturnCode_t	onStartup (RTC::UniqueId exec_handle)

virtual ReturnCode_t	onStateUpdate (RTC::UniqueId exec_handle)

void	postOnAborting (UniqueId ec_id, ReturnCode_t ret)

void	postOnActivated (UniqueId ec_id, ReturnCode_t ret)

void	postOnDeactivated (UniqueId ec_id, ReturnCode_t ret)

void	postOnError (UniqueId ec_id, ReturnCode_t ret)

void	postOnExecute (UniqueId ec_id, ReturnCode_t ret)

void	postOnFinalize (UniqueId ec_id, ReturnCode_t ret)

void	postOnInitialize (UniqueId ec_id, ReturnCode_t ret)

void	postOnRateChanged (UniqueId ec_id, ReturnCode_t ret)

void	postOnReset (UniqueId ec_id, ReturnCode_t ret)

void	postOnShutdown (UniqueId ec_id, ReturnCode_t ret)

void	postOnStartup (UniqueId ec_id, ReturnCode_t ret)

void	postOnStateUpdate (UniqueId ec_id, ReturnCode_t ret)

void	preOnAborting (UniqueId ec_id)

void	preOnActivated (UniqueId ec_id)

void	preOnDeactivated (UniqueId ec_id)

void	preOnError (UniqueId ec_id)

void	preOnExecute (UniqueId ec_id)

void	preOnFinalize (UniqueId ec_id)

void	preOnInitialize (UniqueId ec_id)

void	preOnRateChanged (UniqueId ec_id)

void	preOnReset (UniqueId ec_id)

void	preOnShutdown (UniqueId ec_id)

void	preOnStartup (UniqueId ec_id)

void	preOnStateUpdate (UniqueId ec_id)

void	shutdown ()

Detailed Description

Definition at line 42 of file AutoBalancer.h.

Member Typedef Documentation

typedef boost::shared_ptr<SimpleFullbodyInverseKinematicsSolver> AutoBalancer::fikPtr

private

Definition at line 249 of file AutoBalancer.h.

typedef boost::shared_ptr<rats::gait_generator> AutoBalancer::ggPtr

private

Definition at line 245 of file AutoBalancer.h.

Member Enumeration Documentation

anonymous enum

private

Enumerator
BIPED
TROT
PACE
CRAWL
GALLOP

Definition at line 252 of file AutoBalancer.h.

anonymous enum

private

Enumerator
MODE_IDLE
MODE_ABC
MODE_SYNC_TO_IDLE
MODE_SYNC_TO_ABC

Definition at line 253 of file AutoBalancer.h.

anonymous enum

private

Enumerator
MODE_NO_FORCE
MODE_REF_FORCE
MODE_REF_FORCE_WITH_FOOT
MODE_REF_FORCE_RFU_EXT_MOMENT

Definition at line 278 of file AutoBalancer.h.

Constructor & Destructor Documentation

AutoBalancer::AutoBalancer ( RTC::Manager * manager )

Definition at line 54 of file AutoBalancer.cpp.

AutoBalancer::~AutoBalancer ( )

virtual

Definition at line 89 of file AutoBalancer.cpp.

Member Function Documentation

bool AutoBalancer::adjustFootSteps	(	const OpenHRP::AutoBalancerService::Footstep &	rfootstep,
		const OpenHRP::AutoBalancerService::Footstep &	lfootstep
	)

Definition at line 1908 of file AutoBalancer.cpp.

bool AutoBalancer::calc_inital_support_legs	(	const double &	y,
		std::vector< rats::coordinates > &	initial_support_legs_coords,
		std::vector< rats::leg_type > &	initial_support_legs,
		rats::coordinates &	start_ref_coords
	)

private

Definition at line 2134 of file AutoBalancer.cpp.

void AutoBalancer::calc_static_balance_point_from_forces	(	hrp::Vector3 &	sb_point,
		const hrp::Vector3 &	tmpcog,
		const double	ref_com_height
	)

private

Definition at line 2036 of file AutoBalancer.cpp.

hrp::Vector3 AutoBalancer::calc_vel_from_hand_error ( const rats::coordinates & tmp_fix_coords )

private

Definition at line 2094 of file AutoBalancer.cpp.

void AutoBalancer::calcFixCoordsForAdjustFootstep ( rats::coordinates & tmp_fix_coords )

private

Definition at line 883 of file AutoBalancer.cpp.

hrp::Vector3 AutoBalancer::calcFootMidPosUsingZMPWeightMap ( )

private

Definition at line 983 of file AutoBalancer.cpp.

void AutoBalancer::calcReferenceJointAnglesForIK ( )

private

Definition at line 1013 of file AutoBalancer.cpp.

void AutoBalancer::calculateOutputRefForces ( )

private

Definition at line 958 of file AutoBalancer.cpp.

void AutoBalancer::copyRatscoords2Footstep	(	OpenHRP::AutoBalancerService::Footstep &	out_fs,
		const rats::coordinates &	in_fs
	)

private

Definition at line 1877 of file AutoBalancer.cpp.

void AutoBalancer::distributeReferenceZMPToWrenches ( const hrp::Vector3 & _ref_zmp )

Definition at line 2184 of file AutoBalancer.cpp.

bool AutoBalancer::emergencyStop ( )

Definition at line 1318 of file AutoBalancer.cpp.

void AutoBalancer::fixLegToCoords	(	const hrp::Vector3 &	fix_pos,
		const hrp::Matrix33 &	fix_rot
	)

private

Definition at line 1039 of file AutoBalancer.cpp.

void AutoBalancer::fixLegToCoords2 ( rats::coordinates & tmp_fix_coords )

private

Definition at line 1059 of file AutoBalancer.cpp.

bool AutoBalancer::getAutoBalancerParam ( OpenHRP::AutoBalancerService::AutoBalancerParam & i_param )

Definition at line 1796 of file AutoBalancer.cpp.

bool AutoBalancer::getFootstepParam ( OpenHRP::AutoBalancerService::FootstepParam & i_param )

Definition at line 1887 of file AutoBalancer.cpp.

bool AutoBalancer::getGaitGeneratorParam ( OpenHRP::AutoBalancerService::GaitGeneratorParam & i_param )

Definition at line 1556 of file AutoBalancer.cpp.

bool AutoBalancer::getGoPosFootstepsSequence	(	const double &	x,
		const double &	y,
		const double &	th,
		OpenHRP::AutoBalancerService::FootstepsSequence_out	o_footstep
	)

Definition at line 1988 of file AutoBalancer.cpp.

void AutoBalancer::getOutputParametersForABC ( )

private

Definition at line 803 of file AutoBalancer.cpp.

void AutoBalancer::getOutputParametersForIDLE ( )

private

Definition at line 833 of file AutoBalancer.cpp.

void AutoBalancer::getOutputParametersForWalking ( )

private

Definition at line 763 of file AutoBalancer.cpp.

bool AutoBalancer::getRemainingFootstepSequence	(	OpenHRP::AutoBalancerService::FootstepSequence_out	o_footstep,
		CORBA::Long &	o_current_fs_idx
	)

Definition at line 1972 of file AutoBalancer.cpp.

void AutoBalancer::getTargetParameters ( )

private

Definition at line 666 of file AutoBalancer.cpp.

std::string AutoBalancer::getUseForceModeString ( )

private

Definition at line 2284 of file AutoBalancer.cpp.

bool AutoBalancer::goPos	(	const double &	x,
		const double &	y,
		const double &	th
	)

Definition at line 1247 of file AutoBalancer.cpp.

bool AutoBalancer::goStop ( )

Definition at line 1311 of file AutoBalancer.cpp.

bool AutoBalancer::goVelocity	(	const double &	vx,
		const double &	vy,
		const double &	vth
	)

Definition at line 1275 of file AutoBalancer.cpp.

void AutoBalancer::interpolateLegNamesAndZMPOffsets ( )

private

Definition at line 860 of file AutoBalancer.cpp.

bool AutoBalancer::isOptionalDataContact ( const std::string & ee_name )

inlineprivate

Definition at line 237 of file AutoBalancer.h.

RTC::ReturnCode_t AutoBalancer::onActivated ( RTC::UniqueId ec_id )

virtual

Reimplemented from RTC::RTObject_impl.

Definition at line 412 of file AutoBalancer.cpp.

RTC::ReturnCode_t AutoBalancer::onDeactivated ( RTC::UniqueId ec_id )

virtual

Reimplemented from RTC::RTObject_impl.

Definition at line 419 of file AutoBalancer.cpp.

RTC::ReturnCode_t AutoBalancer::onExecute ( RTC::UniqueId ec_id )

virtual

Reimplemented from RTC::RTObject_impl.

Definition at line 434 of file AutoBalancer.cpp.

RTC::ReturnCode_t AutoBalancer::onFinalize ( void )

virtual

Reimplemented from RTC::RTObject_impl.

Definition at line 389 of file AutoBalancer.cpp.

RTC::ReturnCode_t AutoBalancer::onInitialize ( void )

virtual

Reimplemented from RTC::RTObject_impl.

Definition at line 94 of file AutoBalancer.cpp.

hrp::Matrix33 AutoBalancer::OrientRotationMatrix	(	const hrp::Matrix33 &	rot,
		const hrp::Vector3 &	axis1,
		const hrp::Vector3 &	axis2
	)

private

Definition at line 1028 of file AutoBalancer.cpp.

bool AutoBalancer::releaseEmergencyStop ( )

Definition at line 1327 of file AutoBalancer.cpp.

void AutoBalancer::rotateRefForcesForFixCoords ( rats::coordinates & tmp_fix_coords )

private

Definition at line 901 of file AutoBalancer.cpp.

bool AutoBalancer::setAutoBalancerParam ( const OpenHRP::AutoBalancerService::AutoBalancerParam & i_param )

Definition at line 1647 of file AutoBalancer.cpp.

bool AutoBalancer::setFootSteps	(	const OpenHRP::AutoBalancerService::FootstepSequence &	fs,
		CORBA::Long	overwrite_fs_idx
	)

bool AutoBalancer::setFootSteps	(	const OpenHRP::AutoBalancerService::FootstepsSequence &	fss,
		CORBA::Long	overwrite_fs_idx
	)

Definition at line 1336 of file AutoBalancer.cpp.

bool AutoBalancer::setFootStepsWithParam	(	const OpenHRP::AutoBalancerService::FootstepSequence &	fs,
		const OpenHRP::AutoBalancerService::StepParamSequence &	sps,
		CORBA::Long	overwrite_fs_idx
	)

bool AutoBalancer::setFootStepsWithParam	(	const OpenHRP::AutoBalancerService::FootstepsSequence &	fss,
		const OpenHRP::AutoBalancerService::StepParamsSequence &	spss,
		CORBA::Long	overwrite_fs_idx
	)

Definition at line 1354 of file AutoBalancer.cpp.

bool AutoBalancer::setGaitGeneratorParam ( const OpenHRP::AutoBalancerService::GaitGeneratorParam & i_param )

Definition at line 1468 of file AutoBalancer.cpp.

void AutoBalancer::solveFullbodyIK ( )

private

Definition at line 1078 of file AutoBalancer.cpp.

void AutoBalancer::startABCparam ( const ::OpenHRP::AutoBalancerService::StrSequence & limbs )

private

Definition at line 1138 of file AutoBalancer.cpp.

bool AutoBalancer::startAutoBalancer ( const ::OpenHRP::AutoBalancerService::StrSequence & limbs )

Definition at line 1219 of file AutoBalancer.cpp.

bool AutoBalancer::startWalking ( )

private

Definition at line 1173 of file AutoBalancer.cpp.

void AutoBalancer::static_balance_point_proc_one	(	hrp::Vector3 &	tmp_input_sbp,
		const double	ref_com_height
	)

private

Definition at line 2023 of file AutoBalancer.cpp.

void AutoBalancer::stopABCparam ( )

private

Definition at line 1161 of file AutoBalancer.cpp.

bool AutoBalancer::stopAutoBalancer ( )

Definition at line 1231 of file AutoBalancer.cpp.

void AutoBalancer::stopWalking ( )

private

Definition at line 1207 of file AutoBalancer.cpp.

void AutoBalancer::updateTargetCoordsForHandFixMode ( rats::coordinates & tmp_fix_coords )

private

Definition at line 922 of file AutoBalancer.cpp.

void AutoBalancer::updateWalkingVelocityFromHandError ( rats::coordinates & tmp_fix_coords )

private

Definition at line 1000 of file AutoBalancer.cpp.

void AutoBalancer::waitABCTransition ( )

private

Definition at line 1242 of file AutoBalancer.cpp.

void AutoBalancer::waitFootSteps ( )

Definition at line 1450 of file AutoBalancer.cpp.

void AutoBalancer::waitFootStepsEarly ( const double tm )

Definition at line 1459 of file AutoBalancer.cpp.

Member Data Documentation

hrp::Link* AutoBalancer::additional_force_applied_link

private

Definition at line 293 of file AutoBalancer.h.

hrp::Vector3 AutoBalancer::additional_force_applied_point_offset

private

Definition at line 294 of file AutoBalancer.h.

interpolator* AutoBalancer::adjust_footstep_interpolator

private

Definition at line 271 of file AutoBalancer.h.

double AutoBalancer::adjust_footstep_transition_time

private

Definition at line 268 of file AutoBalancer.h.

std::map<std::string, size_t> AutoBalancer::contact_states_index_map

private

Definition at line 255 of file AutoBalancer.h.

enum { ... } AutoBalancer::control_mode

double AutoBalancer::d_pos_z_root

private

Definition at line 265 of file AutoBalancer.h.

std::vector<hrp::Vector3> AutoBalancer::default_zmp_offsets

private

Definition at line 261 of file AutoBalancer.h.

std::vector<std::string> AutoBalancer::ee_vec

private

Definition at line 257 of file AutoBalancer.h.

fikPtr AutoBalancer::fik

private

Definition at line 250 of file AutoBalancer.h.

rats::coordinates AutoBalancer::fix_leg_coords

private

Definition at line 260 of file AutoBalancer.h.

rats::coordinates AutoBalancer::fix_leg_coords2

private

Definition at line 260 of file AutoBalancer.h.

enum { ... } AutoBalancer::gait_type

ggPtr AutoBalancer::gg

private

Definition at line 246 of file AutoBalancer.h.

bool AutoBalancer::gg_is_walking

private

Definition at line 247 of file AutoBalancer.h.

bool AutoBalancer::gg_solved

private

Definition at line 247 of file AutoBalancer.h.

std::string AutoBalancer::graspless_manip_arm

private

Definition at line 285 of file AutoBalancer.h.

bool AutoBalancer::graspless_manip_mode

private

Definition at line 284 of file AutoBalancer.h.

hrp::Vector3 AutoBalancer::graspless_manip_p_gain

private

Definition at line 286 of file AutoBalancer.h.

rats::coordinates AutoBalancer::graspless_manip_reference_trans_coords

private

Definition at line 287 of file AutoBalancer.h.

hrp::Vector3 AutoBalancer::hand_fix_initial_offset

private

Definition at line 251 of file AutoBalancer.h.

hrp::InvDynStateBuffer AutoBalancer::idsb

private

Definition at line 289 of file AutoBalancer.h.

std::map<std::string, ABCIKparam> AutoBalancer::ikp

private

Definition at line 254 of file AutoBalancer.h.

hrp::Vector3 AutoBalancer::input_basePos

private

Definition at line 273 of file AutoBalancer.h.

hrp::Matrix33 AutoBalancer::input_baseRot

private

Definition at line 274 of file AutoBalancer.h.

hrp::Vector3 AutoBalancer::input_zmp

private

Definition at line 273 of file AutoBalancer.h.

std::vector<IIRFilter> AutoBalancer::invdyn_zmp_filters

private

Definition at line 290 of file AutoBalancer.h.

bool AutoBalancer::is_hand_fix_initial

private

Definition at line 282 of file AutoBalancer.h.

bool AutoBalancer::is_hand_fix_mode

private

Definition at line 282 of file AutoBalancer.h.

bool AutoBalancer::is_legged_robot

private

Definition at line 282 of file AutoBalancer.h.

bool AutoBalancer::is_stop_mode

private

Definition at line 282 of file AutoBalancer.h.

std::vector<std::string> AutoBalancer::leg_names

private

Definition at line 257 of file AutoBalancer.h.

interpolator* AutoBalancer::leg_names_interpolator

private

Definition at line 272 of file AutoBalancer.h.

double AutoBalancer::leg_names_interpolator_ratio

private

Definition at line 268 of file AutoBalancer.h.

double AutoBalancer::limb_stretch_avoidance_time_const

private

Definition at line 265 of file AutoBalancer.h.

double AutoBalancer::limb_stretch_avoidance_vlimit[2]

private

Definition at line 265 of file AutoBalancer.h.

int AutoBalancer::loop

private

Definition at line 283 of file AutoBalancer.h.

TimedAcceleration3D AutoBalancer::m_accRef

protected

Definition at line 164 of file AutoBalancer.h.

OutPort<TimedAcceleration3D> AutoBalancer::m_accRefOut

protected

Definition at line 165 of file AutoBalancer.h.

TimedBooleanSeq AutoBalancer::m_actContactStates

protected

Definition at line 143 of file AutoBalancer.h.

InPort<TimedBooleanSeq> AutoBalancer::m_actContactStatesIn

protected

Definition at line 144 of file AutoBalancer.h.

RTC::CorbaPort AutoBalancer::m_AutoBalancerServicePort

protected

Definition at line 187 of file AutoBalancer.h.

TimedPoint3D AutoBalancer::m_basePos

protected

Definition at line 129 of file AutoBalancer.h.

TimedPose3D AutoBalancer::m_basePose

protected

Definition at line 162 of file AutoBalancer.h.

OutPort<TimedPose3D> AutoBalancer::m_basePoseOut

protected

Definition at line 163 of file AutoBalancer.h.

InPort<TimedPoint3D> AutoBalancer::m_basePosIn

protected

Definition at line 130 of file AutoBalancer.h.

OutPort<TimedPoint3D> AutoBalancer::m_basePosOut

protected

Definition at line 158 of file AutoBalancer.h.

TimedOrientation3D AutoBalancer::m_baseRpy

protected

Definition at line 131 of file AutoBalancer.h.

InPort<TimedOrientation3D> AutoBalancer::m_baseRpyIn

protected

Definition at line 132 of file AutoBalancer.h.

OutPort<TimedOrientation3D> AutoBalancer::m_baseRpyOut

protected

Definition at line 159 of file AutoBalancer.h.

TimedDoubleSeq AutoBalancer::m_baseTform

protected

Definition at line 160 of file AutoBalancer.h.

OutPort<TimedDoubleSeq> AutoBalancer::m_baseTformOut

protected

Definition at line 161 of file AutoBalancer.h.

TimedPoint3D AutoBalancer::m_cog

protected

Definition at line 150 of file AutoBalancer.h.

OutPort<TimedPoint3D> AutoBalancer::m_cogOut

protected

Definition at line 181 of file AutoBalancer.h.

TimedBooleanSeq AutoBalancer::m_contactStates

protected

Definition at line 166 of file AutoBalancer.h.

OutPort<TimedBooleanSeq> AutoBalancer::m_contactStatesOut

protected

Definition at line 167 of file AutoBalancer.h.

TimedDoubleSeq AutoBalancer::m_controlSwingSupportTime

protected

Definition at line 170 of file AutoBalancer.h.

OutPort<TimedDoubleSeq> AutoBalancer::m_controlSwingSupportTimeOut

protected

Definition at line 171 of file AutoBalancer.h.

unsigned int AutoBalancer::m_debugLevel

private

Definition at line 281 of file AutoBalancer.h.

TimedPoint3D AutoBalancer::m_diffCP

protected

Definition at line 141 of file AutoBalancer.h.

InPort<TimedPoint3D> AutoBalancer::m_diffCPIn

protected

Definition at line 142 of file AutoBalancer.h.

double AutoBalancer::m_dt

private

Definition at line 262 of file AutoBalancer.h.

TimedLong AutoBalancer::m_emergencySignal

protected

Definition at line 139 of file AutoBalancer.h.

InPort<TimedLong> AutoBalancer::m_emergencySignalIn

protected

Definition at line 140 of file AutoBalancer.h.

std::vector<TimedDoubleSeq> AutoBalancer::m_force

protected

Definition at line 176 of file AutoBalancer.h.

std::vector<TimedPoint3D> AutoBalancer::m_limbCOPOffset

protected

Definition at line 178 of file AutoBalancer.h.

std::vector<OutPort<TimedPoint3D> *> AutoBalancer::m_limbCOPOffsetOut

protected

Definition at line 179 of file AutoBalancer.h.

coil::Mutex AutoBalancer::m_mutex

private

Definition at line 264 of file AutoBalancer.h.

TimedDoubleSeq AutoBalancer::m_optionalData

protected

Definition at line 135 of file AutoBalancer.h.

InPort<TimedDoubleSeq> AutoBalancer::m_optionalDataIn

protected

Definition at line 136 of file AutoBalancer.h.

OutPort<TimedDoubleSeq> AutoBalancer::m_qOut

protected

Definition at line 156 of file AutoBalancer.h.

TimedDoubleSeq AutoBalancer::m_qRef

protected

Definition at line 127 of file AutoBalancer.h.

InPort<TimedDoubleSeq> AutoBalancer::m_qRefIn

protected

Definition at line 128 of file AutoBalancer.h.

std::vector<TimedDoubleSeq> AutoBalancer::m_ref_force

protected

Definition at line 137 of file AutoBalancer.h.

std::vector<InPort<TimedDoubleSeq> *> AutoBalancer::m_ref_forceIn

protected

Definition at line 138 of file AutoBalancer.h.

std::vector<OutPort<TimedDoubleSeq> *> AutoBalancer::m_ref_forceOut

protected

Definition at line 177 of file AutoBalancer.h.

TimedPoint3D AutoBalancer::m_refFootOriginExtMoment

protected

Definition at line 145 of file AutoBalancer.h.

InPort<TimedPoint3D> AutoBalancer::m_refFootOriginExtMomentIn

protected

Definition at line 146 of file AutoBalancer.h.

TimedBoolean AutoBalancer::m_refFootOriginExtMomentIsHoldValue

protected

Definition at line 147 of file AutoBalancer.h.

InPort<TimedBoolean> AutoBalancer::m_refFootOriginExtMomentIsHoldValueIn

protected

Definition at line 148 of file AutoBalancer.h.

hrp::BodyPtr AutoBalancer::m_robot

private

Definition at line 263 of file AutoBalancer.h.

TimedPoint3D AutoBalancer::m_sbpCogOffset

protected

Definition at line 174 of file AutoBalancer.h.

OutPort<TimedPoint3D> AutoBalancer::m_sbpCogOffsetOut

protected

Definition at line 175 of file AutoBalancer.h.

AutoBalancerService_impl AutoBalancer::m_service0

protected

Definition at line 193 of file AutoBalancer.h.

TimedDoubleSeq AutoBalancer::m_toeheelRatio

protected

Definition at line 168 of file AutoBalancer.h.

OutPort<TimedDoubleSeq> AutoBalancer::m_toeheelRatioOut

protected

Definition at line 169 of file AutoBalancer.h.

std::map<std::string, hrp::VirtualForceSensorParam> AutoBalancer::m_vfs

private

Definition at line 256 of file AutoBalancer.h.

TimedBoolean AutoBalancer::m_walkingStates

protected

Definition at line 172 of file AutoBalancer.h.

OutPort<TimedBoolean> AutoBalancer::m_walkingStatesOut

protected

Definition at line 173 of file AutoBalancer.h.

TimedPoint3D AutoBalancer::m_zmp

protected

Definition at line 133 of file AutoBalancer.h.

InPort<TimedPoint3D> AutoBalancer::m_zmpIn

protected

Definition at line 134 of file AutoBalancer.h.

RTC::OutPort<RTC::TimedPoint3D> AutoBalancer::m_zmpOut

protected

Definition at line 157 of file AutoBalancer.h.

hrp::Vector3 AutoBalancer::prev_imu_sensor_pos

private

Definition at line 251 of file AutoBalancer.h.

hrp::Vector3 AutoBalancer::prev_imu_sensor_vel

private

Definition at line 251 of file AutoBalancer.h.

hrp::Vector3 AutoBalancer::prev_ref_zmp

private

Definition at line 251 of file AutoBalancer.h.

hrp::Vector3 AutoBalancer::ref_cog

private

Definition at line 251 of file AutoBalancer.h.

std::vector<hrp::Vector3> AutoBalancer::ref_forces

private

Definition at line 279 of file AutoBalancer.h.

std::vector<hrp::Vector3> AutoBalancer::ref_moments

private

Definition at line 279 of file AutoBalancer.h.

hrp::Vector3 AutoBalancer::ref_zmp

private

Definition at line 251 of file AutoBalancer.h.

hrp::Vector3 AutoBalancer::sbp_cog_offset

private

Definition at line 277 of file AutoBalancer.h.

hrp::Vector3 AutoBalancer::sbp_offset

private

Definition at line 277 of file AutoBalancer.h.

std::vector<std::string> AutoBalancer::sensor_names

private

Definition at line 257 of file AutoBalancer.h.

hrp::Vector3 AutoBalancer::target_root_p

private

Definition at line 258 of file AutoBalancer.h.

hrp::Matrix33 AutoBalancer::target_root_R

private

Definition at line 259 of file AutoBalancer.h.

interpolator* AutoBalancer::transition_interpolator

private

Definition at line 270 of file AutoBalancer.h.

double AutoBalancer::transition_interpolator_ratio

private

Definition at line 268 of file AutoBalancer.h.

double AutoBalancer::transition_time

private

Definition at line 268 of file AutoBalancer.h.

enum { ... } AutoBalancer::use_force

bool AutoBalancer::use_limb_stretch_avoidance

private

Definition at line 266 of file AutoBalancer.h.

interpolator* AutoBalancer::zmp_offset_interpolator

private

Definition at line 269 of file AutoBalancer.h.

double AutoBalancer::zmp_transition_time

private

Definition at line 268 of file AutoBalancer.h.

The documentation for this class was generated from the following files:

Classes

Public Member Functions

Protected Attributes

Private Types

Private Member Functions

Private Attributes

Additional Inherited Members

Detailed Description

Member Typedef Documentation

Member Enumeration Documentation

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation