/*
 * File:   libpuma.h
 *
 * This file provides an interface to libpuma.cpp
 */

/*
 * modification history
 *----------------------
 *
 * 03/28/98: K.C. Chang: created.
 */

#ifndef _libpuma_h
#define _libpuma_h


#include "matrix/PrTransform.h"
#include "matrix/PrVector6.h"
#include "matrix/PrMatrix6.h"
#include "network/PrDataInput.h"
#include "network/PrDataOutput.h"
#include "robot/PrRobot.h"

class PrForceSensorIntf;
class PrAccelSensorIntf;

extern "C"
{

/*
 * Function: pumaInitManipulator
 * Usage:    pumaInitManipulator(clockrate,paramfile);
 *
 * input:    clockrate is in Hz: (ex)  clockrate = 500 Hz
 *           if paramfile==NULL /local/pr/puma560.prm is used. 
 * Initializes necessary variables and flags for PUMA 560.
 * Also, sets the servo(sampling) rate to clockrate.
 *
 */
void pumaInitManipulator( int clockrate, const char* paramfile=NULL );

/*
 * Resets the encoder values to zero.
 */
void pumaEncoderReset( void );

/*
 * Function: pumaGetTime
 * Usage:    time = pumaGetTime();
 *
 * Returns the current clock(machine) time(sec) from 
 * the beginning of this session.
 */
float pumaGetTime();

/*
 * Function: pumaWaitNextTick
 * Usage:    pumaWaitNextTick();
 *
 * Comes back after a unit tick period.
 * If the servo rate is faster than the actual time needed by your program,
 * this function shuts the manipulator's power down and writes an error
 * message. 
 */
void pumaWaitNextTick();

/*
 * Function: pumaGetPos
 * Usage:    pumaGetPos(q);
 *
 * output:   q
 * Puts the current joint positions to q.
 * the unit is in radians.
 */
void pumaGetPos(PrVector6 &q);

/*
 * Function: pumaGetVel
 * Usage:    pumaGetVel(dq);
 *
 * output:   dq
 * Puts the current estimated joint velocities to dq.
 * the unit is in radians/second.
 */
void pumaGetVel(PrVector6 &dq);

/*
 * Function: pumaGripperCommand
 * Usage:    set goal for the gripper, [0...78]
 *
 * input:    pos
 * Open or close the gripper.
 */
void pumaGripperCommand( int goal );


/*
 * Function: pumaSetOSFrameLocal
 * Usage:    pumaSetOSFrameLocal(T)
 *
 * input:    
 * define operational space frame in wrist frame
 */
void pumaSetOSFrameLocal(PrTransform &localT);

/*
 * Function: pumaSetOSFrameGlobal
 * Usage:    pumaSetOSFrameGlobal(T)
 *
 * input:    
 * define operational space frame globally
 */
void pumaSetOSFrameGlobal(PrTransform &globalT);

/*
 * Function: pumaGetOSFrameLocal
 * Usage:    pumaGetOSFrameLocal(T)
 *
 * input:    
 * caculate operational space frame in wrist frame
 */
void pumaGetOSFrameLocal(PrTransform &localT);

/*
 * Function: pumaGetOSFrameGlobal
 * Usage:    pumaGetOSFrameGlobal(T)
 *
 * input:    
 * caculate operational space frame in global frame
 */
void pumaGetOSFrameGlobal(PrTransform &globalT);

/*
 * Function: pumaGetWristFrameGlobal
 * Usage:    pumaGetWristFrameGlobal(T)
 *
 * input:    
 * caculate wrist frame in global frame
 */
void pumaGetWristFrameGlobal(PrTransform &globalT);

/*
 * Function: pumaGetFSFrameLocal
 * Usage:    pumaGetFSFrameLocal(T)
 *
 * input:    
 * caculate force sensor frame in wrist frame
 */
void pumaGetFSFrameLocal(PrTransform &localT);

/*
 * Function: pumaGetEEFrameLocal
 * Usage:    pumaGetEEFrameLocal(T)
 *
 * input:    
 * caculate end-effector frame in wrist frame
 */
void pumaGetEEFrameLocal(PrTransform &localT);

/*
 * Function: pumaGetOSIM
 * Usage:    pumaGetOSIM(L);
 *
 * output:   L    
 * caculate operational space inertia matrix
 */
void pumaGetOSIM(PrMatrix6 &Lambda);

/*
 * Function: pumaGetJacobian
 * Usage:    pumaGetJacobian(J);
 *
 * output:   J   
 * caculate Jacobian.
 */
void pumaGetJacobian(PrMatrix6 &J);

/*
 * Function: pumaGetComputedTorque
 * Usage:    pumaGetComputedTorque(t,tau);
 *
 * input:    t
 * output:   tau
 * caculate dynamically computed joint torques, tau from unit mass torque, t.
 */
void pumaGetComputedTorque(PrVector6 &t, PrVector6 &tau);

/*
 * Function: pumaSetTorque
 * Usage:    pumaSetTorque(tau);
 *
 * input:    tau
 * Applys the caculated joint torques, tau.
 */
void pumaSetTorque(PrVector6 &tau);

/*
 * Function: pumaShutDown
 * Usage:    pumaShutDown();
 *
 * Shuts the manipulator's power down.
 */
void pumaShutDown();

/*
 * Function: pumaForceSensor
 * Usage:   pumaForceSensor()
 *
 * This function returns an interface to the force sensor.  Do a
 * dynamic_cast<> to get a specific derived type.
 */
PrForceSensorIntf * pumaForceSensor( void );
 
/* 
 * Function: pumaGetForce
 * Usage:    error = pumaGetForce()
 *
 * This function returns the force values.
 * The return value is PUMA_OK if reading was successful.
 * NOTE: compression +
 */
int pumaGetForce(PrVector6 &forces);

/*
 * Function: pumaZeroForce
 * Usage:    error = pumaZeroForce()
 *
 * Zeroes the force sensor.
 * The return value is PUMA_OK if reading was successful.
 */
int pumaZeroForce ();

/*
 * Function: pumaAccelSensor
 * Usage:   pumaAccelSensor()
 *
 * This function returns an interface to the accel sensor.  Do a
 * dynamic_cast<> to get a specific derived type.
 */
PrAccelSensorIntf * pumaAccelSensor( void );

/*
 * Function: pumaGetAccel
 * Usage:    error = pumaGetAccel()
 *
 * This function returns the accel values at the accel sensor.
 * The return value is PUMA_OK if reading was successful.
 * NOTE: compression +
 */
int pumaGetAccel (PrVector6 &accels);

/*
 * Function: pumaZeroAccel
 * Usage:    error = pumaZeroAccel()
 *
 * Zeroes the accel sensor.
 * The return value is PUMA_OK if reading was successful.
 */
int pumaZeroAccel ();

/*
 * Function: pumaGetForceWrist
 * Usage:    error = pumaForceWrist()
 *
 * gets the force values at the wrist frame
 * The return value is PUMA_OK if reading was successful.
 * NOTE: compression +
 */
int pumaGetForceWrist(PrVector6 &Fw);

/*
 * Function: pumaInitSymbolic
 * Usage:    pumaInitSymbolic()
 *
 * initializes internal values for the symbolic eom
 * NOTE: this should be called once in every tick
 */
void pumaInitSymbolic(PrVector6 &q);


/*
 * Function: pumaGetOSIMSymbolic
 * Usage:    pumaGetOSIMSymbolic()
 *
 * returns the operational space inertia matrix
 * Lambda = (J Ainv Jt)inv
 */
void pumaGetOSIMSymbolic(PrMatrix6 &J,PrMatrix6 &A,PrMatrix6 &Lambda);

/*
 * Function: pumaGetWristFrameSymbolic
 * Usage:    pumaGetWristFrameSymbolic()
 *
 * returns the wrist frame in frame {0} 
 */
void pumaGetWristFrameSymbolic(PrMatrix3 &R,PrVector3 &P);

/*
 * Function: pumaGetJacobianSymbolic
 * Usage:    pumaGetJacobianSymbolic()
 *
 * returns the jacobian of the origin of the wrist frame in frame {0} 
 */
void pumaGetJacobianSymbolic(PrMatrix6 &J);

/*
 * Function: pumaGetJacobianDotSymbolic
 * Usage:    pumaGetJacobianDotSymbolic()
 *
 * returns the time derivative of jacobian of the wrist frame in frame {0} 
 */
void pumaGetJacobianDotSymbolic( PrMatrix6 &Jdot, const PrVector6& dq );

/*
 * Function: pumaGetGravitySymbolic
 * Usage:    pumaGetGravitySymbolic()
 *
 * returns the gravity vector
 */
void pumaGetGravitySymbolic(PrVector6 &G);

/*
 * Function: pumaGetJSIMSymbolic
 * Usage:    pumaGetJSIMSymbolic()
 *
 * returns the joint space inertia matrix
 */
void pumaGetJSIMSymbolic(PrMatrix6 &M);


/*
 * Function: pumaGetRobot
 * Usage:    pumaGetRobot()
 *
 * returns the robot object, which can be
 * queried for inv/fwd dynamics, force sensor,
 * gripper, etc.
 */
PrRobot* pumaGetRobot();

}

#endif /* libpuma.h */






