mirror of
https://github.com/nasa/trick.git
synced 2024-12-21 06:03:10 +00:00
fd8252e2c5
* First commit of SIM_robot. * Updates to kinematic arm with controller * Working well enough, still needs some user interface besides trick-TV * Add end-effector path trace to graphics client. * Singularity control bug, remove printouts, udpate makefile * Improve SIM_robot variable server client. * Tidy up RobotDisplay.java * Tidying up * Removing warnings * Working on documentation * Updating documentation * Updating docs * Adding figures for documentation * Removing some stuff in the README carried over from the template * Tidying up * Position vector finally done * Updating based on feedback * Forward position kinematics completed with notation changes * First pass documentation done? * remove printout * Fix typos in text and filenames * Update README.md * Update README.md * made it smaller --------- Co-authored-by: John M. Penn <john.m.penn@nasa.gov>
66 lines
2.3 KiB
C++
66 lines
2.3 KiB
C++
#ifndef __MANIPCONTROL_HH_
|
|
#define __MANIPCONTROL_HH_
|
|
/**************************************************************************
|
|
PURPOSE: (2D Manipulator class definitions including kinematics and control)
|
|
***************************************************************************/
|
|
|
|
#include "kinematics/ManipKinemat.hh"
|
|
#include "utils/utils.hh"
|
|
|
|
|
|
#include <cmath>
|
|
#include <vector>
|
|
|
|
enum ControlFrame
|
|
{
|
|
Task, /* Manual control velocities in the Task Frame */
|
|
EE /* Manual control velocities in the EE's own frame */
|
|
};
|
|
|
|
|
|
class ManipControl
|
|
{
|
|
public:
|
|
|
|
ControlFrame manualFrame; /* -- Frame for Manual Control */
|
|
|
|
/* Single Joint Control Mode data */
|
|
int singleJointNum; /* -- SingleJoint control joint selection */
|
|
double singleJointRate; /* rad/s SingleJoint control joint rate for selected joint */
|
|
|
|
/* Manual Control Mode data */
|
|
double manualCommandRate[2]; /* m/s command rate of EE in manual mode */
|
|
|
|
/* EEPos Control Mode data */
|
|
double desiredPos[2]; /* m Desired EE position in TaskFrame of EE for EEPos control mode */
|
|
double Kp; /* -- Proportional gain for EEPos tracking */
|
|
double Kd; /* -- Derivative gain for EEPos tracking */
|
|
|
|
ManipUtils utils; /* -- math utilities */
|
|
|
|
double commandedJointRate[2]; /* rad/s calculated required joint rates */
|
|
|
|
/* Information required from kinematics */
|
|
double **jacInv; /* -- inverse Jacobian as calculated by kinematics */
|
|
double **R_ee_task; /* -- rot matrix from EE to task frame */
|
|
|
|
void manualControl();
|
|
void singleJointControl();
|
|
bool EEPositionAuto(double *pos, double *vel);
|
|
void clearControlCommands();
|
|
|
|
ManipControl(int numdof);
|
|
|
|
private:
|
|
|
|
int ndof; /* -- number of degrees of freedom */
|
|
|
|
double commandedEERate_Task[2]; /* m/s command rate of EE in Task frame */
|
|
|
|
double posEps; /* m error tolerance for EEPosAuto convergence */
|
|
|
|
void calcResolvedJointRates(); /* -- calculate resolved joint rates for EE motion */
|
|
};
|
|
|
|
#endif
|