/***************************************************************
 * PrExtendedOpControl.h
 *
 * This class provides the extended operational space control interface.
 *
 ****************************************************************/

/*
 * modification history
 *----------------------
 *
 * 10/14/98: K.C. Chang: created.
 */

#ifndef _PrExtendedOpControl_h
#define _PrExtendedOpControl_h

#include "PrGlobalDefn.h"
#include "matrix/PrMatrix6.h"

class PrVector;
class PrMatrix;
class PrOpControl;
class PrTreeRobotIntf;
class PrJointControl;

class PrExtendedOpControl
{
 public:
  PrExtendedOpControl(PrJointControl *jtControl,PrTreeRobotIntf *robot);
  ~PrExtendedOpControl();

  PrMatrix &jacobianMatrix() { return *J_; }
  PrMatrix6 &lambdaMatrix() { return Lambda_; }

  PrOpControl &opControl();
  
  void update();
  //Float VelocityFilter(Float g); // Philip's experimental workaround for demo

  PrVector &computedTorque(const PrVector &ddqn);

  // BOB'S EXPERIMENTAL
//  PrVector &fo(){ return *fo_; }

 private:
  int noj_;
  PrJointControl *jtControl_;
  PrTreeRobotIntf *robot_;
  PrOpControl *opControl_;
  
  PrVector *tau_;
  PrMatrix *J_;
  PrMatrix *Jt_;
  PrMatrix6 Lambda_;

  PrVector *Jddqn_;
  PrVector *fo_;
  PrVector *dx_;

  PrVector6 fo_prime_vec6_;
  PrVector6 f_prime_vec6_;
  PrVector6 f_vec6_;
  PrVector6 fo_vec6_;

  void UpdateJacobianMatrix();
  void UpdateVelocity();
  void UpdateFrame();

  void UpdateLambdaMatrix();

  PrExtendedOpControl(const PrExtendedOpControl &) {}

  //Float dqo, qo; //Philip's experiment

};

#endif // _PrExtendedOpControl_h

