ROBOOP, A Robotics Object Oriented Package in C++: mRobot Class Reference

Definition at line 389 of file robot.h.


Public Member Functions
	mRobot (const int ndof=1)
	Constructor.
	mRobot (const Matrix &initrobot_motor)
	Constructor.
	mRobot (const Matrix &initrobot, const Matrix &initmotor)
	Constructor.
	mRobot (const mRobot &x)
	Copy constructor.
	mRobot (const std::string &filename, const std::string &robotName)
virtual	~mRobot ()
	Destructor.
virtual void	robotType_inv_kin ()
	Identify inverse kinematics familly.
ReturnMatrix	inv_kin (const Matrix &Tobj, const int mj=0)
	Overload inv_kin function.
virtual ReturnMatrix	inv_kin (const Matrix &Tobj, const int mj, const int endlink, bool &converge)
	Inverse kinematics solutions.
virtual ReturnMatrix	inv_kin_rhino (const Matrix &Tobj, bool &converge)
	Analytic Rhino inverse kinematics.
virtual ReturnMatrix	inv_kin_puma (const Matrix &Tobj, bool &converge)
	Analytic Puma inverse kinematics.
virtual ReturnMatrix	inv_kin_schilling (const Matrix &Tobj, bool &converge)
	Analytic Schilling inverse kinematics.
virtual void	kine_pd (Matrix &Rot, ColumnVector &pos, ColumnVector &pos_dot, const int ref) const
	Direct kinematics with velocity.
virtual ReturnMatrix	jacobian (const int ref=0) const
	Jacobian of mobile links expressed at frame ref.
virtual ReturnMatrix	jacobian (const int endlink, const int ref) const
	Jacobian of mobile joints up to endlink expressed at frame ref.
virtual ReturnMatrix	jacobian_dot (const int ref=0) const
	Jacobian derivative of mobile joints expressed at frame ref.
virtual void	dTdqi (Matrix &dRot, ColumnVector &dp, const int i)
	Partial derivative of the robot position (homogeneous transf.).
virtual ReturnMatrix	dTdqi (const int i)
	Partial derivative of the robot position (homogeneous transf.).
virtual ReturnMatrix	torque (const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp)
	Joint torque, without contact force, based on Recursive Newton-Euler formulation.
virtual ReturnMatrix	torque (const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp, const ColumnVector &Fext_, const ColumnVector &Next_)
	Joint torque based on Recursive Newton-Euler formulation.
virtual ReturnMatrix	torque_novelocity (const ColumnVector &qpp)
	Joint torque. when joint velocity is 0, based on Recursive Newton-Euler formulation.
virtual void	delta_torque (const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp, const ColumnVector &dq, const ColumnVector &dqp, const ColumnVector &dqpp, ColumnVector &torque, ColumnVector &dtorque)
	Delta torque dynamics.
virtual void	dq_torque (const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp, const ColumnVector &dq, ColumnVector &torque, ColumnVector &dtorque)
	Delta torque due to delta joint position.
virtual void	dqp_torque (const ColumnVector &q, const ColumnVector &qp, const ColumnVector &dqp, ColumnVector &torque, ColumnVector &dtorque)
	Delta torque due to delta joint velocity.
virtual ReturnMatrix	G ()
	Joint torque due to gravity based on Recursive Newton-Euler formulation.
virtual ReturnMatrix	C (const ColumnVector &qp)
	Joint torque due to centrifugal and Corriolis based on Recursive Newton-Euler formulation.

Member Function Documentation

void mRobot::robotType_inv_kin ( ) [virtual]

Identify inverse kinematics familly.

Identify the inverse kinematics analytic solution based on the similarity of the robot DH parameters and the DH parameters of know robots (ex: Puma, Rhino, ...). The inverse kinematics will be based on a numerical alogorithm if there is no match .

Implements Robot_basic.

Definition at line 1354 of file robot.cpp.

References Robot_basic::DEFAULT, Robot_basic::PUMA, Puma_mDH(), Robot_basic::RHINO, Rhino_mDH(), Robot_basic::robotType, Robot_basic::SCHILLING, and Schilling_mDH().

Referenced by mRobot().

ReturnMatrix mRobot::inv_kin	(	const Matrix &	Tobj,
		const int	mj,
		const int	endlink,
		bool &	converge
	)			`[virtual]`

Inverse kinematics solutions.

