/home/laradmin/lar/bases/phua_haptic/src/humanoid_control_functions.cpp File Reference

Humanoid control functions implementation. More...

#include <humanoid_control_functions.h>
#include <gtk_aux.h>

Include dependency graph for humanoid_control_functions.cpp:

Go to the source code of this file.

Functions
void	ArmsDifferencialSpeedControl (double *avg_speed, shared_vars_t *RobotVars)
	Main robot arm speed control type function.
void	CalculateDifferencialArmServoSpeed (double *avg_speed, shared_vars_t *RobotVars)
	Function to calculate joint angular speed given 3d space velocity vector.
short unsigned int	ConvertJointAngleByID (int id, double angle_position)
	Function to convert a joint angle for a given humanoid servo ID.
double	ConvertServoValueByID (int id, short unsigned int servo_value)
	Function to convert a servo value for a given humanoid servo ID.
void	DemoUserPath_ReadPoints (shared_vars_t *RobotVars)
	Function to read points from file.
void	DemoUserPath_WritePoints (shared_vars_t *RobotVars)
	Function to write points to file.
void	MoveArmToCartesianPosition (double X, double Y, double Z, short int SIDE, shared_vars_t *RobotVars)
	Function to move arm end point to given coordinates.
void	MoveDetachedLegToCartesianPosition (double X, double Y, double Z, double t4, short int SIDE, shared_vars_t *RobotVars)
	Function to move detached leg end point to given coordinates.
void	PathFollowingExecute (shared_vars_t *RobotVars)
	Function to run points read from file.
void	SetArmAutomaticSpeeds (short int SIDE, shared_vars_t *RobotVars)
	Function set different automatic speeds to arm servomotors.
void	SetArmSpeed (short int SIDE, short int speed, shared_vars_t *RobotVars)
	Function to set speed to arm servos.
void	SetLegSpeed (short int SIDE, short int speed, shared_vars_t *RobotVars)
	Function to set speed to leg servos.
void	SetRobotHomePosition (shared_vars_t *RobotVars)
	Function to send commands to set the robot to its home position.
void	StopRobotMovement (shared_vars_t *RobotVars)
	Function to stop robot at its current joint configuration.
void	UpdateArmsDirKinData (shared_vars_t *RobotVars)
	Function to update the arm end point.
void	UpdateDetachedLegsDirKinData (shared_vars_t *RobotVars)
	Function to update the detached legs end point.
void	UpdateJointDataByID (short int id, double joint_angle, shared_vars_t *RobotVars)
	Function to update joint value in the data struture from supplied ID.
void	UpdateKinematicModelDirKin (shared_vars_t *RobotVars)
	Function to update shared structure with the current direct kinematic of the robot.

---

Detailed Description

Humanoid control functions implementation.

Definition in file humanoid_control_functions.cpp.

---

Function Documentation

void ArmsDifferencialSpeedControl	(	double *	avg_speed,
		shared_vars_t *	RobotVars
	)

Main robot arm speed control type function.

This function is the main function for the arms speed control type.

Parameters:

[in]	avg_speed	average joystick cartesian speed vector.
[in]	RobotVars	structure defined in types.h with the joint values.

Returns:

none.

Definition at line 1005 of file humanoid_control_functions.cpp.

void CalculateDifferencialArmServoSpeed	(	double *	avg_speed,
		shared_vars_t *	RobotVars
	)

Function to calculate joint angular speed given 3d space velocity vector.

This function uses the inverse jacobian to calculate joint angular speed.

Parameters:

[in]	avg_speed	3d speed components.
[in]	RobotVars	structure defined in types.h with the joint values.

Returns:

none.

Definition at line 803 of file humanoid_control_functions.cpp.

short unsigned int ConvertJointAngleByID	(	int	id,
		double	angle_position
	)

Function to convert a joint angle for a given humanoid servo ID.

This function converts joint angles to servo values for a given servomotor ID. Depends on external class type "humanoid".

Parameters:

[in]	id	ID of the servo to convert.
[in]	angle_position	angle in degrees of the desired joint angle position.

Returns:

the corresponding servomotor value.

Definition at line 37 of file humanoid_control_functions.cpp.

double ConvertServoValueByID	(	int	id,
		short unsigned int	servo_value
	)

Function to convert a servo value for a given humanoid servo ID.

This function converts servo values to joint values for a given servomotor ID. Depends on external class type "humanoid".

Parameters:

[in]	id	ID of the servo to convert.
[in]	servo_value	servomotor position value.

Returns:

the corresponding joint angle value.

Definition at line 352 of file humanoid_control_functions.cpp.

void DemoUserPath_ReadPoints

(

shared_vars_t *

RobotVars

)

Function to read points from file.

Parameters:

[in]

RobotVars

structure defined in types.h with the joint values.

Returns:

none.

Definition at line 1422 of file humanoid_control_functions.cpp.

void DemoUserPath_WritePoints

(

shared_vars_t *

RobotVars

)

Function to write points to file.

Parameters:

[in]

RobotVars

structure defined in types.h with the joint values.

Returns:

none.

Definition at line 1347 of file humanoid_control_functions.cpp.

