AssemblyStrategy.h

/*
 * AssemblyStrategy.h
 *
 *  Created on: Mar 19, 2012
 *      Author: juan
 *
 *      This class was derived from the one found in /home/juan/openhrp/OpenHRP-3.1.0-Release/sample/controller/PA10Controller2a
 *      This class has abstracted all the code necessary to run the strategy based on the Control Basis approach.
 *      This class currently has not introduced the Straight Line Approach code.
 *
 */
#ifndef ASSEMBLYSTRATEGY_H_
#define ASSEMBLYSTRATEGY_H_
//---------------------------------------------------------------------------------------------------------------------------
#include "hrpModelHeaders.h"			// Dynamics Simulator Folder
//---------------------------------------------------------------------------------------------------------------------------
// TVMet Files
#include <tvmet/Matrix.h>
#include <tvmet/Vector.h>
#include <tvmet/VectorFunctions.h>
//---------------------------------------------------------------------------------------------------------------------------
// Boost Files
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/vector.hpp>
//---------------------------------------------------------------------------------------------------------------------------
// Supporting Libraries: ControlBasis, FilterTools, and OpenRave.
#include "ControlBasis.h"
#include "FilterTools.h"
#include "OpenRAVE/OpenRAVE_IK.h"
//---------------------------------------------------------------------------------------------------------------------------
// STL Libraries.
#include <iostream>
#include <math.h>
#include <fstream>
#include <sstream>
#include <stdio.h>			// string operations
#include <stdlib.h>		// posix
#include <string.h>
#include <string>
#include <time.h>
#include <sys/time.h>
//#include <sys/syspage.h>
#include <vector>
#include <dlfcn.h>
//---------------------------------------------------------------------------------------------------------------------------
// Namespaces
using namespace std;
using namespace OpenHRP;
using namespace tvmet;
using OpenHRP::BodyPtr;
//---------------------------------------------------------------------------------------------------------------------------
// Typedef's
typedef tvmet::Vector<double, 6> vector6;
//---------------------------------------------------------------------------------------------------------------------------

//---------------------------------------------------------------------------------------------------------------------------
// GLOBAL VARIABLES
//---------------------------------------------------------------------------------------------------------------------------
// String length
#define STR_LEN			    		256
//---------------------------------------------------------------------------------------------------------------------------
// Degrees of Freedom for body and arm
#define ARM_DOF               		 6			// Right and left arm only have 6 DOF.
#define ROBOT_DOF            		15			// HIRO has 15 DOF. 3 in torso/head and six in each arm.
//---------------------------------------------------------------------------------------------------------------------------
// Filter parameters
#define HISTBUFF_LENGTH 	 		12 			// This value determines the size of the possible number of samples to be included to perform a moving average for data signals.
//---------------------------------------------------------------------------------------------------------------------------
// FORCE CONSTANTS FOR CONTROL POLICY
#define CONST_FORCE_STATE3     		 5.0		// All forces need to be in world coordinates. Used in Hiro Side Approach (HSA) execution.
#define VERTICAL_FORCE	      		10.0		// HSA: Used in state 3 and 4 of pivot approach/stage 3 of side approach. pos value pushes down. 10kg Force pointing downwards
#define TRANSVERSE_FORCE      		0.25		// HSA: Moves out of the screen. Towards the desired wall of the mold in the side approach
#define HORIZONTAL_FORCE      		0.30		// HSA: There is a natural push to the left by the robot and gravity. This compensates for that during the rotation.
//---------------------------------------------------------------------------------------------------------------------------
// ROTATIONAL Time and Force Variables
#define ROTATIONAL_FORCE     		20.0		// Used in state 3 of side approach to close camera mold with a snap. 20N-m
#define ROTATION_TIME_SLOW    		3.0			// Slow time for testing and faster time for optimized trajectory
//---------------------------------------------------------------------------------------------------------------------------
// CONTACT Transitional Parameters
#define HSA_App2Rot_Fx				9.0			// HSA: Transition condition between Approach and Rotation stages. Used in Fx = 9N
#define HSA_Rot2Ins_My				0.8			// HSA: Transition condition between Approach and Rotation stages. Used in Fx = 9N
//  --- Diro -------------------
#define TwoArm_SA_App2Rot_Fx				0.95//0.45//5.6//9.0		// TwoArm_SA: Transition condition between Approach and Rotation stages.
#define TwoArm_SA_Rot2Ins_My				-0.0332226			// TwoArm_SA: Transition condition between Rotation and Insertion stages.
#define TwoArm_SA_Ins2SubIns_My				-0.167044
//---------------------------------------------------------------------------------------------------------------------------
// Math variables
#define PI 		      				3.1416
#define RAD2DEG            			180.0/PI
//---------------------------------------------------------------------------------------------------------------------------