The solution is based on the analytic inverse kinematics if robot type (familly) is Rhino or Puma, otherwise used the numerical algoritm defined in Robot_basic class.

Reimplemented from Robot_basic.

Definition at line 603 of file invkine.cpp.

References Robot_basic::inv_kin(), inv_kin_puma(), inv_kin_rhino(), inv_kin_schilling(), Robot_basic::PUMA, Robot_basic::RHINO, Robot_basic::robotType, and Robot_basic::SCHILLING.

ReturnMatrix mRobot::inv_kin_rhino	(	const Matrix &	Tobj,
		bool &	converge
	)			`[virtual]`

Analytic Rhino inverse kinematics.

converge will be false if the desired end effector pose is outside robot range.

Implements Robot_basic.

Definition at line 628 of file invkine.cpp.

References Robot_basic::a, Link::a, Link::d, G(), Robot_basic::get_q(), K, Robot_basic::links, and M_PI.

Referenced by inv_kin().

ReturnMatrix mRobot::inv_kin_puma	(	const Matrix &	Tobj,
		bool &	converge
	)			`[virtual]`

Analytic Puma inverse kinematics.

converge will be false if the desired end effector pose is outside robot range.

Implements Robot_basic.

Definition at line 733 of file invkine.cpp.

References Robot_basic::a, Link::a, C(), Link::d, Robot_basic::get_q(), Robot_basic::links, and M_PI.

Referenced by inv_kin().

ReturnMatrix mRobot::inv_kin_schilling	(	const Matrix &	Tobj,
		bool &	converge
	)			`[virtual]`

Analytic Schilling inverse kinematics.

converge will be false if the desired end effector pose is outside robot range.

Implements Robot_basic.

Definition at line 893 of file invkine.cpp.

References Robot_basic::a, Link::a, C(), Link::d, Robot_basic::get_q(), K, Robot_basic::links, and M_PI.

Referenced by inv_kin().

void mRobot::kine_pd	(	Matrix &	Rot,
		ColumnVector &	pos,
		ColumnVector &	pos_dot,
		const int	j
	)			const `[virtual]`

Direct kinematics with velocity.

Parameters:

	Rot,:	Frame j rotation matrix w.r.t to the base frame.
	pos,:	Frame j position vector wr.r.t to the base frame.
	pos_dot,:	Frame j velocity vector w.r.t to the base frame.
	j,:	Frame j. Print an error on the console if j is out of range.

Implements Robot_basic.

Definition at line 552 of file kinemat.cpp.

ReturnMatrix mRobot::jacobian	(	const int	endlink,
		const int	ref
	)			const `[virtual]`

Jacobian of mobile joints up to endlink expressed at frame ref.

See Robot::jacobian for equations.

Implements Robot_basic.

Definition at line 682 of file kinemat.cpp.

ReturnMatrix mRobot::jacobian_dot ( const int ref = 0 ) const [virtual]

Jacobian derivative of mobile joints expressed at frame ref.

See Robot::jacobian_dot for equations.

Implements Robot_basic.

Definition at line 736 of file kinemat.cpp.

void mRobot::dTdqi	(	Matrix &	dRot,
		ColumnVector &	dp,
		const int	i
	)			`[virtual]`

Partial derivative of the robot position (homogeneous transf.).

This function computes the partial derivatives:

$\frac{\partial{}^0 T_n}{\partial q_i} = {}^0 T_{i} Q_i \; {}^{i} T_n$

with

$Q_i = \left [ \begin{array}{cccc} 0 & -1 & 0 & 0 \\ 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \end{array} \right ]$

for a revolute joint and

$Q_i = \left [ \begin{array}{cccc} 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & 0 & 0 \end{array} \right ]$

for a prismatic joint.

$dRot$ and $dp$ are modified on output.

Implements Robot_basic.

Definition at line 582 of file kinemat.cpp.

References Robot_basic::dof, Robot_basic::dp, Robot_basic::links, Robot_basic::p, Link::p, Link::R, Robot_basic::R, and threebythreeident.

Referenced by dTdqi().

ReturnMatrix mRobot::dTdqi ( const int i ) [virtual]

Partial derivative of the robot position (homogeneous transf.).

See mRobot::dTdqi(Matrix & dRot, ColumnVector & dp, const int i) for equations.

Implements Robot_basic.

Definition at line 664 of file kinemat.cpp.

References dTdqi().

