Velocity kinematic simulation routines

Mathematica conversion routine to use numerical differential equations solver

MakeDiffEq[p_, dh_] := <br />    Module[{r, vars, d1, d2, eq}, <br />  ... p;      eqs = (#[[1]] == #[[2]]) & /@ ({d1, d2} // Transpose)] ;

General simulation of a specified Cartesian velocity trajectory

This function simulates a trajectory specified by the velocity relationship Overscript[θ, .] = J^gOverscript[x, .], where J^g (jg) specifies some user defined inverse relationship (matrix), and the other function arguments are: dh = list of DH parameters, traj = desired end-effector velocity trajectory (as a function of t), initial = a list of initial joint angles for θ, and time = the simulation length (1, time).

SimulateVelocityTrajectory[jg_, dh_, traj_, initial_, time_:1] := <br />   &nbs ... ten ; <br />            vars /. sol] ;

Generalized Inverse Jacobian velocity trajectories

This function simulates a trajectory specified by the velocity relationship Overscript[θ, .] = J^* Overscript[x, .], where J^* is the pseudo-inverse of the Jacobian (inverse in the case of square matrices). The function arguments are: dh = list of DH parameters, pick = a list of length six of 0s and 1s, which specifies which rows of the Jacobian to use, traj = desired end-effetor velocity trajectory (as a function of t), initial = a list of initial joint angles for θ, and time = the simulation length (1, time).

SimulateInverseJacobianVelocityTrajectory[dh_, pick_, traj_, initial_, time_:1] := <br />  ... yTrajectory[jg, dh, traj, initial, time] <br />        ] ;

Short-hand for above function

SIJVT[a__] := SimulateInverseJacobianVelocityTrajectory[a] ;

Jacobian transpose velocity trajectories

This function simulates a trajectory specified by the velocity relationship Overscript[θ, .] = J^TOverscript[x, .], where J^T is the transpose of the Jacobian. The function arguments are: dh = list of DH parameters, pick = a list of length six of 0s and 1s, which specifies which rows of the Jacobian to use, traj = desired end-effetor velocity trajectory (as a function of t), initial = a list of initial joint angles for θ, and time = the simulation length (0, time).

SimulateTransposeJacobianVelocityTrajectory[dh_, pick_, traj_, initial_, time_:1] := <br />&nb ... sp; SimulateVelocityTrajectory[jt, dh, traj, initial, time] <br />    ] ;

Short-hand for above function

STJVT[a__] := SimulateTransposeJacobianVelocityTrajectory[a] ;

Inverse kinematics through Jacobian transpose

RowBox[{RowBox[{RowBox[{IKTJOneStep, [, RowBox[{j_, ,, , p_, ,, , dh_, ,, , theta_, ,, xf_, ... sp;   theta + dt * Transpose[jn] . (xf - xc) <br />    ]}], ;}]

These functions provide an iterative solution to the inverse kinematics (position only, for now).

RowBox[{RowBox[{InverseKinematicsTransposeJacobian[dh_, pick_, initial_, xf_, dt_], :=, <br /> ... t * (Length[res] - 1)}}] & /@ Transpose[res]}]}], <br />,     , ]}]}], ;}]

Short-hand for above function

IKTJ[a__] := InverseKinematicsTransposeJacobian[a] ;

Created by Mathematica  (October 1, 2003)