fog::ReachPGSBeta Class Reference

A class that implements the controller for reaching the PGS state. It reads feedback from the position of the fingeres and commands the task velocity of the arm. More...

#include <fog/reach_pgs_beta.h>

Inheritance diagram for fog::ReachPGSBeta:

Collaboration diagram for fog::ReachPGSBeta:

Public Member Functions
	ReachPGSBeta (const std::shared_ptr< arl::robot::Robot > &arm, const std::shared_ptr< arl::robot::Robot > &hand, const std::shared_ptr< arl::robot::Sensor > &sensor, const std::shared_ptr< arl::viz::RosStatePublisher > &viz, double initial_velocity, double theta, double phi)
	The default constructor. More...

Protected Member Functions
void	measure ()
void	update ()
void	command ()
void	init ()
bool	stop ()

Private Member Functions
arma::vec	multiplyPoseVector (const arma::mat &pose, const arma::vec &position)
void	projectionToPlane (const arma::vec &V, const arma::vec &Z, arma::vec &next_command)
arma::vec	updateArmWithClik (const arma::vec &linear_vel)

Private Attributes
double	epsilon_
arma::vec	v_G_
arma::vec	v_G_init_
double	initial_velocity_
std::shared_ptr < arl::robot::Robot >	hand_
std::shared_ptr < arl::viz::RosStatePublisher >	viz_
std::shared_ptr < arl::robot::Sensor >	ft_sensor
arma::vec	hand_jnt_torques_
std::vector< arma::vec >	finger_pos_
std::vector< arma::mat >	finger_orientation_
std::vector< arma::vec >	finger_pos_prev_
arma::vec	arm_position_
arma::mat	arm_orientation_
arma::mat	arm_pose_
arma::vec	arm_wrench_
arma::mat	arm_jac_
arma::vec	arm_jnt_vel_
arl::math::Clik	clik_
unsigned int	arm_control_strategy_
arma::vec	arm_wrench_base_
arma::vec	hand_delta_p_
int	hand_delta_p_filter_
double	hand_delta_p_init_norm_
arma::vec	hand_centroid_
arma::vec	hand_centroid_prev_
bool	first_time_
unsigned int	counter_
arma::vec	direction_centroid_
double	phi
double	theta

Detailed Description

A class that implements the controller for reaching the PGS state. It reads feedback from the position of the fingeres and commands the task velocity of the arm.

Constructor & Destructor Documentation

fog::ReachPGSBeta::ReachPGSBeta	(	const std::shared_ptr< arl::robot::Robot > &	arm,
		const std::shared_ptr< arl::robot::Robot > &	hand,
		const std::shared_ptr< arl::robot::Sensor > &	sensor,
		const std::shared_ptr< arl::viz::RosStatePublisher > &	viz,
		double	initial_velocity,
		double	theta,
		double	phi
	)

The default constructor.

Parameters

arm	The interface with the robot arm
hand	The interface with the robot hand
h	The model of the hand as it is considered in this work
initial_velocity	The amplitude of the initial velocity of the arm

Member Function Documentation

void fog::ReachPGSBeta::command ( )

protected

void fog::ReachPGSBeta::init ( )

protected

Initialize CLIK for the arm

Set the first time the prev position

Epsilon

Measure the initial orientation of the arm This orientation it is assumed to be the orientation of the IGS

Set the initial velocity of the arm, which has an initial ampitude and the direction of the z-axis (the axis coming out of the palm).

Set the robot arma to velocity control

Here is the call graph for this function:

void fog::ReachPGSBeta::measure ( )

protected

Here is the call graph for this function:

vec fog::ReachPGSBeta::multiplyPoseVector ( const arma::mat &  pose, const arma::vec &  position  )

private

void fog::ReachPGSBeta::projectionToPlane ( const arma::vec &  V, const arma::vec &  Z, arma::vec &  next_command  )

private

Function that calculates the projection/rotation of the vector next_command to the plain defined by V (vector from wrist to object) and Z (the Z axis of the object frame)

Normalize the projection and scale it with the initial norm of next command. This is equivelant to a rotation

bool fog::ReachPGSBeta::stop ( )

protected

void fog::ReachPGSBeta::update ( )

protected

Here is the call graph for this function:

arma::vec fog::ReachPGSBeta::updateArmWithClik ( const arma::vec &  linear_vel )

private

Member Data Documentation

unsigned int fog::ReachPGSBeta::arm_control_strategy_

private

arma::mat fog::ReachPGSBeta::arm_jac_

private

arma::vec fog::ReachPGSBeta::arm_jnt_vel_

private

arma::mat fog::ReachPGSBeta::arm_orientation_

private

arma::mat fog::ReachPGSBeta::arm_pose_

private

arma::vec fog::ReachPGSBeta::arm_position_

private

arma::vec fog::ReachPGSBeta::arm_wrench_

private

arma::vec fog::ReachPGSBeta::arm_wrench_base_

private

arl::math::Clik fog::ReachPGSBeta::clik_

private

unsigned int fog::ReachPGSBeta::counter_

private

arma::vec fog::ReachPGSBeta::direction_centroid_

private

double fog::ReachPGSBeta::epsilon_

private

std::vector<arma::mat> fog::ReachPGSBeta::finger_orientation_

private

std::vector<arma::vec> fog::ReachPGSBeta::finger_pos_

private

std::vector<arma::vec> fog::ReachPGSBeta::finger_pos_prev_

private

bool fog::ReachPGSBeta::first_time_

private

std::shared_ptr<arl::robot::Sensor> fog::ReachPGSBeta::ft_sensor

private

std::shared_ptr<arl::robot::Robot> fog::ReachPGSBeta::hand_

private

arma::vec fog::ReachPGSBeta::hand_centroid_

private

arma::vec fog::ReachPGSBeta::hand_centroid_prev_

private

arma::vec fog::ReachPGSBeta::hand_delta_p_

private

int fog::ReachPGSBeta::hand_delta_p_filter_

private

double fog::ReachPGSBeta::hand_delta_p_init_norm_

private

arma::vec fog::ReachPGSBeta::hand_jnt_torques_

private

double fog::ReachPGSBeta::initial_velocity_

private

double fog::ReachPGSBeta::phi

private

double fog::ReachPGSBeta::theta

private

arma::vec fog::ReachPGSBeta::v_G_

private

arma::vec fog::ReachPGSBeta::v_G_init_

private

std::shared_ptr<arl::viz::RosStatePublisher> fog::ReachPGSBeta::viz_

private