Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots: [812] pkg/trunk
|
From: <hsu...@us...> - 2008-06-14 00:26:03
|
Revision: 812
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=812&view=rev
Author: hsujohnhsu
Date: 2008-06-13 17:26:07 -0700 (Fri, 13 Jun 2008)
Log Message:
-----------
trying to fix the path following problem for the PR2 robot.
Modified Paths:
--------------
pkg/trunk/demos/2dnav-gazebo/world/robot.world
pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
pkg/trunk/simulators/rosgazebo/rosgazebo.cc
Modified: pkg/trunk/demos/2dnav-gazebo/world/robot.world
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/world/robot.world 2008-06-13 23:44:59 UTC (rev 811)
+++ pkg/trunk/demos/2dnav-gazebo/world/robot.world 2008-06-14 00:26:07 UTC (rev 812)
@@ -408,73 +408,73 @@
<updateRate>100.0</updateRate>
<joint name="front_left_caster_steer">
<saturationTorque>1000</saturationTorque>
- <pGain>1</pGain>
+ <pGain>2</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
</joint>
<joint name="front_left_wheel_l_drive">
- <saturationTorque>1000</saturationTorque>
- <pGain>1</pGain>
+ <saturationTorque>5</saturationTorque>
+ <pGain>0.5</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
</joint>
<joint name="front_left_wheel_r_drive">
- <saturationTorque>1000</saturationTorque>
- <pGain>1</pGain>
+ <saturationTorque>5</saturationTorque>
+ <pGain>0.5</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
</joint>
<joint name="front_right_caster_steer">
<saturationTorque>1000</saturationTorque>
- <pGain>1</pGain>
+ <pGain>2</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
</joint>
<joint name="front_right_wheel_l_drive">
- <saturationTorque>1000</saturationTorque>
- <pGain>1</pGain>
+ <saturationTorque>5</saturationTorque>
+ <pGain>0.5</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
</joint>
<joint name="front_right_wheel_r_drive">
- <saturationTorque>1000</saturationTorque>
- <pGain>1</pGain>
+ <saturationTorque>5</saturationTorque>
+ <pGain>0.5</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
</joint>
<joint name="rear_left_caster_steer">
<saturationTorque>1000</saturationTorque>
- <pGain>1</pGain>
+ <pGain>2</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
</joint>
<joint name="rear_left_wheel_l_drive">
- <saturationTorque>1000</saturationTorque>
- <pGain>1</pGain>
+ <saturationTorque>5</saturationTorque>
+ <pGain>0.5</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
</joint>
<joint name="rear_left_wheel_r_drive">
- <saturationTorque>1000</saturationTorque>
- <pGain>1</pGain>
+ <saturationTorque>5</saturationTorque>
+ <pGain>0.5</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
</joint>
<joint name="rear_right_caster_steer">
<saturationTorque>1000</saturationTorque>
- <pGain>1</pGain>
+ <pGain>2</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
</joint>
<joint name="rear_right_wheel_l_drive">
- <saturationTorque>1000</saturationTorque>
- <pGain>1</pGain>
+ <saturationTorque>5</saturationTorque>
+ <pGain>0.5</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
</joint>
<joint name="rear_right_wheel_r_drive">
- <saturationTorque>1000</saturationTorque>
- <pGain>1</pGain>
+ <saturationTorque>5</saturationTorque>
+ <pGain>0.5</pGain>
<dGain>0</dGain>
<iGain>0</iGain>
</joint>
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h 2008-06-13 23:44:59 UTC (rev 811)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h 2008-06-14 00:26:07 UTC (rev 812)
@@ -561,6 +561,11 @@
public: PR2_ERROR_CODE SetBaseSteeringAngle(double vx, double vy, double vw);
/*! \fn
+ \brief Checks to see if steering angles are lined up
+ */
+ public: bool CheckBaseSteeringAngle(double errorTol);
+
+ /*! \fn
\brief Retrieve commanded speed for the base in cartesian space in body coordinates
\param vx - forward speed
\param vy - sideways speed
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-06-13 23:44:59 UTC (rev 811)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-06-14 00:26:07 UTC (rev 812)
@@ -1205,34 +1205,24 @@
point newDriveCenterL, newDriveCenterR;
- double currentRateCmd[NUM_CASTERS];
- double currentRateAct[NUM_CASTERS];
- double currentCmd[NUM_CASTERS];
- double currentAngle[NUM_CASTERS];
- double currentError[NUM_CASTERS];
- const double errorTol = M_PI / 50.0;
-
for(int ii=0; ii < NUM_CASTERS; ii++)
{
ComputePointVelocity(vx,vy,vw,BASE_CASTER_OFFSET[ii].x,BASE_CASTER_OFFSET[ii].y,steerPointVelocity[ii].x,steerPointVelocity[ii].y);
steerAngle[ii] = atan2(steerPointVelocity[ii].y,steerPointVelocity[ii].x);
SetJointServoCmd((PR2_JOINT_ID) (CASTER_FL_STEER+3*ii),steerAngle[ii],0);
-// printf("ii: %d, off: (%f, %f), vel: (%f, %f), angle: %f\n",ii,BASE_CASTER_OFFSET[ii].x,BASE_CASTER_OFFSET[ii].y,steerPointVelocity[ii].x,steerPointVelocity[ii].y,steerAngle[ii]);
+ // printf("ii: %d, off: (%f, %f), vel: (%f, %f), angle: %f\n",ii,BASE_CASTER_OFFSET[ii].x,BASE_CASTER_OFFSET[ii].y,steerPointVelocity[ii].x,steerPointVelocity[ii].y,steerAngle[ii]);
}
for(int ii = 0; ii < NUM_CASTERS; ii++)
{
- // do not move forward unless the wheels are aligned...
- // this is a goofy hack, should put a more complicated version later
- GetJointServoCmd((PR2_JOINT_ID)(CASTER_FL_STEER+3*ii),¤tCmd[ii],¤tRateCmd[ii]);
- GetJointServoActual((PR2_JOINT_ID)(CASTER_FL_STEER+3*ii),¤tAngle[ii],¤tRateAct[ii]);
- currentError[ii] = fabs(currentCmd[ii] - currentAngle[ii]);
- //std::cout << " e " << ii << " e " << currentError[ii] << std::endl;
- if (currentError[ii] < errorTol)
- {
newDriveCenterL = Rot2D(CASTER_DRIVE_OFFSET[ii*2 ],steerAngle[ii]);
newDriveCenterR = Rot2D(CASTER_DRIVE_OFFSET[ii*2+1],steerAngle[ii]);
+ newDriveCenterL.x += BASE_CASTER_OFFSET[ii].x;
+ newDriveCenterL.y += BASE_CASTER_OFFSET[ii].y;
+ newDriveCenterR.x += BASE_CASTER_OFFSET[ii].x;
+ newDriveCenterR.y += BASE_CASTER_OFFSET[ii].y;
+
ComputePointVelocity(vx,vy,vw,newDriveCenterL.x,newDriveCenterL.y,drivePointVelocityL.x,drivePointVelocityL.y);
ComputePointVelocity(vx,vy,vw,newDriveCenterR.x,newDriveCenterR.y,drivePointVelocityR.x,drivePointVelocityR.y);
@@ -1242,16 +1232,53 @@
// send command
this->SetJointSpeed((PR2_JOINT_ID) (CASTER_FL_DRIVE_L+3*ii),wheelSpeed[ii*2 ]);
this->SetJointSpeed((PR2_JOINT_ID) (CASTER_FL_DRIVE_R+3*ii),wheelSpeed[ii*2+1]);
- this->SetJointTorque((PR2_JOINT_ID) (CASTER_FL_DRIVE_L+3*ii),100.0 );
- this->SetJointTorque((PR2_JOINT_ID) (CASTER_FL_DRIVE_R+3*ii),100.0 );
- }
}
return PR2_ALL_OK;
};
+// PR2_ERROR_CODE PR2Robot::SetBaseUnicycleSpeedCmd(double vx, double vw)
+// {
+// point drivePointVelocityL, drivePointVelocityR;
+// double wheelSpeed[NUM_WHEELS];
+//
+// point steerPointVelocity[NUM_CASTERS];
+// double steerAngle[NUM_CASTERS];
+//
+// point newDriveCenterL, newDriveCenterR;
+//
+// ComputePointVelocity(vx,vy,vw,BASE_CASTER_OFFSET[0].x,BASE_CASTER_OFFSET[0].y,steerPointVelocity[0].x,steerPointVelocity[0].y);
+// steerAngle[0] = atan2(steerPointVelocity[0].y,steerPointVelocity[0].x);
+// ComputePointVelocity(vx,vy,vw,BASE_CASTER_OFFSET[1].x,BASE_CASTER_OFFSET[1].y,steerPointVelocity[1].x,steerPointVelocity[1].y);
+// steerAngle[1] = atan2(steerPointVelocity[1].y,steerPointVelocity[1].x);
+// SetJointServoCmd((PR2_JOINT_ID) (CASTER_FL_STEER),steerAngle[0],0);
+// SetJointServoCmd((PR2_JOINT_ID) (CASTER_FR_STEER),steerAngle[1],0);
+// SetJointServoCmd((PR2_JOINT_ID) (CASTER_RL_STEER), 0,0);
+// SetJointServoCmd((PR2_JOINT_ID) (CASTER_RR_STEER), 0,0);
+// // printf("ii: %d, off: (%f, %f), vel: (%f, %f), angle: %f\n",ii,BASE_CASTER_OFFSET[ii].x,BASE_CASTER_OFFSET[ii].y,steerPointVelocity[ii].x,steerPointVelocity[ii].y,steerAngle[ii]);
+//
+// for(int ii = 0; ii < NUM_CASTERS; ii++)
+// {
+//
+// newDriveCenterL = Rot2D(CASTER_DRIVE_OFFSET[ii*2 ],steerAngle[ii]);
+// newDriveCenterR = Rot2D(CASTER_DRIVE_OFFSET[ii*2+1],steerAngle[ii]);
+//
+// ComputePointVelocity(vx,vy,vw,newDriveCenterL.x,newDriveCenterL.y,drivePointVelocityL.x,drivePointVelocityL.y);
+// ComputePointVelocity(vx,vy,vw,newDriveCenterR.x,newDriveCenterR.y,drivePointVelocityR.x,drivePointVelocityR.y);
+//
+// wheelSpeed[ii*2 ] = -GetMagnitude(drivePointVelocityL.x,drivePointVelocityL.y)/WHEEL_RADIUS;
+// wheelSpeed[ii*2+1] = -GetMagnitude(drivePointVelocityR.x,drivePointVelocityR.y)/WHEEL_RADIUS;
+//
+// // send speed command
+// this->SetJointSpeed((PR2_JOINT_ID) (CASTER_FL_DRIVE_L),wheelSpeed[ii*2 ]);
+// this->SetJointSpeed((PR2_JOINT_ID) (CASTER_FL_DRIVE_R),wheelSpeed[ii*2+1]);
+// }
+//
+// return PR2_ALL_OK;
+// };
+
PR2_ERROR_CODE PR2Robot::SetBaseSteeringAngle(double vx, double vy, double vw)
{
point steerPointVelocity[NUM_CASTERS];
@@ -1270,16 +1297,40 @@
return PR2_ALL_OK;
};
+bool PR2Robot::CheckBaseSteeringAngle(double errorTol)
+{
+ point steerPointVelocity[NUM_CASTERS];
+ double steerAngle[NUM_CASTERS];
+ point newDriveCenterL, newDriveCenterR;
+ double currentRateCmd[NUM_CASTERS];
+ double currentRateAct[NUM_CASTERS];
+ double currentCmd[NUM_CASTERS];
+ double currentAngle[NUM_CASTERS];
+ double currentError[NUM_CASTERS];
+ double errorTotal;
+ errorTotal = 0.0;
+ for(int ii=0; ii < NUM_CASTERS; ii++)
+ {
+ // do not move forward unless the wheels are aligned...
+ // this is a goofy hack, should put a more complicated version later
+ GetJointServoCmd((PR2_JOINT_ID)(CASTER_FL_STEER+3*ii),¤tCmd[ii],¤tRateCmd[ii]);
+ GetJointServoActual((PR2_JOINT_ID)(CASTER_FL_STEER+3*ii),¤tAngle[ii],¤tRateAct[ii]);
+ currentError[ii] = fabs(currentCmd[ii] - currentAngle[ii]);
+ //std::cout << " e " << ii << " e " << currentError[ii] << std::endl;
+ errorTotal = errorTotal + currentError[ii];
+ }
+ if (errorTotal < errorTol)
+ return true;
+ else
+ return false;
+};
-
-
-
PR2_ERROR_CODE PR2Robot::GetBaseCartesianSpeedCmd(double* vx, double* vy, double* vw)
{
// FIXME: TODO: not implemented
Modified: pkg/trunk/simulators/rosgazebo/rosgazebo.cc
===================================================================
--- pkg/trunk/simulators/rosgazebo/rosgazebo.cc 2008-06-13 23:44:59 UTC (rev 811)
+++ pkg/trunk/simulators/rosgazebo/rosgazebo.cc 2008-06-14 00:26:07 UTC (rev 812)
@@ -70,6 +70,12 @@
// for frame transforms, publish frame transforms
rosTFServer tf;
+ // time step calculation
+ double lastTime, simTime;
+
+ // smooth vx, vw commands
+ double vxSmooth, vwSmooth;
+
public:
// Constructor; stage itself needs argc/argv. fname is the .world file
// that stage should load.
@@ -188,12 +194,44 @@
GazeboNode::cmdvelReceived()
{
this->lock.lock();
+ double dt;
+ double w11, w21, w12, w22;
+ w11 = 1.0;
+ w21 = 1.0;
+ w12 = 1.0;
+ w22 = 1.0;
- //printf("received cmd: %.3f %.3f\n",
- // this->velMsg.vx, this->velMsg.vw);
+ // smooth out the commands by time decay
+ // with w1,w2=1, this means equal weighting for new command every second
+ this->myPR2->GetSimTime(&(this->simTime));
+ dt = simTime - lastTime;
+ //this->vxSmooth = (w11 * this->vxSmooth + w21*dt *this->velMsg.vx)/( w11 + w21*dt);
+ //this->vwSmooth = (w12 * this->vwSmooth + w22*dt *this->velMsg.vw)/( w12 + w22*dt);
+ this->vxSmooth = this->velMsg.vx;
+ this->vwSmooth = this->velMsg.vw;
+
+ fprintf(stderr,"received cmd: %.3f %.3f | %.3f %.3f\n", this->velMsg.vx, this->velMsg.vw,this->vxSmooth, this->vwSmooth);
+
+ this->myPR2->SetBaseSteeringAngle (this->vxSmooth,0.0,this->vwSmooth);
+ while (!this->myPR2->CheckBaseSteeringAngle(M_PI/10.0))
+ {
+ // do nothing and wait...
+ usleep(100000);
+ }
// set base velocity
- this->myPR2->SetBaseCartesianSpeedCmd(this->velMsg.vx, 0.0, this->velMsg.vw);
+ this->myPR2->SetJointTorque(PR2::CASTER_FL_DRIVE_L, 1000.0 );
+ this->myPR2->SetJointTorque(PR2::CASTER_FR_DRIVE_L, 1000.0 );
+ this->myPR2->SetJointTorque(PR2::CASTER_RL_DRIVE_L, 1000.0 );
+ this->myPR2->SetJointTorque(PR2::CASTER_RR_DRIVE_L, 1000.0 );
+ this->myPR2->SetJointTorque(PR2::CASTER_FL_DRIVE_R, 1000.0 );
+ this->myPR2->SetJointTorque(PR2::CASTER_FR_DRIVE_R, 1000.0 );
+ this->myPR2->SetJointTorque(PR2::CASTER_RL_DRIVE_R, 1000.0 );
+ this->myPR2->SetJointTorque(PR2::CASTER_RR_DRIVE_R, 1000.0 );
+ this->myPR2->SetBaseCartesianSpeedCmd(this->vxSmooth, 0.0, this->vwSmooth);
+
+ this->lastTime = this->simTime;
+
this->lock.unlock();
}
@@ -224,6 +262,9 @@
*/
this->myPR2->EnableGripperLeft();
this->myPR2->EnableGripperRight();
+
+ this->myPR2->GetSimTime(&(this->lastTime));
+ this->myPR2->GetSimTime(&(this->simTime));
}
void GazeboNode::finalize(int)
@@ -267,7 +308,6 @@
uint32_t intensities_size;
uint32_t intensities_alloc_size;
std_msgs::Point3DFloat32 tmp_cloud_pt;
- double simTime;
/***************************************************************/
/* */
@@ -323,10 +363,8 @@
}
+ this->myPR2->GetSimTime(&(this->simTime));
-
- this->myPR2->GetSimTime(&simTime);
-
/***************************************************************/
/* */
/* laser - base */
@@ -353,8 +391,8 @@
}
this->laserMsg.header.frame_id = FRAMEID_LASER;
- this->laserMsg.header.stamp.sec = (unsigned long)floor(simTime);
- this->laserMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( simTime - this->laserMsg.header.stamp.sec) );
+ this->laserMsg.header.stamp.sec = (unsigned long)floor(this->simTime);
+ this->laserMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->simTime - this->laserMsg.header.stamp.sec) );
//publish("laser",this->laserMsg); // for laser_view FIXME: can alias this at the commandline or launch script
publish("scan",this->laserMsg); // for rosstage
@@ -387,8 +425,8 @@
// TODO: get the frame ID from somewhere
this->odomMsg.header.frame_id = FRAMEID_ODOM;
- this->odomMsg.header.stamp.sec = (unsigned long)floor(simTime);
- this->odomMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( simTime - this->odomMsg.header.stamp.sec) );
+ this->odomMsg.header.stamp.sec = (unsigned long)floor(this->simTime);
+ this->odomMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->simTime - this->odomMsg.header.stamp.sec) );
publish("odom",this->odomMsg);
@@ -447,10 +485,10 @@
simPitchTimeScale = 2.0*M_PI*simPitchFreq;
simPitchAmp = M_PI / 8.0;
simPitchOffset = -M_PI / 8.0;
- simPitchAngle = simPitchOffset + simPitchAmp * sin(simTime * simPitchTimeScale);
- simPitchRate = simPitchAmp * simPitchTimeScale * cos(simTime * simPitchTimeScale); // TODO: check rate correctness
- this->myPR2->GetSimTime(&simTime);
- //std::cout << "sim time: " << simTime << std::endl;
+ simPitchAngle = simPitchOffset + simPitchAmp * sin(this->simTime * simPitchTimeScale);
+ simPitchRate = simPitchAmp * simPitchTimeScale * cos(this->simTime * simPitchTimeScale); // TODO: check rate correctness
+ this->myPR2->GetSimTime(&this->simTime);
+ //std::cout << "sim time: " << this->simTime << std::endl;
//std::cout << "ang: " << simPitchAngle*180.0/M_PI << "rate: " << simPitchRate*180.0/M_PI << std::endl;
this->myPR2->SetJointTorque(PR2::HEAD_LASER_PITCH , 1000.0);
this->myPR2->SetJointGains(PR2::HEAD_LASER_PITCH, 10.0, 0.0, 0.0);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
