Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[2111] pkg/trunk
|
From: <hsu...@us...> - 2008-07-25 01:25:41
|
Revision: 2111
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2111&view=rev
Author: hsujohnhsu
Date: 2008-07-25 01:25:49 +0000 (Fri, 25 Jul 2008)
Log Message:
-----------
updates to mechanism control object, with some patches to armcontroller to make it compile.
Modified Paths:
--------------
pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/ArmController.h
pkg/trunk/controllers/pr2Controllers/src/ArmController.cpp
pkg/trunk/drivers/simulator/gazebo_hardware/include/gazebo_hardware/gazebo_hardware.h
pkg/trunk/drivers/simulator/gazebo_hardware/src/gazebo_hardware.cpp
pkg/trunk/mechanism/mechanism_control/src/gazebo_control.cpp
Modified: pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/ArmController.h
===================================================================
--- pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/ArmController.h 2008-07-25 01:24:12 UTC (rev 2110)
+++ pkg/trunk/controllers/pr2Controllers/include/pr2Controllers/ArmController.h 2008-07-25 01:25:49 UTC (rev 2111)
@@ -16,11 +16,23 @@
#include <iostream>
+// to be replaced by xml
#include <pr2Core/pr2Core.h>
+
+// generic controllers
#include <genericControllers/Controller.h>
#include <genericControllers/JointController.h>
+
+// robot model
#include <mechanism_model/joint.h>
+#include <mechanism_model/robot.h>
+
+// kinematics library
#include <libKDL/kdl_kinematics.h>
+
+// for temporary gettimeofday call, will be passed in from hardware interface and deprecate
+#include <sys/time.h>
+
namespace controller
{
class ArmController : Controller
@@ -46,13 +58,21 @@
* \param
*/
ArmController();
-
+ ArmController(Robot *robot, std::string name);
+ ArmController(Robot *robot);
+ ArmController(Robot *robot, std::string name,int armNumJoints, int jcToRobotJointMap[], int jcToHIActuatorMap[]);
+
/*!
* \brief Destructor.
*/
~ArmController( );
/*!
+ * \brief load XML file
+ */
+ controllerErrorCode loadXML(std::string filename);
+
+ /*!
* \brief initialize controller variables
*/
void init();
@@ -64,6 +84,13 @@
*/
void initJoint(int jointNum, double pGain, double iGain, double dGain, double iMax, double iMin, controllerControlMode mode, double time,double maxEffort,double minEffort, mechanism::Joint *joint);
+ //TEMPORARY
+ /*!
+ * \brief get system time, want to get simulator time for simulation.
+ *
+ */
+ double getTime();
+
//TEMPORARY
/*!
* \brief Call after initializing individual joints to initialize arm as a whole
@@ -352,7 +379,7 @@
/*!
* \brief Update controller
*/
- void update( );
+ void update();
//---------------------------------------------------------------------------------//
// MISC CALLS
@@ -366,15 +393,60 @@
bool enabled; /**<Is the arm controller active?>*/
controllerControlMode controlMode; /**< Arm controller control mode >*/
- std::string name; /**<Namespace identifier for ROS>*/
+ /*!
+ * \brief initialize joint controllers
+ */
+ void initJointControllers();
+ JointController *armJointControllers; /**< Lower level control done by JointControllers>*/
- JointController lowerControl[ARM_MAX_JOINTS]; /**< Lower level control done by JointControllers>*/
-
double cmdPos[6]; /**<Last commanded cartesian position>*/
double cmdVel[6]; /**<Last commanded cartesian velocity>*/
PR2_kinematics pr2_kin; /**<PR2 kinematics>*/
+ double pGain; /**< Proportional gain for position control */
+
+ double iGain; /**< Integral gain for position control */
+
+ double dGain; /**< Derivative gain for position control */
+
+ double iMax; /**< Max integral error term */
+
+ double iMin; /**< Min integral error term */
+
+ double maxEffort; /**< maximum effort */
+
+ double minEffort; /**< maximum effort */
+
+ double xDotCmd; /**< Forward speed cmd */
+
+ double yDotCmd; /**< Sideways speed cmd (motion to the left is positive) */
+
+ double yawDotCmd; /**< Rotational speed cmd (motion counter-clockwise is positive) */
+
+ double xDotNew; /**< New forward speed cmd */
+
+ double yDotNew; /**< New sideways speed cmd (motion to the left is positive) */
+
+ double yawDotNew; /**< New rotational speed cmd (motion counter-clockwise is positive) */
+
+ double maxXDot;
+
+ double maxYDot;
+
+ double maxYawDot;
+
+ Robot *robot;
+
+ std::map<std::string,std::string> paramMap;
+
+ std::string name; /**<Namespace identifier for ROS>*/
+
+ void loadParam(std::string label, double &value);
+
+ void loadParam(std::string label, int &value);
+
+
};
}
Modified: pkg/trunk/controllers/pr2Controllers/src/ArmController.cpp
===================================================================
--- pkg/trunk/controllers/pr2Controllers/src/ArmController.cpp 2008-07-25 01:24:12 UTC (rev 2110)
+++ pkg/trunk/controllers/pr2Controllers/src/ArmController.cpp 2008-07-25 01:25:49 UTC (rev 2111)
@@ -1,4 +1,5 @@
#include <pr2Controllers/ArmController.h>
+#define NDOF 6 // accounts for x,y,z,roll,pitch,yaw
using namespace controller;
/***************************************************/
/*! \class controller::ArmController
@@ -40,31 +41,157 @@
ArmController::ArmController()
{
- //Default construction of JointControllers should init them to blank controllers
+ this->robot = NULL;
+ this->name = "armController";
}
-
-ArmController::~ArmController( )
+ArmController::ArmController(Robot *robot)
{
+ this->robot = robot;
+ this->name = "armController";
}
+ArmController::ArmController(Robot *robot, std::string name)
+{
+ this->robot = robot;
+ this->name = name;
+}
+
+ArmController::ArmController(Robot *robot, std::string name,int armNumJoints, int jcToRobotJointMap[], int jcToHIActuatorMap[])
+{
+ this->robot = robot;
+ this->name = "armController";
+}
+
+ArmController::~ArmController()
+{
+}
+
+
+controllerErrorCode ArmController::loadXML(std::string filename)
+{
+ robot_desc::URDF model;
+ int exists = 0;
+
+ if(!model.loadFile(filename.c_str()))
+ return CONTROLLER_MODE_ERROR;
+
+ const robot_desc::URDF::Data &data = model.getData();
+
+ std::vector<std::string> types;
+ std::vector<std::string> names;
+ std::vector<std::string>::iterator iter;
+
+ data.getDataTagTypes(types);
+
+ for(iter = types.begin(); iter != types.end(); iter++){
+ if(*iter == "controller"){
+ exists = 1;
+ break;
+ }
+ }
+
+ if(!exists)
+ return CONTROLLER_MODE_ERROR;
+
+ exists = 0;
+
+ for(iter = names.begin(); iter != names.end(); iter++){
+ if(*iter == this->name){
+ exists = 1;
+ break;
+ }
+ }
+ paramMap = data.getDataTagValues("controller",this->name);
+
+ loadParam("pGain",pGain);
+ printf("BC:: %f\n",pGain);
+ loadParam("dGain",dGain);
+ loadParam("iGain",iGain);
+
+ loadParam("maxXDot",maxXDot);
+ loadParam("maxYDot",maxYDot);
+ loadParam("maxYawDot",maxYawDot);
+
+ return CONTROLLER_ALL_OK;
+}
+
+
void ArmController::init()
{
// to be filled in here
- // initJointControllers();
+ initJointControllers();
}
+void ArmController::initJointControllers()
+{
+ this->armJointControllers = new JointController[ARM_MAX_JOINTS];
+ /**************************************************************************************/
+ /* */
+ /* MAPPING BETWEEN: armController->jointController array and robot->joint array */
+ /* */
+ /* note: this is how the arm's joint controller arrays knows when robot->joint */
+ /* to use */
+ /* */
+ /**************************************************************************************/
+
+ for (int ii=0; ii < ARM_MAX_JOINTS; ii++) {
+ // FIXME: hardcoded values for gazebo for now
+ //armJointControllers[ii].init(pGain, iGain, dGain, iMax, iMin, ETHERDRIVE_SPEED, getTime(), maxEffort, minEffort, &(robot->joint[ii]));
+ armJointControllers[ii].init( 1000, 0, 0, 500, -500, controller::CONTROLLER_POSITION, getTime(), 1000, -1000, &(robot->joint[ii]));
+ armJointControllers[ii].enableController();
+ }
+ /***************************************************************************************/
+ /* */
+ /* initialize controllers */
+ /* hardcoded values, should be replaced by reading controllers.xml file */
+ /* TODO: assigned by xml fils and param servers */
+ /* */
+ /***************************************************************************************/
+#if 0
+ // initialize each jointController jc[i], associate with a joint joint[i]
+ for (int i = 0;i<PR2::MAX_JOINTS;i++){
+ jc[i] = new controller::JointController();
+ jc[i]->init(joint[i]);
+ }
+
+ //Explicitly initialize the controllers we wish to use. Don't forget to set the controllers to torque mode in the world file!
+ armJointController[PR2::ARM_L_PAN]->initJoint(100,0,0,500,-500, controller::CONTROLLER_POSITION,time,-100,100,joint[PR2::ARM_L_PAN]);
+ armJointController[PR2::ARM_L_SHOULDER_PITCH]->init(1000,0,0,500,-500, controller::CONTROLLER_POSITION,time,-1000,1000,joint[PR2::ARM_L_SHOULDER_PITCH]);
+ armJointController[PR2::ARM_L_SHOULDER_ROLL]->init(100,0,0,500,-500, controller::CONTROLLER_POSITION,time,-100,100,joint[PR2::ARM_L_SHOULDER_ROLL]);
+ armJointController[PR2::ARM_L_ELBOW_PITCH]->init(300,0,0,500,-500, controller::CONTROLLER_POSITION,time,-100,100,joint[PR2::ARM_L_ELBOW_PITCH]);
+ armJointController[PR2::ARM_L_ELBOW_ROLL]->init(100,0,0,500,-500, controller::CONTROLLER_POSITION,time,-100,100,joint[PR2::ARM_L_ELBOW_ROLL]);
+ armJointController[PR2::ARM_L_WRIST_PITCH]->init(100,0,0,500,-500, controller::CONTROLLER_POSITION,time,-100,100,joint[PR2::ARM_L_WRIST_PITCH]);
+ armJointController[PR2::ARM_L_WRIST_ROLL]->init(100,0,0,500,-500, controller::CONTROLLER_POSITION,time,-100,100,joint[PR2::ARM_L_WRIST_ROLL]);
+
+ armJointController[PR2::ARM_R_PAN]->init(100,0,0,500,-500, controller::CONTROLLER_POSITION,time,-100,100,joint[PR2::ARM_R_PAN]);
+ armJointController[PR2::ARM_R_SHOULDER_PITCH]->init(1000,0,0,500,-500, controller::CONTROLLER_POSITION,time,-1000,1000,joint[PR2::ARM_R_SHOULDER_PITCH]);
+ armJointController[PR2::ARM_R_SHOULDER_ROLL]->init(100,0,0,500,-500, controller::CONTROLLER_POSITION,time,-100,100,joint[PR2::ARM_R_SHOULDER_ROLL]);
+ armJointController[PR2::ARM_R_ELBOW_PITCH]->init(300,0,0,500,-500, controller::CONTROLLER_POSITION,time,-100,100,joint[PR2::ARM_R_ELBOW_PITCH]);
+ armJointController[PR2::ARM_R_ELBOW_ROLL]->init(100,0,0,500,-500, controller::CONTROLLER_POSITION,time,-100,100,joint[PR2::ARM_R_ELBOW_ROLL]);
+ armJointController[PR2::ARM_R_WRIST_PITCH]->init(100,0,0,500,-500, controller::CONTROLLER_POSITION,time,-100,100,joint[PR2::ARM_R_WRIST_PITCH]);
+ armJointController[PR2::ARM_R_WRIST_ROLL]->init(100,0,0,500,-500, controller::CONTROLLER_POSITION,time,-100,100,joint[PR2::ARM_R_WRIST_ROLL]);
+#endif
+}
+
+double ArmController::getTime()
+{
+ struct timeval t;
+ gettimeofday( &t, 0);
+ return (double) (t.tv_usec *1e-6 + t.tv_sec);
+}
+
void ArmController::initJoint(int jointNum, double PGain, double IGain, double DGain, double IMax, double IMin, controllerControlMode mode, double time, double maxEffort,double minEffort, mechanism::Joint *joint)
{
- lowerControl[jointNum].init(PGain, IGain, DGain, IMax, IMin, CONTROLLER_DISABLED, time, maxEffort, minEffort, joint); //Initialize joint, but keep in disabled state
+ armJointControllers[jointNum].init(PGain, IGain, DGain, IMax, IMin, CONTROLLER_DISABLED, time, maxEffort, minEffort, joint); //Initialize joint, but keep in disabled state
}
controllerErrorCode ArmController::initArm(controllerControlMode mode)
{
//Reset commanded positions
- for(int i =0 ;i<6;i++){
+ for(int i =0 ;i< NDOF ;i++){
cmdPos[i] = 0.0;
cmdVel[i] = 0.0;
}
@@ -72,7 +199,7 @@
//Set mode
controlMode = mode;
for(int i =0;i<ARM_MAX_JOINTS;i++){
- lowerControl[i].setMode(mode); //Set underlying jointControllers to position control
+ armJointControllers[i].setMode(mode); //Set underlying jointControllers to position control
}
//Turn on the controller
@@ -87,7 +214,7 @@
controllerControlMode ArmController::setMode(controllerControlMode mode)
{
for(int i = 0;i<ARM_MAX_JOINTS;i++){
- lowerControl[i].setMode(mode);
+ armJointControllers[i].setMode(mode);
}
return CONTROLLER_MODE_SET;
}
@@ -96,7 +223,7 @@
{
enabled = true;
for(int i = 0;i<ARM_MAX_JOINTS;i++){
- lowerControl[i].enableController();
+ armJointControllers[i].enableController();
}
return CONTROLLER_ENABLED;
}
@@ -105,7 +232,7 @@
{
enabled = false;
for(int i = 0;i<ARM_MAX_JOINTS;i++){
- lowerControl[i].disableController();
+ armJointControllers[i].disableController();
}
return CONTROLLER_DISABLED;
@@ -120,7 +247,7 @@
void ArmController::checkForSaturation(bool* status[])
{
for(int i= 0;i<ARM_MAX_JOINTS;i++){
- *status[i] = lowerControl[i].checkForSaturation();
+ *status[i] = armJointControllers[i].checkForSaturation();
}
}
@@ -159,16 +286,16 @@
}
//Perform inverse kinematics
- if (pr2_kin.IK(q_init, f, q_out)){
+ if (this->pr2_kin.IK(q_init, f, q_out)){
// cout<<"IK result:"<<q_out<<endl;
- }else{
+ }else{
//cout<<"Could not compute Inv Kin."<<endl;
return CONTROLLER_JOINT_ERROR;
}
//------ checking that IK returned a valid soln -----
KDL::Frame f_ik;
- if (pr2_kin.FK(q_out,f_ik))
+ if (this->pr2_kin.FK(q_out,f_ik))
{
// cout<<"End effector after IK:"<<f_ik<<endl;
}
@@ -179,7 +306,7 @@
//Record commands and shove to arm
for(int ii = 0; ii < ARM_MAX_JOINTS; ii++){
- lowerControl[ii].setPosCmd(q_out(ii));
+ armJointControllers[ii].setPosCmd(q_out(ii));
//std::cout<<"*"<<q_out(ii);
}
//std::cout<<std::endl;
@@ -187,12 +314,12 @@
}
controllerErrorCode ArmController::getHandCartesianPosCmd(double *x, double *y, double *z, double *roll, double *pitch, double *yaw)
{
- *x = cmdPos[0];
- *y = cmdPos[1];
- *z = cmdPos[2];
- *roll = cmdPos[3];
+ *x = cmdPos[0];
+ *y = cmdPos[1];
+ *z = cmdPos[2];
+ *roll = cmdPos[3];
*pitch = cmdPos[4];
- *yaw = cmdPos[5];
+ *yaw = cmdPos[5];
return CONTROLLER_ALL_OK;
}
controllerErrorCode ArmController::getHandCartesianPosAct(double *x, double *y, double *z, double *roll, double *pitch, double *yaw)
@@ -201,13 +328,13 @@
KDL::JntArray q_jts(ARM_MAX_JOINTS);
//Read current joint locations
for(int i = 0;i<ARM_MAX_JOINTS;i++){
- lowerControl[i].getPosAct(&pos);
+ armJointControllers[i].getPosAct(&pos);
q_jts(i) = pos;
}
//Perform forward kinematics to get cartesian location
KDL::Frame fk;
- if (pr2_kin.FK(q_jts,fk))
+ if (this->pr2_kin.FK(q_jts,fk))
{
// cout<<"End effector:"<<fk<<endl;
}
@@ -234,9 +361,9 @@
controllerErrorCode ArmController::getHandOrientationCmd(double *roll, double *pitch, double *yaw)
{
- *roll = cmdPos[3];
+ *roll = cmdPos[3];
*pitch = cmdPos[4];
- *yaw = cmdPos[5];
+ *yaw = cmdPos[5];
return CONTROLLER_ALL_OK;
}
@@ -280,7 +407,7 @@
controllerErrorCode current;
if(controlMode!= CONTROLLER_POSITION) return CONTROLLER_MODE_ERROR; //TODO implement errors?
for (int i = 0;i<getNumJoints();i++){
- current = lowerControl[i].setPosCmd(angles[i]);
+ current = armJointControllers[i].setPosCmd(angles[i]);
// std::cout<<i<<":"<<current<<std::endl;
if(current!=CONTROLLER_ALL_OK) error = current;
}
@@ -292,7 +419,7 @@
controllerErrorCode current;
for (int i = 0;i<getNumJoints();i++){
- current = lowerControl[i].getPosCmd(angles[i]); //If we find even a single error,return error for overall
+ current = armJointControllers[i].getPosCmd(angles[i]); //If we find even a single error,return error for overall
if(current!=CONTROLLER_ALL_OK) error = current;
}
return error;
@@ -304,7 +431,7 @@
controllerErrorCode current;
for (int i = 0;i<getNumJoints();i++){
- current = lowerControl[i].getPosAct(angles[i]); //If we find even a single error,return error for overall
+ current = armJointControllers[i].getPosAct(angles[i]); //If we find even a single error,return error for overall
if(current!=CONTROLLER_ALL_OK) error = current;
}
@@ -314,19 +441,19 @@
controllerErrorCode ArmController::setOneArmJointPos(int numJoint, double angle)
{
if(numJoint<0 || numJoint > ARM_MAX_JOINTS) return CONTROLLER_JOINT_ERROR; //Index out of bounds
- return lowerControl[numJoint].setPosCmd(angle);
+ return armJointControllers[numJoint].setPosCmd(angle);
}
controllerErrorCode ArmController::getOneArmJointPosCmd(int numJoint, double *angle)
{
if(numJoint<0 || numJoint > ARM_MAX_JOINTS) return CONTROLLER_JOINT_ERROR; //Index out of bounds
- return lowerControl[numJoint].getPosCmd(angle);
+ return armJointControllers[numJoint].getPosCmd(angle);
}
controllerErrorCode ArmController::getOneArmJointPosAct(int numJoint, double *angle)
{
if(numJoint<0 || numJoint > ARM_MAX_JOINTS) return CONTROLLER_JOINT_ERROR; //Index out of bounds
- return lowerControl[numJoint].getPosAct(angle);
+ return armJointControllers[numJoint].getPosAct(angle);
}
//---------------------------------------------------------------------------------//
@@ -339,7 +466,7 @@
controllerErrorCode current;
if(controlMode!= CONTROLLER_TORQUE) return CONTROLLER_MODE_ERROR; //TODO implement errors?
for (int i = 0;i<getNumJoints();i++){
- current = lowerControl[i].setTorqueCmd(torque[i]); //If we find even a single error, complete the set position and return error for overall
+ current = armJointControllers[i].setTorqueCmd(torque[i]); //If we find even a single error, complete the set position and return error for overall
if(current!=CONTROLLER_ALL_OK) error = current;
}
@@ -352,7 +479,7 @@
controllerErrorCode current;
for (int i = 0;i<getNumJoints();i++){
- current = lowerControl[i].getTorqueCmd(torque[i]); //If we find even a single error,return error for overall
+ current = armJointControllers[i].getTorqueCmd(torque[i]); //If we find even a single error,return error for overall
if(current!=CONTROLLER_ALL_OK) error = current;
}
@@ -365,7 +492,7 @@
controllerErrorCode current;
for (int i = 0;i<getNumJoints();i++){
- current = lowerControl[i].getTorqueAct(torque[i]); //If we find even a single error,return error for overall
+ current = armJointControllers[i].getTorqueAct(torque[i]); //If we find even a single error,return error for overall
if(current!=CONTROLLER_ALL_OK) error = current;
}
return error;
@@ -375,21 +502,21 @@
controllerErrorCode ArmController::setOneArmJointTorque(int numJoint,double torque)
{
if(numJoint<0 || numJoint > ARM_MAX_JOINTS) return CONTROLLER_JOINT_ERROR; //Index out of bounds
- return lowerControl[numJoint].setTorqueCmd(torque);
+ return armJointControllers[numJoint].setTorqueCmd(torque);
}
controllerErrorCode ArmController::getArmJointTorqueCmd(int numJoint, double *torque)
{
if(numJoint<0 || numJoint > ARM_MAX_JOINTS) return CONTROLLER_JOINT_ERROR; //Index out of bounds
- return lowerControl[numJoint].getTorqueCmd(torque);
+ return armJointControllers[numJoint].getTorqueCmd(torque);
}
controllerErrorCode ArmController::getArmJointTorqueAct(int numJoint, double *torque)
{
if(numJoint<0 || numJoint > ARM_MAX_JOINTS) return CONTROLLER_JOINT_ERROR; //Index out of bounds
- return lowerControl[numJoint].getTorqueAct(torque);
+ return armJointControllers[numJoint].getTorqueAct(torque);
}
@@ -404,7 +531,7 @@
controllerErrorCode current;
if(controlMode!= CONTROLLER_VELOCITY) return CONTROLLER_MODE_ERROR; //TODO implement errors?
for (int i = 0;i<getNumJoints();i++){
- current = lowerControl[i].setVelCmd(speed[i]); //If we find even a single error, complete the set position and return error for overall
+ current = armJointControllers[i].setVelCmd(speed[i]); //If we find even a single error, complete the set position and return error for overall
if(current!=CONTROLLER_ALL_OK) error = current;
}
return error;
@@ -416,7 +543,7 @@
controllerErrorCode current;
for (int i = 0;i<getNumJoints();i++){
- current = lowerControl[i].getVelCmd(speed[i]); //If we find even a single error,return error for overall
+ current = armJointControllers[i].getVelCmd(speed[i]); //If we find even a single error,return error for overall
if(current!=CONTROLLER_ALL_OK) error = current;
}
return error;
@@ -428,7 +555,7 @@
controllerErrorCode current;
for (int i = 0;i<getNumJoints();i++){
- current = lowerControl[i].getVelAct(speed[i]); //If we find even a single error,return error for overall
+ current = armJointControllers[i].getVelAct(speed[i]); //If we find even a single error,return error for overall
if(current!=CONTROLLER_ALL_OK) error = current;
}
return error;
@@ -437,18 +564,18 @@
controllerErrorCode ArmController::setOneArmJointSpeed(int numJoint, double speed){
if(numJoint<0 || numJoint > ARM_MAX_JOINTS) return CONTROLLER_JOINT_ERROR; //Index out of bounds
- return lowerControl[numJoint].setVelCmd(speed);
+ return armJointControllers[numJoint].setVelCmd(speed);
}
controllerErrorCode ArmController::getOneArmJointSpeedCmd(int numJoint, double *speed)
{
if(numJoint<0 || numJoint > ARM_MAX_JOINTS) return CONTROLLER_JOINT_ERROR; //Index out of bounds
- return lowerControl[numJoint].getVelCmd(speed);
+ return armJointControllers[numJoint].getVelCmd(speed);
}
controllerErrorCode ArmController::getOneArmJointSpeedAct(int numJoint, double *speed)
{
if(numJoint<0 || numJoint > ARM_MAX_JOINTS) return CONTROLLER_JOINT_ERROR; //Index out of bounds
- return lowerControl[numJoint].getVelAct(speed);
+ return armJointControllers[numJoint].getVelAct(speed);
}
@@ -481,21 +608,30 @@
{
if(!enabled){
for(int i = 0;i<ARM_MAX_JOINTS;i++){
- lowerControl[i].disableController();
+ armJointControllers[i].disableController();
}
} //Make sure controller is enabled. Otherwise, tell all jointcontrollers to shut down
for(int i = 0;i<ARM_MAX_JOINTS;i++){
- lowerControl[i].update();
+ armJointControllers[i].update();
}
+}
+void ArmController::loadParam(std::string label, double ¶m)
+{
+ if(paramMap.find(label) != paramMap.end()) // if parameter value has been initialized in the xml file, set internal parameter value
+ param = atof(paramMap[label].c_str());
}
+void ArmController::loadParam(std::string label, int ¶m)
+{
+ if(paramMap.find(label) != paramMap.end())
+ param = atoi(paramMap[label].c_str());
+}
-//---------------------------------------------------------------------------------//
-// MISC CALLS
-//---------------------------------------------------------------------------------//
-int ArmController::getNumJoints(){
- return ARM_MAX_JOINTS;
+int ArmController::getNumJoints()
+{
+ return ARM_MAX_JOINTS;
}
+
Modified: pkg/trunk/drivers/simulator/gazebo_hardware/include/gazebo_hardware/gazebo_hardware.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_hardware/include/gazebo_hardware/gazebo_hardware.h 2008-07-25 01:24:12 UTC (rev 2110)
+++ pkg/trunk/drivers/simulator/gazebo_hardware/include/gazebo_hardware/gazebo_hardware.h 2008-07-25 01:25:49 UTC (rev 2111)
@@ -79,11 +79,6 @@
void updateState();
/*!
- * \brief tick send most recent motor commands and retrieve updates. This command must be run at a sufficient rate or else the motors will be disabled.
- */
- void tick();
-
- /*!
* \brief Read the command values from the hardware interface and send them out to the actual motors
\param HardwareInterface* hw pointer to the hardware interface
*/
Modified: pkg/trunk/drivers/simulator/gazebo_hardware/src/gazebo_hardware.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_hardware/src/gazebo_hardware.cpp 2008-07-25 01:24:12 UTC (rev 2110)
+++ pkg/trunk/drivers/simulator/gazebo_hardware/src/gazebo_hardware.cpp 2008-07-25 01:25:49 UTC (rev 2111)
@@ -175,6 +175,10 @@
hardwareInterface->actuator[jointId[ii]].state.encoderCount = GAZEBO_POS_TO_ENCODER*( pr2PTZCameraRightIface->data->tilt );
pr2PTZCameraRightIface->Unlock();
}
+ else
+ {
+ //FIXME: default action??
+ }
//fprintf(stderr,"edh:: %d\n",hardwareInterface->actuator[jointId[ii]].state.encoderCount);
}
@@ -335,40 +339,6 @@
}
}
-void GazeboHardware::tick() {
- for(int ii = 0; ii < numActuators; ii++)
- {
- if (boardLookUp[ii] == 0)
- {
- //pr2ActarrayIface->Lock(1);
- //pr2ActarrayIface->data->actuators[portLookUp[ii]].cmdEnableMotor = motorsOn;
- //pr2ActarrayIface->Unlock();
- }
- else if (boardLookUp[ii] == 1)
- {
- //pr2GripperLeftIface->Lock(1);
- //pr2GripperLeftIface->data->cmdEnableMotor = motorsOn;
- //pr2GripperLeftIface->Unlock();
- }
- else if (boardLookUp[ii] == 2)
- {
- //pr2GripperRightIface->Lock(1);
- //pr2GripperRightIface->data->cmdEnableMotor = motorsOn;
- //pr2GripperRightIface->Unlock();
- }
- else if (boardLookUp[ii] == 3)
- {
- }
- else if (boardLookUp[ii] == 4)
- {
- }
- else
- {
- //FIXME: default action??
- }
- }
-}
-
GazeboHardware::~GazeboHardware()
{
// printf("Switching off motors \n");
Modified: pkg/trunk/mechanism/mechanism_control/src/gazebo_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/gazebo_control.cpp 2008-07-25 01:24:12 UTC (rev 2110)
+++ pkg/trunk/mechanism/mechanism_control/src/gazebo_control.cpp 2008-07-25 01:25:49 UTC (rev 2111)
@@ -65,6 +65,7 @@
r->joint = new Joint[PR2::MAX_JOINTS];
r->link = new Link[PR2::MAX_JOINTS];
+ // MAPPING BETWEEN hwInterface->actuators and robot->joints
// assign transmissions for all joints
for(int ii=0; ii<PR2::MAX_JOINTS; ii++){
r->transmission[ii].actuator = &(hardwareInterface->actuator[ii]);
@@ -81,13 +82,59 @@
}
void GazeboMechanismControl::initControllers(){
- baseController = new BaseController(r,"baseController");
- leftArmController = new ArmController(); //r,"leftArmController");
- rightArmController = new ArmController(); //r,"rightArmController");
- //headController = new HeadController(r,"headController");
- //laserScannerController = new LaserScannerController(r,"headController");
- //spineController = new SpineController(r,"headController");
+ // assuming that: robot joint array == hardware interface actuator array
+ // hard coded mapping between jointcontroller array and robot joint array
+ int baseMapToRobotJointIndex[] = {
+ PR2::CASTER_FL_STEER , PR2::CASTER_FL_DRIVE_L , PR2::CASTER_FL_DRIVE_R ,
+ PR2::CASTER_FR_STEER , PR2::CASTER_FR_DRIVE_L , PR2::CASTER_FR_DRIVE_R ,
+ PR2::CASTER_RL_STEER , PR2::CASTER_RL_DRIVE_L , PR2::CASTER_RL_DRIVE_R ,
+ PR2::CASTER_RR_STEER , PR2::CASTER_RR_DRIVE_L , PR2::CASTER_RR_DRIVE_R };
+ int leftArmMapToRobotJointIndex[] = {
+ PR2::ARM_L_PAN , PR2::ARM_L_SHOULDER_PITCH, PR2::ARM_L_SHOULDER_ROLL ,
+ PR2::ARM_L_ELBOW_PITCH , PR2::ARM_L_ELBOW_ROLL , PR2::ARM_L_WRIST_PITCH ,
+ PR2::ARM_L_WRIST_ROLL };
+ int rightArmMapToRobotJointIndex[] = {
+ PR2::ARM_R_PAN , PR2::ARM_R_SHOULDER_PITCH, PR2::ARM_R_SHOULDER_ROLL ,
+ PR2::ARM_R_ELBOW_PITCH , PR2::ARM_R_ELBOW_ROLL , PR2::ARM_R_WRIST_PITCH ,
+ PR2::ARM_R_WRIST_ROLL };
+ int headMapToRobotJointIndex[] = {
+ PR2::HEAD_YAW , PR2::HEAD_PITCH };
+ int laserScannerMapToRobotJointIndex[] = {
+ PR2::HEAD_LASER_PITCH };
+ int spineMapToRobotJointIndex[] = {
+ PR2::SPINE_ELEVATOR };
+ // hard coded mapping between jointcontroller array and hardware interface actuator array
+ int baseMapToHIActuatorIndex[] = {
+ PR2::CASTER_FL_STEER , PR2::CASTER_FL_DRIVE_L , PR2::CASTER_FL_DRIVE_R ,
+ PR2::CASTER_FR_STEER , PR2::CASTER_FR_DRIVE_L , PR2::CASTER_FR_DRIVE_R ,
+ PR2::CASTER_RL_STEER , PR2::CASTER_RL_DRIVE_L , PR2::CASTER_RL_DRIVE_R ,
+ PR2::CASTER_RR_STEER , PR2::CASTER_RR_DRIVE_L , PR2::CASTER_RR_DRIVE_R };
+ int leftArmMapToHIActuatorIndex[] = {
+ PR2::ARM_L_PAN , PR2::ARM_L_SHOULDER_PITCH, PR2::ARM_L_SHOULDER_ROLL ,
+ PR2::ARM_L_ELBOW_PITCH , PR2::ARM_L_ELBOW_ROLL , PR2::ARM_L_WRIST_PITCH ,
+ PR2::ARM_L_WRIST_ROLL };
+ int rightArmMapToHIActuatorIndex[] = {
+ PR2::ARM_R_PAN , PR2::ARM_R_SHOULDER_PITCH, PR2::ARM_R_SHOULDER_ROLL ,
+ PR2::ARM_R_ELBOW_PITCH , PR2::ARM_R_ELBOW_ROLL , PR2::ARM_R_WRIST_PITCH ,
+ PR2::ARM_R_WRIST_ROLL };
+ int headMapToHIActuatorIndex[] = {
+ PR2::HEAD_YAW , PR2::HEAD_PITCH };
+ int laserScannerMapToHIActuatorIndex[] = {
+ PR2::HEAD_LASER_PITCH };
+ int spineMapToHIActuatorIndex[] = {
+ PR2::SPINE_ELEVATOR };
+
+ int leftArmNumJoints = 7;
+ int rightArmNumJoints = 7;
+
+ baseController = new BaseController(r,"baseController"); // controller got lucky, starts at 0 in pr2Core, if pr2Core changes, update.
+ leftArmController = new ArmController(r,"leftArmController",leftArmNumJoints,leftArmMapToRobotJointIndex,leftArmMapToHIActuatorIndex);
+ rightArmController = new ArmController(r,"rightArmController",rightArmNumJoints,rightArmMapToRobotJointIndex,rightArmMapToHIActuatorIndex);
+ //headController = new HeadController(r,"headController",headNumJoints,headMapToRobotJointIndex[],headMapToHIActuatorIndex[]);
+ //laserScannerController = new LaserScannerController(r,"headController",laserScannerNumJoints,laserScannerMapToRobotJointIndex[],laserScannerMapToHIActuatorIndex[]);
+ //spineController = new SpineController(r,"headController",spienNumJoints,spineMapToRobotJointIndex[],spineMapToHIActuatorIndex[]);
+
// xml information, hardcoded
char *c_filename = getenv("ROS_PACKAGE_PATH");
std::stringstream filename;
@@ -96,7 +143,8 @@
baseController->loadXML(filename.str());
baseController->init();
- //leftArmController->loadXML(filename.str());
+ filename << c_filename << "/robot_descriptions/wg_robot_description/pr2/pr2.xml" ;
+ leftArmController->loadXML(filename.str());
leftArmController->init();
//rightArmController->loadXML(filename.str());
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
