/***************************************************************
 * PrTaskControl.cpp
 *
 * This class provides the Multi-tasks control impelementation.
 *
 ****************************************************************/

/*
 * modification history
 *----------------------
 *
 * 7/28/03: Jaeheung Park: created. "experimental version"
 */

#include "matrix/PrVector.h"
#include "matrix/PrMatrix.h"
#include "matrix/PrQuaternion.h"
#include "matrix/PrTransform.h"
#include "robot/PrLink.h"
#include "robot/PrRobot.h"
#include "robot/PrJointState.h"
#include "PrOpControl.h"
#include "PrJointControl.h"
#include "PrTreeRobotIntf.h"
#include "PrTask.h"
#include "EndEffectorCtrl.h"
#include "BaseCtrl.h"
#include "PrTaskControl.h"


PrTaskControl::PrTaskControl(int servoRate,PrTreeRobotIntf *robot)
{
  servoRate_ = servoRate;
  dof_ = 9;
  robot_ = robot;
  noj_ = robot_->noj(); 

  robotVirtual_ = new PrRobot(noj_,"./samm.prm",
			PR_ROBOT_FWD_DYNAMICS, PR_ROBOT_FILETYPE_GLOBAL);
  // Initialize virtualRobot.
  for(int i=0;i<noj_;i++) {
	robotVirtual_->jointState()->q()[i] = 0.0;
	robotVirtual_->jointState()->dq()[i] = 0.0;
    }
  robotVirtual_->updateTransform(); // not necessary , I guess

  q_ = new PrVector(noj_);
  dq_ = new PrVector(noj_);

  //opControl_ = new PrOpControl;

  // Assign the last link for the operational point.
  // opControl_->link(robot_->robot()->lastLink());
  // //opControl_->link(robot_->robot()->endEffector());

  // //opControl_->servoRate((int)(1.0/jtControl->period()));
  // opControl_->servoRate(servoRate_);
  // opControl_->id(0);

  CreateInitialTasks();

  tau_ = new PrVector(noj_);

  Gamma_ = new PrVector(noj_);
  Nt_ = new PrMatrix(noj_,noj_);

  /*
  /*
  J_ = new PrMatrix(6,noj_);
  Jt_ = new PrMatrix(noj_,6);

  Jddqn_ = new PrVector(6);
  fo_ = new PrVector(6);
  dx_ = new PrVector(6);
  */

}

PrTaskControl::~PrTaskControl()
{
  // delete opControl_;
  delete q_;
  delete dq_;
  for(int i=0;