ReturnMatrix mRobot::torque	(	const ColumnVector &	q,
		const ColumnVector &	qp,
		const ColumnVector &	qpp,
		const ColumnVector &	Fext_,
		const ColumnVector &	Next_
	)			`[virtual]`

Joint torque based on Recursive Newton-Euler formulation.

In order to apply the RNE, let us define the following variables (referenced in the $i^{th}$ coordinate frame if applicable):

$\sigma_i$ is the joint type; $\sigma_i = 1$ for a revolute joint and $\sigma_i = 0$ for a prismatic joint.

$z_0 = \left [ \begin{array}{ccc} 0 & 0 & 1 \end{array} \right ]^T$

$p_i = \left [ \begin{array}{ccc} a_{i-1} & -d_i sin \alpha_{i-1} & d_i cos \alpha_{i-1} \end{array} \right ]^T$ is the position of the $i^{th}$ with respect to the $i-1^{th}$ frame.

Forward Iterations for $i=1, 2, \ldots, n$ . Initialize: $\omega_0 = \dot{\omega}_0 = 0$ and $\dot{v}_0 = - g$ .

$\omega_i = R_i^T\omega_{i-1} + \sigma_i z_0\dot{\theta_i}$

$\dot{\omega}_i = R_i^T\dot{\omega}_{i-1} + \sigma_i R_i^T\omega_{i-1}\times z_0 \dot{\theta}_i + \sigma_iz_0\ddot{\theta}_i$

$\dot{v}_i = R_i^T(\dot{\omega}_{i-1}\times p_i + \omega_{i-1}\times(\omega_{i-1}\times p_i) + \dot{v}_{i-1}) + (1 - \sigma_i)(2\omega_i\times z_0 dot{d}_i + z_0\ddot{d}_i)$

Backward Iterations for $i=n, n-1, \ldots, 1$ . Initialize: $f_{n+1} = n_{n+1} = 0$ .

$\dot{v}_{ci} = \dot{\omega}_i\times r_i + \omega_i\times(\omega_i\times r_i) + v_i$

$F_i = m_i \dot{v}_{ci}$

$N_i = I_{ci}\ddot{\omega}_i\ + \omega_i \times I_{ci}\omega_i$

$f_i = R_{i+1}f_{i+1} + F_i$

$n_i = N_i + R_{i+1} n_{i+1} + r_i \times F_i + p_{i+1}\times R_{i+1}f_{i+1}$

$\tau_i = \sigma_i n_i^T z_0 + (1 - \sigma_i) f_i^T z_0$

Implements Robot_basic.

Definition at line 422 of file dynamics.cpp.

References Robot_basic::a, Link::B, Link::Cf, Robot_basic::dof, Robot_basic::f, Robot_basic::F, Robot_basic::fix, Link::Gr, Robot_basic::gravity, Link::I, Link::Im, Robot_basic::links, Robot_basic::n, Robot_basic::N, Robot_basic::p, Link::R, Robot_basic::R, Robot_basic::set_q(), Robot_basic::set_qp(), sign(), Robot_basic::vp, Robot_basic::w, Robot_basic::wp, and Robot_basic::z0.

void mRobot::delta_torque	(	const ColumnVector &	q,
		const ColumnVector &	qp,
		const ColumnVector &	qpp,
		const ColumnVector &	dq,
		const ColumnVector &	dqp,
		const ColumnVector &	dqpp,
		ColumnVector &	ltorque,
		ColumnVector &	dtorque
	)			`[virtual]`

Delta torque dynamics.

This function computes

$\delta \tau = D(q) \delta \ddot{q} + S_1(q,\dot{q}) \delta \dot{q} + S_2(q,\dot{q},\ddot{q}) \delta q$

Murray and Neuman Cite_: Murray86 have developed an efficient recursive linearized Newton-Euler formulation. In order to apply the RNE as presented in let us define the following variables

$p_{di} = \frac{\partial p_i}{\partial d_i} = \left [ \begin{array}{ccc} 0 & \sin \alpha_i & \cos \alpha_i \end{array} \right ]^T$

$Q = \left [ \begin{array}{ccc} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 0 \end{array} \right ]$

Forward Iterations for $i=1, 2, \ldots, n$ . Initialize: $\delta \omega_0 = \delta \dot{\omega}_0 = \delta \dot{v}_0 = 0$ .

