Chapter 11. Robot Control

Functions
const arma::vec	mr::ComputeTorque (const arma::vec &thetalist, const arma::vec &dthetalist, const arma::vec &eint, const arma::vec3 &g, const std::vector< arma::mat44 > &Mlist, const std::vector< arma::mat66 > &Glist, const std::vector< arma::vec6 > &Slist, const arma::vec &thetalistd, const arma::vec &dthetalistd, const arma::vec &ddthetalistd, const double kp, const double ki, const double kd)
	Computes the joint control torques at a particular time instant. More...
const std::tuple< std::vector< arma::vec >, std::vector< arma::vec > >	mr::SimulateControl (const arma::vec &thetalist, const arma::vec &dthetalist, const arma::vec3 &g, const std::vector< arma::vec6 > &Ftipmat, const std::vector< arma::mat44 > &Mlist, const std::vector< arma::mat66 > &Glist, const std::vector< arma::vec6 > &Slist, const std::vector< arma::vec > &thetamatd, const std::vector< arma::vec > &dthetamatd, const std::vector< arma::vec > &ddthetamatd, const arma::vec3 &gtilde, const std::vector< arma::mat44 > &Mtildelist, const std::vector< arma::mat66 > &Gtildelist, const double kp, const double ki, const double kd, const double dt, const size_t intRes)
	Simulates the computed torque controller over a given desired trajectory. More...

Detailed Description

Function Documentation

ComputeTorque()

const arma::vec mr::ComputeTorque	(	const arma::vec &	thetalist,
		const arma::vec &	dthetalist,
		const arma::vec &	eint,
		const arma::vec3 &	g,
		const std::vector< arma::mat44 > &	Mlist,
		const std::vector< arma::mat66 > &	Glist,
		const std::vector< arma::vec6 > &	Slist,
		const arma::vec &	thetalistd,
		const arma::vec &	dthetalistd,
		const arma::vec &	ddthetalistd,
		const double	kp,
		const double	ki,
		const double	kd
	)

Computes the joint control torques at a particular time instant.

Parameters

thetalist	n-vector of joint variables
dthetalist	n-vector of joint rates
eint	n-vector of the time-integral of joint errors
g	Gravity vector g
Mlist	List of link frames {i} relative to {i-1} at the home position
Glist	Spatial inertia matrices Gi of the links
Slist	Screw axes Si of the joints in a space frame, in the format of a matrix with axes as the columns
thetalistd	n-vector of reference joint variables
dthetalistd	n-vector of reference joint velocities
ddthetalistd	n-vector of reference joint accelerations
kp	The feedback proportional gain (identical for each joint)
ki	The feedback integral gain (identical for each joint)
kd	The feedback derivative gain (identical for each joint)

Returns

The vector of joint forces/torques computed by the feedback linearizing controller at the current instant

SimulateControl()

const std::tuple<std::vector<arma::vec>, std::vector<arma::vec> > mr::SimulateControl	(	const arma::vec &	thetalist,
		const arma::vec &	dthetalist,
		const arma::vec3 &	g,
		const std::vector< arma::vec6 > &	Ftipmat,
		const std::vector< arma::mat44 > &	Mlist,
		const std::vector< arma::mat66 > &	Glist,
		const std::vector< arma::vec6 > &	Slist,
		const std::vector< arma::vec > &	thetamatd,
		const std::vector< arma::vec > &	dthetamatd,
		const std::vector< arma::vec > &	ddthetamatd,
		const arma::vec3 &	gtilde,
		const std::vector< arma::mat44 > &	Mtildelist,
		const std::vector< arma::mat66 > &	Gtildelist,
		const double	kp,
		const double	ki,
		const double	kd,
		const double	dt,
		const size_t	intRes
	)

Simulates the computed torque controller over a given desired trajectory.

Parameters

thetalist	n-vector of initial joint variables
dthetalist	n-vector of initial joint velocities
g	Actual gravity vector g
Ftipmat	An N x 6 matrix of spatial forces applied by the end- effector (If there are no tip forces the user should input a zero and a zero matrix will be used)
Mlist	Actual list of link frames i relative to i-1 at the home position
Glist	Actual spatial inertia matrices Gi of the links
Slist	Screw axes Si of the joints in a space frame, in the format of a matrix with axes as the columns
thetamatd	An Nxn matrix of desired joint variables from the reference trajectory
dthetamatd	An Nxn matrix of desired joint velocities
ddthetamatd	An Nxn matrix of desired joint accelerations
gtilde	The gravity vector based on the model of the actual robot (actual values given above)
Mtildelist	The link frame locations based on the model of the actual robot (actual values given above)
Gtildelist	The link spatial inertias based on the model of the actual robot (actual values given above)
kp	The feedback proportional gain (identical for each joint)
ki	The feedback integral gain (identical for each joint)
kd	The feedback derivative gain (identical for each joint)
dt	The timestep between points on the reference trajectory
intRes	Integration resolution is the number of times integration (Euler) takes places between each time step. Must be an integer value greater than or equal to 1

Returns

taumat: An Nxn matrix of the controllers commanded joint forces/ torques, where each row of n forces/torques corresponds to a single time instant

thetamat: An Nxn matrix of actual joint angles The end of this function plots all the actual and desired joint angles using matplotlib and random libraries.