// Class Forwarding
class ControlBasis;							// Need this declaration in order for the AssemblyStrategy class to know the type of the Control Basis Class

class AssemblyStrategy {
 public:
  AssemblyStrategy();
  AssemblyStrategy(int NUM_q0, vector3 base2endEffectorPos, matrix33 base2endEffectorRot, vector3 ePh, double momentGainFac);
  virtual ~AssemblyStrategy();

  /************************************* Snap Assembly and Control Basis Members *************************************/

  /***************************************** Enumerations ********************************************************/
  // Control Paradigm
  enum ControlParadigm
  {
    motionData,				// Uses kinematics and way-points
    controlBasis			// Uses FT controllers.
  };

  // Controller Type Enumeration: What type of basis control are you working with?
  enum ControllerType
  {
    Null,
    PositionCtrl,
    ForceCtrl,
    MomentCtrl,
  };

  // Control Strategy: What kind of approach will you adopt to complete the snap assembly
  enum CtrlStrategy
  {
    ManipulationTest,
    StraightLineApproach,
    PivotApproach,
    SideApproach,
    FailureCharacerization,
    // ==== Diro ======
    TwoArm_HSA
    ,Left_Arm_Hold
  };

  enum TestAxis
  {
    posFx, negFx,
    posFy, negFy,
    posFz, negFz,
    posMx, negMx,
    posMy, negMy,
    posMz, negMz,
    all,none
  };

  // Different kind of control compositions
  enum ctrlComp
  {
    // Jacobian Position
    PositionComposition = 1,
    PositionOrientationComposition,
    PoseComposition,
    // Inv Kin
    IKinComposition,
    IKinForceComposition,
    IKinMomentComposition,
    // Force
    ForceComposition,
    ForceIKinComposition,
    ForceMomentComposition,
    FMPComposition,
    // Moment
    MomentComposition,
    MomentIKinComposition,
    MomentForceComposition,
    MomentForceIKinComposition,
    MFPComposition,
  };

  // Enumerate the six force-moment axes for easy indexing.
  enum forceAxes
  {
	  Fx, Fy, Fz, Mx, My, Mz,
  };

  /*-------------------------------------------------- STATES -----------------------------------------------------*/
  /*---------------------------------------- States: PA10 Pivot Approach -------------------------------------------*/
  enum PA10_PA_States
  {
	  paApproach= 1,
	  paRotation,
	  paAlignment,
	  paInsertion,
	  paMating,
	  paFinish,
  };
  /*---------------------------------------- States: HIRO Side Approach -------------------------------------------*/
  enum HIRO_SA_States
  {
	  hsaApproach= 1,
	  hsaRotation,
	  hsaInsertion,
	  hsaSubInsertion,
	  hsaMating,
	  hsaFinish,
  };

  //  ==== Diro ====
  /*---------------------------------------- States: TWOARM Side Approach -------------------------------------------*/
  enum TWOARM_HSA_States
  {
	  twoArm_hsaApproach= 1,
	  twoArm_hsaRotation,
	  twoArm_hsaInsertion,
	  twoArm_hsaSubInsertion,
	  twoArm_hsaMating,
	  twoArm_hsaFinish,
  };

  /*-------------------------------------------------- TRANSITIONS -----------------------------------------------------*/
  /*---------------------------------------- Transitions: PA10 Pivot Approach -------------------------------------------*/
  enum PA10_PA_Transitions
  {
	  paApproach2Rotation = 1,
	  paRotation2IAlignment,
	  paAlignment2Insertion,
	  paInsertion2Mating,
	  paMating2FinishTime,
  };
  /*---------------------------------------- Transitions: HIRO Side Approach -------------------------------------------*/
  enum HIRO_SA_Transitions
  {
	  hsaApproach2Rotation = 1,
	  hsaRotation2Insertion,
	  hsaInsertion2InsPartB,
	  hsaInsPartB2Mating,
	  hsaMating2FinishTime,
  };
  /*---------------------------------------- Transitions: TWOARM Side Approach -------------------------------------------*/
  enum TWOARM_SA_Transitions
  {
	  TWOARM_hsaApproach2Rotation = 1,
	  TWOARM_hsaRotation2Insertion,
	  TWOARM_hsaInsertion2InsPartB,
	  TWOARM_hsaInsPartB2Mating,
	  TWOARM_hsaMating2FinishTime,
  };
  /*---------------------------------------- Transitions: HIRO Side Approach Exceptions-------------------------------------------*/
  enum HIRO_SA_Trnasition_Exceptions
  {
	  normal=1,							// Increases state number and writes time entry to state vector file
	  Ins2InsSubPart,					// Increases state number but does not write entry into state vector file
	  DoNotIncreaseStateNum				// Does not increase state number but writes entry into state vector file
  };