$\delta \omega_i = R_i^T \delta \omega_{i-1} + \sigma_i (z_0 \delta \dot{\theta}_i - QR_i^T \omega_i \delta \theta_i)$

$\delta \dot{\omega}_i = R_i^T \delta \dot{w}_{i-1} + \sigma_i [R_i^T \delta \omega_{i-1} \times z_0 \dot{\theta}_i + R_i^T \omega_{i-1} \times z_0 \delta \dot{\theta}_i + z_0 \ddot{\theta}_i - (QR_i^T \dot{\omega}_{i-1} + QR_i^T \omega_{i-1} \times \omega{z}_0 \dot{\theta}_i) \delta \theta_i ]$

$\delta \dot{v}_i = R_i^T\Big(\delta \dot{\omega}_{i-1} \times p_i + \delta \omega_{i-1} \times(\omega_{i-1} \times p_i) + \omega_{i-1} \times( \delta \omega_{i-1} \times p_i) + \delta \dot{v}_i\Big) + (1-\sigma_i)\Big(2\delta \omega_i \times z_0\dot{d}_i + 2\omega_i \times z_0 \delta \dot{d}_i + z_0 \delta \ddot{d}_i\Big) - \sigma_i QR_i^T \Big(\dot{\omega}_{i-1} \times p_i + \omega_{i-1} \times(w_{i-1}\times p_i) + \dot{v}_i\Big) \delta \theta_i + (1-\sigma_i) R_i^T \Big(\dot{\omega}_{i-1} \times p_{di} + \omega_{i-1} \times(\omega_{i-1}\times p_{di})\Big) \delta d_i$

Backward Iterations for $i=n, n-1, \ldots, 1$ . Initialize: $\delta f_{n+1} = \delta n_{n+1} = 0$ .

$\delta \dot{v}_{ci} = \delta \dot{v}_i + \delta \dot{\omega}_i\times r_i + \delta \omega_i \times(\omega_i \times r_i) + \omega_i \times ( \delta \omega_i \times r_i)$

$\delta F_i = m_i \delta \dot{v}_{ci}$

$\delta N_i = I_{ci} \delta \dot{\omega}_i + \delta \omega_i \times (I_{ci}\omega_i) + \omega_i \times (I_{ci} \delta \omega_i)$

$\delta f_i = R_{i+1} \delta f_{i+1} + \delta F_i + \sigma_{i+1} R_{i+1} Qf_{i+1} \delta \theta_{i+1}$

$\delta n_i = \delta N_i + R_{i+1} \delta n_{i+1} + r_i\times \delta F_i + p_{i+1} \times R_{i+1} \delta f_{i+1} + \sigma_{i+1} \Big( R_{i+1} Qn_{i+1} + p_{i+1} \times R_{i+1} Qf_{i+1} \Big) \delta \theta_{i+1} + (1-\sigma_{i+1} ) p_{di+1} p_{di+1} \times R_{i+1} f_{i+1} \delta d_{i+1}$

$\delta \tau_i = \sigma \delta n_i^T z_0 + (1 - \sigma_i) \delta f_i^T z_0$

Implements Robot_basic.

Definition at line 291 of file delta_t.cpp.

References Robot_basic::a, Robot_basic::da, Robot_basic::df, Robot_basic::dF, Robot_basic::dn, Robot_basic::dN, Robot_basic::dof, Robot_basic::dp, Robot_basic::dvp, Robot_basic::dw, Robot_basic::dwp, Robot_basic::f, Robot_basic::F, Robot_basic::gravity, Link::I, Robot_basic::links, Link::m, Robot_basic::n, Robot_basic::N, Link::p, Robot_basic::p, Link::R, Robot_basic::R, Robot_basic::set_q(), Robot_basic::vp, Robot_basic::w, Robot_basic::wp, and Robot_basic::z0.

void mRobot::dq_torque	(	const ColumnVector &	q,
		const ColumnVector &	qp,
		const ColumnVector &	qpp,
		const ColumnVector &	dq,
		ColumnVector &	ltorque,
		ColumnVector &	dtorque
	)			`[virtual]`

void mRobot::dqp_torque	(	const ColumnVector &	q,
		const ColumnVector &	qp,
		const ColumnVector &	dqp,
		ColumnVector &	ltorque,
		ColumnVector &	dtorque
	)			`[virtual]`

mRobot Class Reference

Detailed Description

Public Member Functions

Member Function Documentation