/*!
 * @file fk.h
 * @brief forward kinema for pa10
 *
 * @date 2003.11.3
 *
 * Copyright 2003 Takashi Suehiro
 */
// #include <stdio.h>
#include <geo_std.h>		//mod.start 2010.1.15 by FSI
#define LB 250		//#define LB 315.0
#define LS 250		//#define LS 450.0
#define LE 200		//#define LE 500.0
#define LW 55			//#define LW 80.0
typedef REAL Jacob[6][6];	//typedef REAL Jacob[6][7];
				//mod.start 2010.1.15 by FSI
/*!
 * @class FK
 * @brief class for forward kinema for refhard1
 *
 */
class FK
{
 public:
  //! 
  FK();
  ~FK();
  //! 
  int set_tool(FRAME * frm);
  //! 
  int calc_fk(REAL * th);
  //! 
  int link_frame(int idx, FRAME * frm);
  //! 
  int jacob(Jacob jcb);
  //! 
 private:
  //! 				//mod.start 2010.1.15 by FSI
  MATRIX link_mat[7];		//MATRIX link_mat[8];
  VECTOR link_pos[7];		//VECTOR link_pos[8];
  VECTOR joint_vec[6];		//VECTOR joint_vec[7];
  VECTOR joint_arm[6];		//VECTOR joint_arm[7];
  //!				//mod.end 2010.1.15 by FSI
  VECTOR ua,fa,fr,ef;
  VECTOR dir_vtr; // direction vector to make "ua"

  //! 				//mod.start 2010.1.15 by FSI
  MATRIX a_mat[7];		//MATRIX a_mat[8];
  //! 				//mod.end 2010.1.15 by FSI
  VECTOR v_tool;
};

