//////////////////////////////////////////////////////////////////////////////
//
//       Title           : DynamicsTut1.cpp
//       Project         : OSCAR
//       Created         : 7/24/2003
//       Author          : Chalongrath Pholsiri, Oziel Rios
//       Platforms       : All
///       Copyright      : Copyright© The University of Texas at Austin, 2002. All rights reserved.
///                 
///          This software and documentation constitute an unpublished work
///          and contain valuable trade secrets and proprietary information
///          belonging to University.  None of the foregoing material may be
///          copied  or duplicated or disclosed without the express, written
///          permission of University.  UNIVERSITY EXPRESSLY DISCLAIMS ANY
///          AND ALL WARRANTIES CONCERNING THIS SOFTWARE AND DOCUMENTATION,
///          INCLUDING ANY WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A
///          PARTICULAR PURPOSE, AND WARRANTIES OF PERFORMANCE, AND ANY WARRANTY
///          THAT MIGHT OTHERWISE ARISE FROM COURSE OF DEALING OR USAGE OF TRADE.
///          NO WARRANTY IS EITHER EXPRESS OR IMPLIED WITH RESPECT TO THE USE OF
///          THE SOFTWARE OR DOCUMENTATION.  Under no circumstances shall
///          University be liable for incidental, special, indirect, direct or
///          consequential damages or loss of profits, interruption of business,
///          or related expenses which may arise from use of software or documentation,
///          including but not limited to those resulting from defects in software
///          and/or documentation, or loss or inaccuracy of data of any kind.
//       Access          : Company Confidential
//       Purpose         :
//
//----------------------------------------------------------------------------
//
//       Classes:
//               <none>
//
//       Global Functions:
//               <none>
//
//       Global Variables:
//               <none>
//
//----------------------------------------------------------------------------
//
//       $Revisions$
//
//       $Log: DynamicsTut1.cpp,v $
//       Revision 1.1  2003/08/14 16:53:37  pmarch
//       no message
//
//
//////////////////////////////////////////////////////////////////////////////
#include "ForwardKinematics/FKAcceleration.h"		// provides generalized forward acceleration solution
#include "Dynamics/IDNewtonEuler.h"							// provides Newton-Euler inverse dynamics

using namespace std;

int main()
{
  //create an RRFKAcceleration object from the DH file
	RRFKAcceleration fka("DataFiles/3dof_planar.dh");

  //Set up the tool point for the robot
  RRVector3 toolPoint(0.5, 0.0, 0.0);
  fka.SetTool(toolPoint);

  //get the degrees of freedom of the robot from the forward kinematics object
	const unsigned int DOF = fka.GetDOF();

  //create a vector for the gravity constant in the Y direction
  RRVector3D gravity(0.0, -9.807, 0);

  //create an RRIDNewtonEuler object from the CGM (centers of gravity and masses) file, 
  //INE (inertia) file, and the vector gravity
  RRIDNewtonEuler robot(3, "DataFiles/3dof_planar.cgm", "DataFiles/3dof_planar.ine", gravity);
  
  //create vectors to hold joint positions, velocities, and accelerations
  RRVector pos(DOF), vel(DOF), acc(DOF);
  pos[0] = 0;	pos[1] = 45;	pos[2] = 0;
  vel[0] = 50;	vel[1] = 20;	vel[2] = 10;
  acc[0] = 10;	acc[1] = 10;	acc[2] = 10;
  pos *= DegToRad;
  vel *= DegToRad;
  acc *= DegToRad;

  //set the current joint positions in the forward kinematics object to pos
  fka.GetHandPose(pos);
  //get all the local transformation matrices at the current joint positions from the forward kinematics object
  RRXform* transformations = (RRXform*)fka.GetAllLocalTransformations();
  //set the transformation matrices in the Newton-Euler dynamics object
  robot.SetLocalTransformations(transformations);
 
  //create vectors to hold joint torques due to gravity, velocity, and acceleration
  RRVector gravTorques(DOF), velTorques(DOF), accTorques(DOF);
  //compute the gravity torques (torques due to weights)
  robot.GetGravityTorques(gravTorques);
  //compute the velocity torques (torques due to Coriolis/centripetal effects)
  robot.GetVelocityTorques(vel, velTorques);
  //compute the acceleration torques (torques due to inertia)
  robot.GetAccelerationTorques(acc, accTorques);
  //compute the totol joint torques
  RRVector totalTorques = (RRVector)robot.GetJointTorques(vel,acc);

  cout << "Gravity Torques:\t" << gravTorques << endl;
  cout << "Velocity Torques:\t" << velTorques << endl;
  cout << "Acceleration Torques:\t" << accTorques << endl;
  cout << "Total Torques:\t" << totalTorques << endl;
 
  return 0;
}