void MoveArmToCartesianPosition	(	double	X,
		double	Y,
		double	Z,
		short int	SIDE,
		shared_vars_t *	RobotVars
	)

Function to move arm end point to given coordinates.

This function calculates joint value for given 3D space coordinates for the arms and sends commands to the servos to reach that position.

Parameters:

[in]	X	xx axis desired position.
[in]	Y	yy axis desired position.
[in]	Z	zz axis desired position.
[in]	SIDE	LEFT or RIGHT.
[in]	RobotVars	structure defined in types.h with the joint values.

Returns:

none.

Definition at line 536 of file humanoid_control_functions.cpp.

void MoveDetachedLegToCartesianPosition	(	double	X,
		double	Y,
		double	Z,
		double	t4,
		short int	SIDE,
		shared_vars_t *	RobotVars
	)

Function to move detached leg end point to given coordinates.

This function calculates joint value for given 3D space coordinates for the detached legs and sends commands to the servos to reach that position.

Parameters:

[in]	X	xx axis desired position.
[in]	Y	yy axis desired position.
[in]	Z	zz axis desired position.
[in]	t4	hip abduction desired angle.
[in]	SIDE	LEFT or RIGHT.
[in]	RobotVars	structure defined in types.h with the joint values.

Returns:

none.

Definition at line 574 of file humanoid_control_functions.cpp.

void PathFollowingExecute

(

shared_vars_t *

RobotVars

)

Function to run points read from file.

Parameters:

[in]

RobotVars

structure defined in types.h with the joint values.

Returns:

none.

Definition at line 1572 of file humanoid_control_functions.cpp.

void SetArmAutomaticSpeeds	(	short int	SIDE,
		shared_vars_t *	RobotVars
	)

Function set different automatic speeds to arm servomotors.

This function sends commands to the arms servomotors to update the current speed and updates the information in the data structure.

Parameters:

[in]	SIDE	LEFT or RIGHT.
[in]	RobotVars	structure defined in types.h with the joint values.

Returns:

none.

Definition at line 695 of file humanoid_control_functions.cpp.

void SetArmSpeed	(	short int	SIDE,
		short int	speed,
		shared_vars_t *	RobotVars
	)

Function to set speed to arm servos.

This function sets a new speed to all the arm servos.

Parameters:

[in]	SIDE	LEFT or RIGHT.
[in]	speed	servo digital speed to set.
[in]	RobotVars	structure defined in types.h with the joint values.

Returns:

none.

Definition at line 468 of file humanoid_control_functions.cpp.

void SetLegSpeed	(	short int	SIDE,
		short int	speed,
		shared_vars_t *	RobotVars
	)

Function to set speed to leg servos.

This function sets a new speed to all the leg servos.

Parameters:

[in]	SIDE	LEFT or RIGHT.
[in]	speed	servo digital speed to set.
[in]	RobotVars	structure defined in types.h with the joint values.

Returns:

none.

Definition at line 498 of file humanoid_control_functions.cpp.

void SetRobotHomePosition

(

shared_vars_t *

RobotVars

)

Function to send commands to set the robot to its home position.

This function sends a go to zero command to all joints of the robot. Depends on external class type "hitec5980sg".

Parameters:

[in]

RobotVars

structure defined in types.h with the joint values.

Returns:

none.

Definition at line 239 of file humanoid_control_functions.cpp.

void StopRobotMovement

(

shared_vars_t *

RobotVars

)

Function to stop robot at its current joint configuration.

This function sends commands to read current robot joint coordinates followed by commands for that position.

Parameters:

[in]

RobotVars

structure defined in types.h with the joint values.

Returns:

none.

Definition at line 912 of file humanoid_control_functions.cpp.

void UpdateArmsDirKinData

(

shared_vars_t *

RobotVars

)

Function to update the arm end point.

This function updates the arm end point values with the current arm joint values.

Parameters:

[in]

RobotVars

structure defined in types.h with the joint values.

Returns:

none.

Definition at line 153 of file humanoid_control_functions.cpp.

void UpdateDetachedLegsDirKinData

(

shared_vars_t *

RobotVars

)

Function to update the detached legs end point.

This function updates the detached legs end point values with the current detached legs joint values.

Parameters:

[in]

RobotVars

structure defined in types.h with the joint values.

Returns:

none.

Definition at line 187 of file humanoid_control_functions.cpp.

void UpdateJointDataByID	(	short int	id,
		double	joint_angle,
		shared_vars_t *	RobotVars
	)

Function to update joint value in the data struture from supplied ID.

This function updates the joint values in the data struture from by means of the servo ID. If parameter id is 1000, the function will scan all the robot joints and update the structure.

Parameters:

[in]	id	ID of the joint servo.
[in]	joint_angle	joint value to update.
[in]	RobotVars	structure defined in types.h with the joint values.

Returns:

none.

Definition at line 626 of file humanoid_control_functions.cpp.

void UpdateKinematicModelDirKin

(

shared_vars_t *

RobotVars

)

Function to update shared structure with the current direct kinematic of the robot.

Parameters:

[in]

RobotVars

structure defined in types.h with the joint values.

Returns:

none.

Definition at line 1768 of file humanoid_control_functions.cpp.