#include <stdio.h>
#include <string.h>
#include <stdlib.h>

#include "PrGlobalDefn.h"
#include "PrRobot.h"
#include "PrJointState.h"
#include "PrLink.h"
#include "matrix/PrVector3.h"
#include "matrix/PrMatrix6.h"
#include "matrix/PrTransform.h"

PrRobot *robot;
PrTransform T;
PrMatrix6 Lambda,J;

void UpdateState();
void Frame(PrTransform &globalT);
void Jacobian(PrMatrix6 &J);
void OSIM(PrMatrix6 &Lambda);

static PrMatrix6 Js,A,Lambdas,M;
static PrVector6 G;
static PrMatrix3 R;
static PrVector3 P;

void G