  /*--------------------------------------------------- Strategy --------------------------------------------------------*/
  int		approachType;				// What kind of assembly strategy will we use?
  int 		controlType;				// What kind of control method will we use?
  bool 		approachFlag;				// Used to switch values
  bool 		ctrlInitFlag;				// Used to determine if its our first time in a state

  // Pivot Approach
  bool 		noContact;					// Flag become true when after a contact, the private member m_ContactPt return to the zero value.

  // Manipulation Test
  int 		testCounter;
  int 		compositionTypeTest;
  int 		DesForceSwitch;
  bool 		initialFlag;
  bool 		completionFlag;

  // Transition
  double  	signChanger;				// Changes the sign of data after a certain amount of time.
  bool 		switchFlag;					// Flag to be triggered to change signs
  bool 		nextState;					// Used to tell the state machine when to move to the next state
  int 		hsaHIROTransitionExepction;	// Flag to activate different kinds of transitions
  int 		State;						// What state are we in
  float 	transitionTime;				// Time between a two state stransition
  bool 		transitionTimebool;			// Flag for transition time
  double	state3_zPos;				// Used to identify the position in the z-direction of the wrist
  double	state3_zPrevPos;			// Keeps the previous record
  double 	SA_S4_Height;
  float		mating2EndTime;				// Hard-coded time interval used to indicate the end-time of the task. It is some x secs after mating has ocurred.

  // Control Basis Objects
  ControlBasis* c1;
  ControlBasis* c2;
  ControlBasis* c3;

  double momentGainFactor;

  // Controller
  int 	NumCtlrs;						// How many primitive controllers are being compounded in the CtrlBasis framework?
  bool 	ErrorFlag;						// If error is low, set to true.

  // Data vectors
  dvector6 DesIKin;						// Final Desired Position
  dvector6 CurCartPos;					// 6 element vector to hold xyz rpy
  dvector6 DesCartPos;
  dvector6 contactPos;					// Position versions for strategies that use inverse kinematics.
  dvector6 CurJointAngles;				// Save current joint angles
  dvector6 JointAngleUpdate;			// It's the output of the control basis computation (single or primitive controllers) used to update the joint angle position

  // Filter Objects
  FilterTools* ft;

  // Kinematics
  int IKinTestFlag;						// Used to indicate IKin to control basis
  int zerothJoint;						// Zero index
  vector3 transWrist2CamXEff;			// Translation from wrist to end-effector holding camera. TCP is on edge of camera
  vector3 ContactWristAngle;
  dmatrix Jacobian;

  // Motion
  vector3 			wrist_p, EndEff_p, EndEff_p_org;			// Original and updated position angles for the wrist
  vector3 			wrist_r, EndEff_r, EndEff_r_org;			// Original and updated rotational angles
  vector<double> 	ex_time, x_pos, y_pos, z_pos;				// Save robot position in vectors
  vector<double>	roll_angle, pitch_angle, yaw_angle;			// Endeffector orientation in terms of roll picth angle
  double angle_o[7]; 											// rhand_org, lhand_org, hando[2],

  // Failure Case Characterization
  dvector6 divPoint;											// This vector will hold x,y, and roll, yall values that will be added to the way-points in order to control which directions we want to modify to test failure case scenarios

  // Files
  ifstream ifstr_pivApproachState1;																			// Input Streams
  ofstream ostr_TrajState2, ostr_cartPos, ostr_angles, ostr_state, ostr_Forces, ostr_des, ostr_cur; 		// Output Streams
  char strTrajState1[STR_LEN], strTrajState2[STR_LEN], strCartPos[STR_LEN], strAngles[STR_LEN], strState[STR_LEN], strForces[STR_LEN];

  // Imported values
  double 		cur_time;												// Save current time

