Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots: [927] pkg/trunk
|
From: <hsu...@us...> - 2008-06-24 17:47:48
|
Revision: 927
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=927&view=rev
Author: hsujohnhsu
Date: 2008-06-24 10:47:55 -0700 (Tue, 24 Jun 2008)
Log Message:
-----------
bugfix in rosgazebo for joint position calls. added gripper state request calls.
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h
pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp
pkg/trunk/simulators/rosgazebo/rosgazebo.cc
Modified: pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h 2008-06-24 17:44:12 UTC (rev 926)
+++ pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h 2008-06-24 17:47:55 UTC (rev 927)
@@ -267,6 +267,16 @@
public: PR2_ERROR_CODE CloseGripper(PR2_MODEL_ID id,double gap, double force);
/*! \fn
+ \brief - Get gripper gap and force setpoint
+ */
+ public: PR2_ERROR_CODE GetGripperCmd(PR2_MODEL_ID id,double *gap,double *force);
+
+ /*! \fn
+ \brief - Get gripper gap and force status
+ */
+ public: PR2_ERROR_CODE GetGripperActual(PR2_MODEL_ID id,double *gap,double *force);
+
+ /*! \fn
\brief - Set gripper p,i,d gains
*/
public: PR2_ERROR_CODE SetGripperGains(PR2_MODEL_ID id,double p,double i, double d);
Modified: pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp 2008-06-24 17:44:12 UTC (rev 926)
+++ pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp 2008-06-24 17:47:55 UTC (rev 927)
@@ -857,6 +857,44 @@
return PR2_ALL_OK;
};
+PR2_ERROR_CODE PR2HW::GetGripperCmd(PR2_MODEL_ID id,double *gap,double *force)
+{
+ switch(id)
+ {
+ case PR2_LEFT_GRIPPER:
+ *force = pr2GripperLeftIface->data->cmdForce;
+ *gap = pr2GripperLeftIface->data->cmdGap ;
+ break;
+ case PR2_RIGHT_GRIPPER:
+ *force = pr2GripperRightIface->data->cmdForce;
+ *gap = pr2GripperRightIface->data->cmdGap ;
+ break;
+ default:
+ break;
+ }
+ return PR2_ALL_OK;
+};
+
+PR2_ERROR_CODE PR2HW::GetGripperActual(PR2_MODEL_ID id,double *gap,double *force)
+{
+ switch(id)
+ {
+ case PR2_LEFT_GRIPPER:
+ *force = pr2GripperLeftIface->data->gripperForce; // FIXME: this is saturation force
+ *gap = ( pr2GripperLeftIface->data->actualFingerPosition[0] - 0.015) // FIXME: not debugged
+ +(-pr2GripperLeftIface->data->actualFingerPosition[1] + 0.015);
+ break;
+ case PR2_RIGHT_GRIPPER:
+ *force = pr2GripperRightIface->data->gripperForce; // FIXME: this is saturation force
+ *gap = ( pr2GripperRightIface->data->actualFingerPosition[0] - 0.015) // FIXME: not debugged
+ +(-pr2GripperRightIface->data->actualFingerPosition[1] + 0.015);
+ break;
+ default:
+ break;
+ }
+ return PR2_ALL_OK;
+};
+
PR2_ERROR_CODE PR2HW::SetGripperGains(PR2_MODEL_ID id,double p,double i, double d)
{
switch(id)
Modified: pkg/trunk/simulators/rosgazebo/rosgazebo.cc
===================================================================
--- pkg/trunk/simulators/rosgazebo/rosgazebo.cc 2008-06-24 17:44:12 UTC (rev 926)
+++ pkg/trunk/simulators/rosgazebo/rosgazebo.cc 2008-06-24 17:47:55 UTC (rev 927)
@@ -223,10 +223,10 @@
// when running with the 2dnav stack, we need to refrain from moving when steering angles are off.
// when operating with the keyboard, we need instantaneous setting of both velocity and angular velocity.
- std::cout << "received cmd: vx " << this->velMsg.vx << " vw " << this->velMsg.vw
- << " vxsmoo " << this->vxSmooth << " vxsmoo " << this->vwSmooth
- << " | steer erros: " << this->myPR2->BaseSteeringAngleError() << " - " << M_PI/100.0
- << std::endl;
+ // std::cout << "received cmd: vx " << this->velMsg.vx << " vw " << this->velMsg.vw
+ // << " vxsmoo " << this->vxSmooth << " vxsmoo " << this->vwSmooth
+ // << " | steer erros: " << this->myPR2->BaseSteeringAngleError() << " - " << M_PI/100.0
+ // << std::endl;
// 2dnav: if steering angle is wrong, don't move or move slowly
if (this->myPR2->BaseSteeringAngleError() > M_PI/100.0)
@@ -596,39 +596,45 @@
std_msgs::PR2Arm larm, rarm;
/* get left arm position */
- this->myPR2->GetJointPositionActual(PR2::ARM_L_PAN, &position, &velocity);
+ this->myPR2->hw.GetJointPositionActual(PR2::ARM_L_PAN, &position, &velocity);
larm.turretAngle = position;
- this->myPR2->GetJointPositionActual(PR2::ARM_L_SHOULDER_PITCH, &position, &velocity);
+ this->myPR2->hw.GetJointPositionActual(PR2::ARM_L_SHOULDER_PITCH, &position, &velocity);
larm.shoulderLiftAngle = position;
- this->myPR2->GetJointPositionActual(PR2::ARM_L_SHOULDER_ROLL, &position, &velocity);
+ this->myPR2->hw.GetJointPositionActual(PR2::ARM_L_SHOULDER_ROLL, &position, &velocity);
larm.upperarmRollAngle = position;
- this->myPR2->GetJointPositionActual(PR2::ARM_L_ELBOW_PITCH, &position, &velocity);
+ this->myPR2->hw.GetJointPositionActual(PR2::ARM_L_ELBOW_PITCH, &position, &velocity);
larm.elbowAngle = position;
- this->myPR2->GetJointPositionActual(PR2::ARM_L_ELBOW_ROLL, &position, &velocity);
+ this->myPR2->hw.GetJointPositionActual(PR2::ARM_L_ELBOW_ROLL, &position, &velocity);
larm.forearmRollAngle = position;
- this->myPR2->GetJointPositionActual(PR2::ARM_L_WRIST_PITCH, &position, &velocity);
+ this->myPR2->hw.GetJointPositionActual(PR2::ARM_L_WRIST_PITCH, &position, &velocity);
larm.wristPitchAngle = position;
- this->myPR2->GetJointPositionActual(PR2::ARM_L_WRIST_ROLL, &position, &velocity);
+ this->myPR2->hw.GetJointPositionActual(PR2::ARM_L_WRIST_ROLL, &position, &velocity);
larm.wristRollAngle = position;
// JOHN: We need to set the gripperForceCmd and gripperGapCmd as well; I think an API call is missing from libPR2API
+ this->myPR2->hw.GetGripperActual (PR2::PR2_LEFT_GRIPPER, &position, &velocity);
+ larm.gripperForceCmd = velocity;
+ larm.gripperGapCmd = position;
publish("left_pr2arm_pos", larm);
/* get left arm position */
- this->myPR2->GetJointPositionActual(PR2::ARM_L_PAN, &position, &velocity);
+ this->myPR2->hw.GetJointPositionActual(PR2::ARM_L_PAN, &position, &velocity);
rarm.turretAngle = position;
- this->myPR2->GetJointPositionActual(PR2::ARM_L_SHOULDER_PITCH, &position, &velocity);
+ this->myPR2->hw.GetJointPositionActual(PR2::ARM_L_SHOULDER_PITCH, &position, &velocity);
rarm.shoulderLiftAngle = position;
- this->myPR2->GetJointPositionActual(PR2::ARM_L_SHOULDER_ROLL, &position, &velocity);
+ this->myPR2->hw.GetJointPositionActual(PR2::ARM_L_SHOULDER_ROLL, &position, &velocity);
rarm.upperarmRollAngle = position;
- this->myPR2->GetJointPositionActual(PR2::ARM_L_ELBOW_PITCH, &position, &velocity);
+ this->myPR2->hw.GetJointPositionActual(PR2::ARM_L_ELBOW_PITCH, &position, &velocity);
rarm.elbowAngle = position;
- this->myPR2->GetJointPositionActual(PR2::ARM_L_ELBOW_ROLL, &position, &velocity);
+ this->myPR2->hw.GetJointPositionActual(PR2::ARM_L_ELBOW_ROLL, &position, &velocity);
rarm.forearmRollAngle = position;
- this->myPR2->GetJointPositionActual(PR2::ARM_L_WRIST_PITCH, &position, &velocity);
+ this->myPR2->hw.GetJointPositionActual(PR2::ARM_L_WRIST_PITCH, &position, &velocity);
rarm.wristPitchAngle = position;
- this->myPR2->GetJointPositionActual(PR2::ARM_L_WRIST_ROLL, &position, &velocity);
+ this->myPR2->hw.GetJointPositionActual(PR2::ARM_L_WRIST_ROLL, &position, &velocity);
rarm.wristRollAngle = position;
// JOHN: We need to set the gripperForceCmd and gripperGapCmd as well; I think an API call is missing from libPR2API
+ this->myPR2->hw.GetGripperActual (PR2::PR2_RIGHT_GRIPPER, &position, &velocity);
+ rarm.gripperForceCmd = velocity;
+ rarm.gripperGapCmd = position;
publish("right_pr2arm_pos", rarm);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
