ARAP180.h

00001 /*This file is part of dvrk-dynamics package.
00002  * Copyright (C) 2017, Giuseppe Andrea Fontanelli
00003  
00004  * Email id : giuseppeandrea.fontanelli@unina.it
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *   * Redistributions of source code must retain the above copyright notice,
00008  *     this list of conditions and the following disclaimer.
00009  *   * Redistributions in binary form must reproduce the above copyright
00010  *     notice, this list of conditions and the following disclaimer in the
00011  *     documentation and/or other materials provided with the distribution.
00012  *   * Neither the names of Stanford University or Willow Garage, Inc. nor the names of its
00013  *     contributors may be used to endorse or promote products derived from
00014  *     this software without specific prior written permission.
00015  *
00016  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00017  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00018  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00019  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00020  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00021  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00022  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00023  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00024  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00025  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00026  * POSSIBILITY OF SUCH DAMAGE.
00027 * This code will subscriber integer values from demo_topic_publisher
00028 */
00029 
00030 #ifndef _ARAP180_H
00031 #define _ARAP180_H
00032 #include "mbed.h"
00033 #include "Core.h"
00034 #include <Eigen/Dense.h>
00035 #include "utilities.hpp"
00036 #include "UARTSerial_half.h"
00037 #include "MX.h"
00038 
00039 using namespace Eigen;
00040 using namespace std;
00041 
00042 typedef Matrix<double, 6, 1> Vector6d;
00043 typedef Matrix<double, 7, 1> Vector7d;
00044 typedef Matrix<double, 6, 6> Matrix6d;
00045 typedef Matrix<float, 6, 1> Vector6f;
00046 typedef Matrix<float, 7, 1> Vector7f;
00047 typedef Matrix<float, 6, 6> Matrix6f;
00048 
00049 class ARAP180
00050 {
00051     private:
00052 
00053         string robName;
00054         string paramFile;
00055         
00056          
00057         bool read_parameters_from_file(string paramFile);
00058         Vector6f q_sat_max; 
00059         Vector6f q_sat_min; 
00060         
00061         
00062         UARTSerial_half *dxl_port; 
00063         
00064         int ID[5];
00065         int operatingMode[5];
00066         bool enableVal[5];
00067         MX *mx_MotorChain;  
00068         
00069         Vector6f offsets;
00070         
00071         Vector6f motorSign;
00072 
00073 
00074 
00075 
00076     public:
00077 
00078         /* brief Constructor. */
00079         ARAP180();
00080         VectorXf get_parameters();
00081         
00082         Matrix6f jacobianMatrix(Vector6f q);    //Jacobian matrix
00083         Matrix4f forwardKinematics(Vector6f q); //Dyrect kinematics
00084         Vector6f backwardKinematics(Matrix4f T_d, Vector6f q, float _Tsam, int _n_max_iter, float _max_pError, float _max_oError ,float _Kp, float _Ko);    //Dyrect kinematics
00085         
00086         Vector6f jointSaturation(Vector6f q);
00087 
00088         Matrix6f inertiaMatrix(Vector6f q); //Inertia matrix
00089         Matrix6f coriolisCentrifugalMatrix(Vector6f q, Vector6f dq); //Coriolis and centrifugal matrix
00090     
00091         Vector6f gravityVector(Vector6f q); //Gravity vector
00092         Vector6f frictionVector(Vector6f dq);   //Friction vector
00093         Vector6f stiffnessVector(Vector6f q);   //Friction vector
00094         
00095         void initArmMotors();
00096         Vector6f getJointPos();
00097         void setJointPos(Vector6f q);
00098         
00099                 
00100         void setMaxVel(float maxVel);
00101         void setMaxAcc(float maxAcc);
00102         
00103         
00104 
00105 
00106 
00107 };
00108 
00109 #endif // _ARMHyfliers_H
00110