  // Filtering
  bool 			flagFiltering;
  dvector6 		avgSig;													// contains filtered/moving average result signal
  /**************************************************************************** Methods **********************************************************************************/

  int StateMachine(TestAxis 		axis,																		// Used exclusively for AssemblyStrategy::manipulatorTest
				   CtrlStrategy 	approach,																	// Straight Line Approach or Pivot Approach
				   JointPathPtr 	m_path,																		// Path structure. Use for right/left arm.
				   BodyPtr 			bodyPtr,																	// Body structure. Can access all 15 DOF
				   double 			cur_time,																	// Internal simulation time
				   vector3& 		pos,																		// TCP position
				   matrix33& 		rot,																		// TCP rotation
				   dvector6 		currForces,																	//	Forces in world coordinates
				   dvector6& 		JointAngleUpdate,
				   dvector6& 		CurrAngles,
				   dmatrix 			Jacobian,
				   dmatrix 			PseudoJacobian);

  int StateSwitcher(CtrlStrategy 	approach,
					int& 			State,
					double 			ErrorNorm1,
					double 			ErrorNorm2,
					vector3 		pos,
					matrix33 		rot,
					dvector6 		CurJointAngles,
					dvector6		currForces,
					double 			cur_time);																			// Switches the states of the state machine according to approach.

  void NextStateActions(double cur_time, int insertionStateSubFlag);												// Fixed number of operations to be done between switching of states

  // Call any combination of legal control compositions (upto three for now), and pass desired data.
  int ControlCompositions(JointPathPtr 	m_path,
							  BodyPtr 		bodyPtr,
							  dvector6& 	JointAngleUpdate,
							  dvector6& 	CurrAngles,
							  CtrlStrategy 	strat,
							  ctrlComp 		type,
							  vector3& 		DesData1,
							  vector3& 		DesData2,
							  dvector6& 	DesIKin, 															// Optional arguments for subordinate controllers if they exist.
							  double& 		ErrorNorm1,
							  double& 		ErrorNorm2,
							  vector3& 		pos,
							  matrix33& 	rot,
							  double 		cur_time,
							  dmatrix 		Jacobian,
							  dmatrix 		PseudoJacobian);

  // Motion Methods
  // moveRobot: take waypoints, convert them from end-effector coords to wrist coordinates and write the values to the private member vars. Later used by Inverse Kinematics functions to convert the wrist position into joint angle variables.
  bool moveRobot(double cur_time);

  // manipulationTest:
  // User can assign axis of motion in which one would like to test either the force controllers or the moment controllers
  int manipulationTest(TestAxis 		axis,
					   bool&			completionFlag,
					   JointPathPtr 	m_path,
					   BodyPtr 			bodyPtr,
					   dvector6& 		JointAngleUpdate,
					   dvector6& 		CurrAngles,
					   vector3& 		DesData1,
					   vector3& 		DesData2,
					   dvector6& 		DesIKin, 															// Optional arguments for subordinate controllers if they exist.
					   vector3 			pos,
					   matrix33 		rot,
					   double 			cur_time,
					   dmatrix 			Jacobian,
					   dmatrix 			PseudoJacobian);

  // Other
  int  Initialize(char TrajState1[STR_LEN], char TrajState2[STR_LEN], char Angles[STR_LEN], char Position[STR_LEN], char State[STR_LEN], char Forces[STR_LEN],
		  	  	  	vector3 pos, matrix33 rot, double CurAngles[15],
		            int strategyType, int controlMethodType);

  int  EndEff2WristTrans(/*in*/ Vector3 EndEff_p, /*in*/ Vector3 EndEff_r, /*out*/ Vector3& WristPos, /*out*/ Vector3& WristRot);
  int  wrist2EndEffTrans(/*in,out*/vector3& WristPos, /*in,out*/vector3& WristRot);
  void FreeResources(ctrlComp type);

  // Files
  void OpenFiles();
  void CloseFiles();
  int  WriteFiles(double cur_time, dvector6 CurrAngles, dvector6 JointAngleUpdate, vector3 CurrPos, vector3 CurrRPY, dvector6 CurrForces);
  int  ProcessTrajFile(char path[STR_LEN], int State, vector3 pos, vector3 rpy, double cur_time);
  bool checkArmLimit(JointPathPtr arm_path);
  bool IK_arm(JointPathPtr arm_path, Link *base, Link *waist, const vector3& p, const matrix33& R);
};

#endif /* ASSEMBLYSTRATEGY_H_ */