Revision: 7350
http://playerstage.svn.sourceforge.net/playerstage/?rev=7350&view=rev
Author: natepak
Date: 2009-02-23 22:53:56 +0000 (Mon, 23 Feb 2009)
Log Message:
-----------
Updates to the pioneer2gripper controller
Modified Paths:
--------------
code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.cc
code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.hh
code/gazebo/trunk/server/controllers/ptz/generic/Generic_PTZ.cc
Modified:
code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.cc
===================================================================
--- code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.cc
2009-02-23 18:42:00 UTC (rev 7349)
+++ code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.cc
2009-02-23 22:53:56 UTC (rev 7350)
@@ -39,7 +39,7 @@
GZ_REGISTER_STATIC_CONTROLLER("pioneer2_gripper", Pioneer2_Gripper);
-enum {RIGHT, LEFT};
+enum {LEFT, RIGHT, LIFT};
////////////////////////////////////////////////////////////////////////////////
// Constructor
@@ -50,45 +50,102 @@
if (!this->myParent)
gzthrow("Pioneer2_Gripper controller requires a Model as its parent");
+
}
////////////////////////////////////////////////////////////////////////////////
// Destructor
Pioneer2_Gripper::~Pioneer2_Gripper()
{
+ for (int i=0; i<3; i++)
+ {
+ GZ_DELETE(this->jointNamesP[i]);
+ GZ_DELETE(this->gainsP[i]);
+ GZ_DELETE(this->forcesP[i]);
+ }
}
////////////////////////////////////////////////////////////////////////////////
// Load the controller
void Pioneer2_Gripper::LoadChild(XMLConfigNode *node)
{
+ int i;
+ XMLConfigNode *jNode;
this->myIface = dynamic_cast<GripperIface*>(this->ifaces[0]);
if (!this->myIface)
gzthrow("Pioneer2_Gripper controller requires a GripperIface");
Param::Begin(&this->parameters);
- this->leftJointNameP = new ParamT<std::string>("leftJoint", "", 1);
- this->rightJointNameP = new ParamT<std::string>("rightJoint", "", 1);
+ jNode = node->GetChild("leftJoint");
+ if (jNode)
+ {
+ this->jointNamesP[LEFT] = new ParamT<std::string>("name", "", 1);
+ this->jointNamesP[LEFT]->Load(jNode);
+
+ this->forcesP[LEFT] = new ParamT<float>("force",0.01,0);
+ this->forcesP[LEFT]->Load(jNode);
+
+ this->gainsP[LEFT] = new ParamT<float>("gain",0.01,0);
+ this->gainsP[LEFT]->Load(jNode);
+
+ this->joints[LEFT] =
dynamic_cast<SliderJoint*>(this->myParent->GetJoint(this->jointNamesP[LEFT]->GetValue()));
+ }
+
+ jNode = node->GetChild("rightJoint");
+ if (jNode)
+ {
+ this->jointNamesP[RIGHT] = new ParamT<std::string>("name", "", 1);
+ this->jointNamesP[RIGHT]->Load(jNode);
+
+ this->forcesP[RIGHT] = new ParamT<float>("force",0.01,0);
+ this->forcesP[RIGHT]->Load(jNode);
+
+ this->gainsP[RIGHT] = new ParamT<float>("gain",0.01,0);
+ this->gainsP[RIGHT]->Load(jNode);
+
+ this->joints[RIGHT] =
dynamic_cast<SliderJoint*>(this->myParent->GetJoint(this->jointNamesP[RIGHT]->GetValue()));
+ }
+
+ jNode = node->GetChild("liftJoint");
+ if (jNode)
+ {
+ this->jointNamesP[LIFT] = new ParamT<std::string>("name", "", 1);
+ this->jointNamesP[LIFT]->Load(jNode);
+
+ this->forcesP[LIFT] = new ParamT<float>("force",0.01,0);
+ this->forcesP[LIFT]->Load(jNode);
+
+ this->gainsP[LIFT] = new ParamT<float>("gain",0.01,0);
+ this->gainsP[LIFT]->Load(jNode);
+
+ this->joints[LIFT] =
dynamic_cast<SliderJoint*>(this->myParent->GetJoint(this->jointNamesP[LIFT]->GetValue()));
+ }
+
Param::End();
- this->joints[LEFT] =
dynamic_cast<SliderJoint*>(this->myParent->GetJoint(this->leftJointNameP->GetValue()));
- this->joints[RIGHT] =
dynamic_cast<SliderJoint*>(this->myParent->GetJoint(this->rightJointNameP->GetValue()));
-
if (!this->joints[LEFT])
gzthrow("couldn't get left slider joint");
if (!this->joints[RIGHT])
gzthrow("couldn't get right slider joint");
+ if (!this->joints[LIFT])
+ gzthrow("couldn't get lift slider joint");
}
////////////////////////////////////////////////////////////////////////////////
/// Save the controller
void Pioneer2_Gripper::SaveChild(std::string &prefix, std::ostream &stream)
{
- stream << prefix << *(this->leftJointNameP) << "\n";
- stream << prefix << *(this->rightJointNameP) << "\n";
+ for (int i=0; i < 3; i++)
+ {
+ stream << prefix << "<joint name=\"" << this->jointNamesP[i]->GetValue()
<< "\">\n";
+ stream << prefix << " " << *(this->forcesP[i]) << "\n";
+ stream << prefix << " " << *(this->gainsP[i]) << "\n";
+ stream << prefix << "</joint>\n";
+ }
+
}
////////////////////////////////////////////////////////////////////////////////
@@ -115,19 +172,43 @@
switch( this->myIface->data->cmd)
{
case GAZEBO_GRIPPER_CMD_OPEN:
- this->joints[RIGHT]->SetParam(dParamVel,0.1);
- this->joints[LEFT]->SetParam(dParamVel, -0.1);
+ this->joints[RIGHT]->SetParam(dParamVel,0.1);
+ this->joints[LEFT]->SetParam(dParamVel, -0.1);
break;
case GAZEBO_GRIPPER_CMD_CLOSE:
this->joints[RIGHT]->SetParam(dParamVel,-0.1);
this->joints[LEFT]->SetParam(dParamVel,0.1);
break;
+
+ case GAZEBO_GRIPPER_CMD_STORE:
+ this->joints[LIFT]->SetParam(dParamVel, 0.2);
+ break;
+
+ case GAZEBO_GRIPPER_CMD_RETRIEVE:
+ this->joints[LIFT]->SetParam(dParamVel, -0.2);
+ break;
+
+ case GAZEBO_GRIPPER_CMD_STOP:
+ this->joints[RIGHT]->SetParam(dParamVel,0);
+ this->joints[LEFT]->SetParam(dParamVel,0);
+ this->joints[LIFT]->SetParam(dParamVel,0);
+ break;
+
+
+ /*default:
+ this->joints[RIGHT]->SetParam(dParamVel,0.0);
+ this->joints[LEFT]->SetParam(dParamVel,0.0);
+ this->joints[LIFT]->SetParam(dParamVel,0.0);
+ break;
+ */
}
- this->joints[LEFT]->SetParam(dParamFMax,.01);
- this->joints[RIGHT]->SetParam(dParamFMax,.01);
+ this->joints[LEFT]->SetParam(dParamFMax, **(this->forcesP[LEFT]));
+ this->joints[RIGHT]->SetParam(dParamFMax, **(this->forcesP[RIGHT]));
+ this->joints[LIFT]->SetParam(dParamFMax, **(this->forcesP[LIFT]));
+
this->myIface->Unlock();
}
Modified:
code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.hh
===================================================================
--- code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.hh
2009-02-23 18:42:00 UTC (rev 7349)
+++ code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.hh
2009-02-23 22:53:56 UTC (rev 7350)
@@ -93,10 +93,12 @@
/// The parent Model
private: Model *myParent;
- private: SliderJoint *joints[2];
+ private: SliderJoint *joints[3];
- private: ParamT<std::string> *leftJointNameP;
- private: ParamT<std::string> *rightJointNameP;
+ private: ParamT<std::string> *jointNamesP[3];
+ private: ParamT<float> *gainsP[3];
+ private: ParamT<float> *forcesP[3];
+
};
/** \} */
Modified: code/gazebo/trunk/server/controllers/ptz/generic/Generic_PTZ.cc
===================================================================
--- code/gazebo/trunk/server/controllers/ptz/generic/Generic_PTZ.cc
2009-02-23 18:42:00 UTC (rev 7349)
+++ code/gazebo/trunk/server/controllers/ptz/generic/Generic_PTZ.cc
2009-02-23 22:53:56 UTC (rev 7350)
@@ -136,45 +136,55 @@
// Update the controller
void Generic_PTZ::UpdateChild()
{
+ float tiltSpeed = 0;
+ float panSpeed = 0;
+
this->ptzIface->Lock(1);
+ if (this->ptzIface->data->control_mode == GAZEBO_PTZ_POSITION_CONTROL)
+ {
+ this->cmdPan = this->ptzIface->data->cmd_pan;
+ this->cmdTilt = this->ptzIface->data->cmd_tilt;
+ //this->cmdZoom = this->hfov / this->ptzIface->data->cmd_zoom;
- this->cmdPan = this->ptzIface->data->cmd_pan;
- this->cmdTilt = this->ptzIface->data->cmd_tilt;
- //this->cmdZoom = this->hfov / this->ptzIface->data->cmd_zoom;
+ // Apply joint limits to commanded pan/tilt angles
+ if (this->cmdTilt > M_PI*0.3)
+ this->cmdTilt = M_PI*0.3;
+ else if (this->cmdTilt < -M_PI*0.3)
+ this->cmdTilt = -M_PI*0.3;
- this->ptzIface->Unlock();
+ if (this->cmdPan > M_PI*0.3)
+ this->cmdPan = M_PI*0.3;
+ else if (this->cmdPan < -M_PI*0.3)
+ this->cmdPan = -M_PI*0.3;
- // Apply joint limits to commanded pan/tilt angles
- if (this->cmdTilt > M_PI*0.3)
- this->cmdTilt = M_PI*0.3;
- else if (this->cmdTilt < -M_PI*0.3)
- this->cmdTilt = -M_PI*0.3;
+ // Apply limits on commanded zoom
+ //if (this->cmdZoom < this->zoomMin)
+ // this->cmdZoom = this->zoomMin;
+ //if (this->cmdZoom > this->zoomMax)
+ // this->cmdZoom = this->zoomMax;
- if (this->cmdPan > M_PI*0.3)
- this->cmdPan = M_PI*0.3;
- else if (this->cmdPan < -M_PI*0.3)
- this->cmdPan = -M_PI*0.3;
+ // Set the pan and tilt motors; can't set angles so track cmds with
+ // a proportional control
+ tiltSpeed = this->cmdTilt - this->tiltJoint->GetAngle();
+ panSpeed = this->cmdPan - this->panJoint->GetAngle();
+ }
+ else
+ {
+ tiltSpeed = this->ptzIface->data->cmd_tilt_speed;
+ panSpeed = this->ptzIface->data->cmd_pan_speed;
+ }
- // Apply limits on commanded zoom
- //if (this->cmdZoom < this->zoomMin)
- // this->cmdZoom = this->zoomMin;
- //if (this->cmdZoom > this->zoomMax)
- // this->cmdZoom = this->zoomMax;
+ this->ptzIface->Unlock();
- // Set the pan and tilt motors; can't set angles so track cmds with
- // a proportional control
- float tilt = this->cmdTilt - this->tiltJoint->GetAngle();
- float pan = this->cmdPan - this->panJoint->GetAngle();
-
- if (fabs(tilt) > 0.01)
- this->tiltJoint->SetParam( dParamVel, **(this->motionGainP) * tilt);
+ if (fabs(tiltSpeed) > 0.01)
+ this->tiltJoint->SetParam( dParamVel, **(this->motionGainP) * tiltSpeed);
else
this->tiltJoint->SetParam( dParamVel, 0);
this->tiltJoint->SetParam( dParamFMax, **(this->forceP) );
- if (fabs(pan) > 0.01)
- this->panJoint->SetParam( dParamVel, **(this->motionGainP) * pan);
+ if (fabs(panSpeed) > 0.01)
+ this->panJoint->SetParam( dParamVel, **(this->motionGainP) * panSpeed);
else
this->panJoint->SetParam( dParamVel, 0);
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Open Source Business Conference (OSBC), March 24-25, 2009, San Francisco, CA
-OSBC tackles the biggest issue in open source: Open Sourcing the Enterprise
-Strategies to boost innovation and cut costs with open source participation
-Receive a $600 discount off the registration fee with the source code: SFAD
http://p.sf.net/sfu/XcvMzF8H
_______________________________________________
Playerstage-commit mailing list
Playerstage-commit@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
