Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[2549] pkg/trunk
|
From: <hsu...@us...> - 2008-08-05 07:46:43
|
Revision: 2549
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2549&view=rev
Author: hsujohnhsu
Date: 2008-08-05 07:46:49 +0000 (Tue, 05 Aug 2008)
Log Message:
-----------
removed old gripper interface implementation. updated sensors in pr2.xml, for parsing only.
Modified Paths:
--------------
pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
pkg/trunk/drivers/robot/pr2/libpr2API/src/test/pick_object.cpp
pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h
pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp
pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h
pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Misc.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Camera.hh
pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Camera.cc
pkg/trunk/manip/teleop_arm_keyboard/teleop_arm_keyboard.cc
pkg/trunk/robot_control_loops/pr2_gazebo/src/RosGazeboControlsNode.cpp
pkg/trunk/robot_control_loops/pr2_gazebo/src/pr2_gazebo.cpp
pkg/trunk/robot_control_loops/pr2_gazebo/src/test2.cpp
pkg/trunk/robot_control_loops/pr2_gazebo/src/test3.cpp
pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
pkg/trunk/simulators/rosgazebo/include/RosGazeboNode/RosGazeboNode.h
pkg/trunk/simulators/rosgazebo/manifest.xml
pkg/trunk/simulators/rosgazebo/src/RosGazeboNode.cpp
pkg/trunk/simulators/rosgazebo/src/rosgazebo.cc
pkg/trunk/simulators/rosgazebo/src/rosgazebowriter.cc
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h 2008-08-05 07:01:19 UTC (rev 2548)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h 2008-08-05 07:46:49 UTC (rev 2549)
@@ -250,17 +250,6 @@
public: PR2_ERROR_CODE EnableHead();
/*! \fn
- \brief Enable the left gripper (i.e. enable all actuators corresponding to the left gripper)
- */
- public: PR2_ERROR_CODE EnableGripperLeft();
-
- /*! \fn
- \brief Enable the right gripper (i.e. enable all actuators corresponding to the right gripper)
- */
- public: PR2_ERROR_CODE EnableGripperRight();
-
-
- /*! \fn
\brief Enable the arm (i.e. enable all actuators corresponding to an arm)
\param id - model ID, see pr2Core.h for a list of model IDs
*/
@@ -268,13 +257,6 @@
/*! \fn
- \brief Enable the gripper (i.e. enable all actuators corresponding to the gripper)
- \param id - model ID, see pr2Core.h for a list of model IDs
- */
- public: PR2_ERROR_CODE EnableGripper(PR2_MODEL_ID id);
-
-
- /*! \fn
\brief Enable the base (i.e. enable all actuators corresponding to the base)
*/
public: PR2_ERROR_CODE EnableBase();
@@ -291,17 +273,6 @@
public: PR2_ERROR_CODE DisableHead();
/*! \fn
- \brief Disable the head (i.e. enable all actuators corresponding to the head)
- */
- public: PR2_ERROR_CODE DisableGripperLeft();
-
- /*! \fn
- \brief Disable the head (i.e. enable all actuators corresponding to the head)
- */
- public: PR2_ERROR_CODE DisableGripperRight();
-
-
- /*! \fn
\brief Disable the arm (i.e. disable all actuators corresponding to an arm)
\param id - model ID, see pr2Core.h for a list of model IDs
*/
@@ -309,13 +280,6 @@
/*! \fn
- \brief Disable the gripper (i.e. disable all actuators corresponding to a gripper)
- \param id - model ID, see pr2Core.h for a list of model IDs
- */
- public: PR2_ERROR_CODE DisableGripper(PR2_MODEL_ID id);
-
-
- /*! \fn
\brief Disable the base (i.e. disable all actuators corresponding to the base)
*/
public: PR2_ERROR_CODE DisableBase();
@@ -627,6 +591,9 @@
public: PR2_ERROR_CODE StopRobot();
+ /*! \fn
+ \brief - Shortcuts to get gripper commands
+ */
public: PR2_ERROR_CODE GetLeftGripperCmd(double *gap,double *force);
public: PR2_ERROR_CODE GetLeftGripperActual(double *gap,double *force);
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-08-05 07:01:19 UTC (rev 2548)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-08-05 07:46:49 UTC (rev 2549)
@@ -41,40 +41,12 @@
return PR2_ALL_OK;
};
-PR2_ERROR_CODE PR2Robot::EnableGripper(PR2_MODEL_ID id)
-{
- switch(id)
- {
- case PR2_LEFT_GRIPPER:
- EnableGripperLeft();
- break;
- case PR2_RIGHT_GRIPPER:
- EnableGripperRight();
- break;
- default:
- break;
- }
- return PR2_ALL_OK;
-};
-
PR2_ERROR_CODE PR2Robot::EnableHead()
{
hw.EnableModel(HEAD);
return PR2_ALL_OK;
}
-PR2_ERROR_CODE PR2Robot::EnableGripperLeft()
-{
- hw.EnableModel(PR2_LEFT_GRIPPER);
- return PR2_ALL_OK;
-}
-
-PR2_ERROR_CODE PR2Robot::EnableGripperRight()
-{
- hw.EnableModel(PR2_RIGHT_GRIPPER);
- return PR2_ALL_OK;
-}
-
PR2_ERROR_CODE PR2Robot::EnableBase()
{
hw.EnableModel(PR2_BASE);
@@ -103,40 +75,12 @@
return PR2_ALL_OK;
};
-PR2_ERROR_CODE PR2Robot::DisableGripper(PR2_MODEL_ID id)
-{
- switch(id)
- {
- case PR2_LEFT_GRIPPER:
- DisableGripperLeft();
- break;
- case PR2_RIGHT_GRIPPER:
- DisableGripperRight();
- break;
- default:
- break;
- }
- return PR2_ALL_OK;
-};
-
PR2_ERROR_CODE PR2Robot::DisableHead()
{
hw.DisableModel(HEAD);
return PR2_ALL_OK;
}
-PR2_ERROR_CODE PR2Robot::DisableGripperLeft()
-{
- hw.DisableModel(PR2_LEFT_GRIPPER);
- return PR2_ALL_OK;
-}
-
-PR2_ERROR_CODE PR2Robot::DisableGripperRight()
-{
- hw.DisableModel(PR2_RIGHT_GRIPPER);
- return PR2_ALL_OK;
-}
-
PR2_ERROR_CODE PR2Robot::DisableBase()
{
hw.DisableModel(PR2_BASE);
@@ -927,25 +871,25 @@
PR2_ERROR_CODE PR2Robot::GetLeftGripperCmd(double *gap,double *force)
{
- hw.GetGripperCmd((PR2_MODEL_ID)PR2::PR2_LEFT_GRIPPER,gap,force);
+ hw.GetJointServoCmd((PR2_JOINT_ID)PR2::ARM_L_GRIPPER_GAP,gap,force);
return PR2_ALL_OK;
}
PR2_ERROR_CODE PR2Robot::GetLeftGripperActual(double *gap,double *force)
{
- hw.GetGripperActual((PR2_MODEL_ID)PR2::PR2_LEFT_GRIPPER,gap,force);
+ hw.GetJointServoActual((PR2_JOINT_ID)PR2::ARM_L_GRIPPER_GAP,gap,force);
return PR2_ALL_OK;
}
PR2_ERROR_CODE PR2Robot::GetRightGripperCmd(double *gap,double *force)
{
- hw.GetGripperCmd((PR2_MODEL_ID)PR2::PR2_RIGHT_GRIPPER,gap,force);
+ hw.GetJointServoCmd((PR2_JOINT_ID)PR2::ARM_R_GRIPPER_GAP,gap,force);
return PR2_ALL_OK;
}
PR2_ERROR_CODE PR2Robot::GetRightGripperActual(double *gap,double *force)
{
- hw.GetGripperActual((PR2_MODEL_ID)PR2::PR2_RIGHT_GRIPPER,gap,force);
+ hw.GetJointServoActual((PR2_JOINT_ID)PR2::ARM_R_GRIPPER_GAP,gap,force);
return PR2_ALL_OK;
}
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/src/test/pick_object.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/test/pick_object.cpp 2008-08-05 07:01:19 UTC (rev 2548)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/test/pick_object.cpp 2008-08-05 07:46:49 UTC (rev 2549)
@@ -101,12 +101,12 @@
void close_gripper()
{
- myPR2.hw.CloseGripper(PR2::PR2_RIGHT_GRIPPER, 0.05, 500);
+ myPR2.hw.SetJointServoCmd(PR2::ARM_R_GRIPPER_GAP, 0.0, 500);
}
void open_gripper()
{
- myPR2.hw.CloseGripper(PR2::PR2_RIGHT_GRIPPER, 0.15, 500);
+ myPR2.hw.SetJointServoCmd(PR2::ARM_R_GRIPPER_GAP, 0.15, 500);
}
void go_down()
Modified: pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h 2008-08-05 07:01:19 UTC (rev 2548)
+++ pkg/trunk/drivers/robot/pr2/libpr2HW/include/libpr2HW/pr2HW.h 2008-08-05 07:46:49 UTC (rev 2549)
@@ -260,34 +260,6 @@
double* simTime);
/*! \fn
- \brief - Open gripper
- */
- public: PR2_ERROR_CODE OpenGripper(PR2_MODEL_ID id,double gap, double force);
-
- /*! \fn
- \brief - Close gripper
- */
- 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);
- /*!
- \brief - control mode for the arms, possible values are joint space control or cartesian space control
- */
-
- /*! \fn
\brief - Get camera data
*/
public: PR2_ERROR_CODE GetCameraImage(PR2_SENSOR_ID id ,
Modified: pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp 2008-08-05 07:01:19 UTC (rev 2548)
+++ pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp 2008-08-05 07:46:49 UTC (rev 2549)
@@ -34,8 +34,6 @@
gazebo::Client *client;
gazebo::SimulationIface *simIface;
gazebo::PR2ArrayIface *pr2Iface;
-gazebo::PR2GripperIface *pr2GripperLeftIface;
-gazebo::PR2GripperIface *pr2GripperRightIface;
gazebo::LaserIface *pr2LaserIface;
gazebo::LaserIface *pr2BaseLaserIface;
gazebo::CameraIface *pr2CameraGlobalIface;
@@ -79,8 +77,6 @@
client = new gazebo::Client();
simIface = new gazebo::SimulationIface();
pr2Iface = new gazebo::PR2ArrayIface();
- pr2GripperLeftIface = new gazebo::PR2GripperIface();
- pr2GripperRightIface = new gazebo::PR2GripperIface();
pr2LaserIface = new gazebo::LaserIface();
pr2BaseLaserIface = new gazebo::LaserIface();
pr2CameraGlobalIface = new gazebo::CameraIface();
@@ -162,7 +158,7 @@
/// Open the wrist and forearm cameras
try
{
- pr2WristCameraLeftIface->Open(client, "wrist_cam_left_iface");
+ pr2WristCameraLeftIface->Open(client, "wrist_cam_left_iface_1");
}
catch (std::string e)
{
@@ -173,7 +169,7 @@
try
{
- pr2WristCameraRightIface->Open(client, "wrist_cam_right_iface");
+ pr2WristCameraRightIface->Open(client, "wrist_cam_right_iface_1");
}
catch (std::string e)
{
@@ -204,30 +200,6 @@
pr2ForearmCameraRightIface = NULL;
}
- /// Open the Position interface for gripper left
- try
- {
- pr2GripperLeftIface->Open(client, "pr2_gripper_left_iface");
- }
- catch (std::string e)
- {
- std::cout << "Gazebo error: Unable to connect to the pr2 gripper left interface\n"
- << e << "\n";
- pr2GripperLeftIface = NULL;
- }
-
- /// Open the Position interface for gripper right
- try
- {
- pr2GripperRightIface->Open(client, "pr2_gripper_right_iface");
- }
- catch (std::string e)
- {
- std::cout << "Gazebo error: Unable to connect to the pr2 gripper right interface\n"
- << e << "\n";
- pr2GripperRightIface = NULL;
- }
-
/// Open the laser interface for hokuyo
try
{
@@ -336,51 +308,16 @@
// fill in actuator data
for (int id = 0; id < PR2::HEAD_PTZ_R_TILT; id++)
{
- if(IsGripperLeft((PR2::PR2_JOINT_ID)id))
- {
- if (pr2GripperLeftIface)
- {
- pr2GripperLeftIface->Lock(1);
- this->jointData[id].cmdEnableMotor = pr2GripperLeftIface->data->cmdEnableMotor ;
- this->jointData[id].controlMode = pr2GripperLeftIface->data->controlMode ;
- this->jointData[id].pGain = pr2GripperLeftIface->data->pGain ;
- this->jointData[id].iGain = pr2GripperLeftIface->data->iGain ;
- this->jointData[id].dGain = pr2GripperLeftIface->data->dGain ;
- this->jointData[id].cmdGap = pr2GripperLeftIface->data->cmdGap ;
- this->jointData[id].cmdEffectorForce = pr2GripperLeftIface->data->cmdForce ;
- this->jointData[id].cmdSpeed = pr2GripperLeftIface->data->cmdPositionRate ;
- pr2GripperLeftIface->Unlock();
- }
- }
- else if(IsGripperRight((PR2::PR2_JOINT_ID)id))
- {
- if (pr2GripperRightIface)
- {
- pr2GripperRightIface->Lock(1);
- this->jointData[id].cmdEnableMotor = pr2GripperRightIface->data->cmdEnableMotor ;
- this->jointData[id].controlMode = pr2GripperRightIface->data->controlMode ;
- this->jointData[id].pGain = pr2GripperRightIface->data->pGain ;
- this->jointData[id].iGain = pr2GripperRightIface->data->iGain ;
- this->jointData[id].dGain = pr2GripperRightIface->data->dGain ;
- this->jointData[id].cmdGap = pr2GripperRightIface->data->cmdGap ;
- this->jointData[id].cmdEffectorForce = pr2GripperRightIface->data->cmdForce ;
- this->jointData[id].cmdSpeed = pr2GripperRightIface->data->cmdPositionRate ;
- pr2GripperRightIface->Unlock();
- }
- }
- else // corresponds to Pr2_Actarray from CASTER to ARM_R_GRIPPER
- {
- pr2Iface->Lock(1);
- this->jointData[id].cmdEnableMotor = pr2Iface->data->actuators[id].cmdEnableMotor ;
- this->jointData[id].controlMode = pr2Iface->data->actuators[id].controlMode ;
- this->jointData[id].pGain = pr2Iface->data->actuators[id].pGain ;
- this->jointData[id].iGain = pr2Iface->data->actuators[id].iGain ;
- this->jointData[id].dGain = pr2Iface->data->actuators[id].dGain ;
- this->jointData[id].cmdPosition = pr2Iface->data->actuators[id].cmdPosition ;
- this->jointData[id].cmdSpeed = pr2Iface->data->actuators[id].cmdSpeed ;
- this->jointData[id].cmdEffectorForce = pr2Iface->data->actuators[id].cmdEffectorForce ;
- pr2Iface->Unlock();
- }
+ pr2Iface->Lock(1);
+ this->jointData[id].cmdEnableMotor = pr2Iface->data->actuators[id].cmdEnableMotor ;
+ this->jointData[id].controlMode = pr2Iface->data->actuators[id].controlMode ;
+ this->jointData[id].pGain = pr2Iface->data->actuators[id].pGain ;
+ this->jointData[id].iGain = pr2Iface->data->actuators[id].iGain ;
+ this->jointData[id].dGain = pr2Iface->data->actuators[id].dGain ;
+ this->jointData[id].cmdPosition = pr2Iface->data->actuators[id].cmdPosition ;
+ this->jointData[id].cmdSpeed = pr2Iface->data->actuators[id].cmdSpeed ;
+ this->jointData[id].cmdEffectorForce = pr2Iface->data->actuators[id].cmdEffectorForce ;
+ pr2Iface->Unlock();
}
@@ -655,103 +592,6 @@
}
};
-PR2_ERROR_CODE PR2HW::OpenGripper(PR2_MODEL_ID id,double gap,double force)
-{
- switch(id)
- {
- case PR2_LEFT_GRIPPER:
- case PR2_RIGHT_GRIPPER:
- for (int ii = JointStart[id];ii <= JointEnd[id];ii++)
- {
- this->jointData[ii].cmdMode = GAZEBO_PR2GRIPPER_CMD_OPEN;
- this->jointData[ii].cmdSpeed = 1.0;
- this->jointData[ii].cmdEffectorForce = force;
- this->jointData[ii].cmdGap = gap;
- }
- break;
- default:
- break;
- }
- return PR2_ALL_OK;
-};
-
-PR2_ERROR_CODE PR2HW::CloseGripper(PR2_MODEL_ID id,double gap,double force)
-{
- switch(id)
- {
- case PR2_LEFT_GRIPPER:
- case PR2_RIGHT_GRIPPER:
- std::cout << "JointStart " << JointStart[id] << " joint end " << JointEnd[id] << std::endl;
- for (int ii = JointStart[id];ii <= JointEnd[id];ii++)
- {
- this->jointData[ii].cmdMode = GAZEBO_PR2GRIPPER_CMD_OPEN;
- this->jointData[ii].cmdSpeed = 1.0;
- this->jointData[ii].cmdEffectorForce = force;
- this->jointData[ii].cmdGap = gap;
- std::cout << "Setting gap for " << ii << " " << gap << std::endl;
- }
- break;
- default:
- break;
- }
- return PR2_ALL_OK;
-};
-
-PR2_ERROR_CODE PR2HW::GetGripperCmd(PR2_MODEL_ID id,double *gap,double *force)
-{
- switch(id)
- {
- case PR2_LEFT_GRIPPER:
- case PR2_RIGHT_GRIPPER:
- for (int ii = JointStart[id];ii <= JointEnd[id];ii++)
- {
- *force = this->jointData[ii].cmdEffectorForce;
- *gap = this->jointData[ii].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:
- case PR2_RIGHT_GRIPPER:
- for (int ii = JointStart[id];ii <= JointEnd[id];ii++)
- {
- *force = this->jointData[ii].actualEffectorForce;
- *gap = this->jointData[ii].actualGap ;
- }
- break;
- default:
- break;
- }
- return PR2_ALL_OK;
-};
-
-PR2_ERROR_CODE PR2HW::SetGripperGains(PR2_MODEL_ID id,double p,double i, double d)
-{
- switch(id)
- {
- case PR2_LEFT_GRIPPER:
- case PR2_RIGHT_GRIPPER:
- for (int ii = JointStart[id];ii <= JointEnd[id];ii++)
- {
- this->jointData[ii].pGain = p;
- this->jointData[ii].iGain = i;
- this->jointData[ii].dGain = d;
- }
- break;
- default:
- break;
- }
- return PR2_ALL_OK;
-};
-
PR2_ERROR_CODE PR2HW::GetCameraImage(PR2_SENSOR_ID id ,
uint32_t* width ,uint32_t* height ,
uint32_t* depth ,
@@ -1061,56 +901,11 @@
// receive data from hardware
for (int id = 0; id < PR2::HEAD_PTZ_R_TILT; id++)
{
- if(IsGripperLeft((PR2::PR2_JOINT_ID)id))
- {
- if (pr2GripperLeftIface)
- {
- pr2GripperLeftIface->Lock(1);
- this->jointData[id].actualPosition = ( pr2GripperLeftIface->data->actualFingerPosition[0] - 0.015) // FIXME: not debugged
- +(-pr2GripperLeftIface->data->actualFingerPosition[1] + 0.015);
- this->jointData[id].actualGap = ( pr2GripperLeftIface->data->actualFingerPosition[0] - 0.015) // FIXME: not debugged
- +(-pr2GripperLeftIface->data->actualFingerPosition[1] + 0.015);
- this->jointData[id].actualSpeed = ( pr2GripperLeftIface->data->actualFingerPositionRate[0]) // FIXME: not debugged
- +(-pr2GripperLeftIface->data->actualFingerPositionRate[1]);
- this->jointData[id].actualSpeed = pr2GripperLeftIface->data->actualFingerPositionRate[0]; // FIXME: temporary numbers
-
- this->jointData[id].actualEffectorForce = pr2GripperLeftIface->data->gripperForce;
- this->jointData[id].actualFingerPosition[0] = pr2GripperLeftIface->data->actualFingerPosition[0];
- this->jointData[id].actualFingerPosition[1] = pr2GripperLeftIface->data->actualFingerPosition[1];
- this->jointData[id].actualFingerPositionRate[0] = pr2GripperLeftIface->data->actualFingerPositionRate[0];
- this->jointData[id].actualFingerPositionRate[1] = pr2GripperLeftIface->data->actualFingerPositionRate[1];
- pr2GripperLeftIface->Unlock();
- }
- }
- else if(IsGripperRight((PR2::PR2_JOINT_ID)id))
- {
- if (pr2GripperRightIface)
- {
- pr2GripperRightIface->Lock(1);
- this->jointData[id].actualPosition = ( pr2GripperRightIface->data->actualFingerPosition[0] - 0.015) // FIXME: not debugged
- +(-pr2GripperRightIface->data->actualFingerPosition[1] + 0.015);
- this->jointData[id].actualGap = ( pr2GripperRightIface->data->actualFingerPosition[0] - 0.015) // FIXME: not debugged
- +(-pr2GripperRightIface->data->actualFingerPosition[1] + 0.015);
- this->jointData[id].actualSpeed = ( pr2GripperRightIface->data->actualFingerPositionRate[0]) // FIXME: not debugged
- +(-pr2GripperRightIface->data->actualFingerPositionRate[1]);
- this->jointData[id].actualSpeed = pr2GripperRightIface->data->actualFingerPositionRate[0]; // FIXME: temporary numbers
-
- this->jointData[id].actualEffectorForce = pr2GripperRightIface->data->gripperForce;
- this->jointData[id].actualFingerPosition[0] = pr2GripperRightIface->data->actualFingerPosition[0];
- this->jointData[id].actualFingerPosition[1] = pr2GripperRightIface->data->actualFingerPosition[1];
- this->jointData[id].actualFingerPositionRate[0] = pr2GripperRightIface->data->actualFingerPositionRate[0];
- this->jointData[id].actualFingerPositionRate[1] = pr2GripperRightIface->data->actualFingerPositionRate[1];
- pr2GripperRightIface->Unlock();
- }
- }
- else // corresponds to Pr2_Actarray from CASTER to ARM_R_GRIPPER
- {
- pr2Iface->Lock(1);
- this->jointData[id].actualPosition = pr2Iface->data->actuators[id].actualPosition ;
- this->jointData[id].actualSpeed = pr2Iface->data->actuators[id].actualSpeed ;
- this->jointData[id].actualEffectorForce = pr2Iface->data->actuators[id].actualEffectorForce;
- pr2Iface->Unlock();
- }
+ pr2Iface->Lock(1);
+ this->jointData[id].actualPosition = pr2Iface->data->actuators[id].actualPosition ;
+ this->jointData[id].actualSpeed = pr2Iface->data->actuators[id].actualSpeed ;
+ this->jointData[id].actualEffectorForce = pr2Iface->data->actuators[id].actualEffectorForce;
+ pr2Iface->Unlock();
}
@@ -1118,51 +913,16 @@
// send commands to hardware
for (int id = 0; id < PR2::HEAD_PTZ_R_TILT; id++)
{
- if(IsGripperLeft((PR2::PR2_JOINT_ID)id))
- {
- if (pr2GripperLeftIface)
- {
- pr2GripperLeftIface->Lock(1);
- pr2GripperLeftIface->data->cmdEnableMotor = this->jointData[id].cmdEnableMotor ;
- pr2GripperLeftIface->data->controlMode = this->jointData[id].controlMode ;
- pr2GripperLeftIface->data->pGain = this->jointData[id].pGain ;
- pr2GripperLeftIface->data->iGain = this->jointData[id].iGain ;
- pr2GripperLeftIface->data->dGain = this->jointData[id].dGain ;
- pr2GripperLeftIface->data->cmdGap = this->jointData[id].cmdGap ;
- pr2GripperLeftIface->data->cmdForce = this->jointData[id].cmdEffectorForce;
- pr2GripperLeftIface->data->cmdPositionRate = this->jointData[id].cmdSpeed;
- pr2GripperLeftIface->Unlock();
- }
- }
- else if(IsGripperRight((PR2::PR2_JOINT_ID)id))
- {
- if (pr2GripperRightIface)
- {
- pr2GripperRightIface->Lock(1);
- pr2GripperRightIface->data->cmdEnableMotor = this->jointData[id].cmdEnableMotor ;
- pr2GripperRightIface->data->controlMode = this->jointData[id].controlMode ;
- pr2GripperRightIface->data->pGain = this->jointData[id].pGain ;
- pr2GripperRightIface->data->iGain = this->jointData[id].iGain ;
- pr2GripperRightIface->data->dGain = this->jointData[id].dGain ;
- pr2GripperRightIface->data->cmdGap = this->jointData[id].cmdGap ;
- pr2GripperRightIface->data->cmdForce = this->jointData[id].cmdEffectorForce;
- pr2GripperRightIface->data->cmdPositionRate = this->jointData[id].cmdSpeed;
- pr2GripperRightIface->Unlock();
- }
- }
- else // corresponds to Pr2_Actarray from CASTER to ARM_R_GRIPPER
- {
- pr2Iface->Lock(1);
- pr2Iface->data->actuators[id].cmdEnableMotor = this->jointData[id].cmdEnableMotor ;
- pr2Iface->data->actuators[id].controlMode = this->jointData[id].controlMode ;
- pr2Iface->data->actuators[id].pGain = this->jointData[id].pGain ;
- pr2Iface->data->actuators[id].iGain = this->jointData[id].iGain ;
- pr2Iface->data->actuators[id].dGain = this->jointData[id].dGain ;
- pr2Iface->data->actuators[id].cmdPosition = this->jointData[id].cmdPosition ;
- pr2Iface->data->actuators[id].cmdSpeed = this->jointData[id].cmdSpeed ;
- pr2Iface->data->actuators[id].cmdEffectorForce = this->jointData[id].cmdEffectorForce;
- pr2Iface->Unlock();
- }
+ pr2Iface->Lock(1);
+ pr2Iface->data->actuators[id].cmdEnableMotor = this->jointData[id].cmdEnableMotor ;
+ pr2Iface->data->actuators[id].controlMode = this->jointData[id].controlMode ;
+ pr2Iface->data->actuators[id].pGain = this->jointData[id].pGain ;
+ pr2Iface->data->actuators[id].iGain = this->jointData[id].iGain ;
+ pr2Iface->data->actuators[id].dGain = this->jointData[id].dGain ;
+ pr2Iface->data->actuators[id].cmdPosition = this->jointData[id].cmdPosition ;
+ pr2Iface->data->actuators[id].cmdSpeed = this->jointData[id].cmdSpeed ;
+ pr2Iface->data->actuators[id].cmdEffectorForce = this->jointData[id].cmdEffectorForce;
+ pr2Iface->Unlock();
}
@@ -1176,55 +936,11 @@
// receive data from hardware
for (int id = 0; id < PR2::HEAD_PTZ_R_TILT; id++)
{
- if(IsGripperLeft((PR2::PR2_JOINT_ID)id))
- {
- /* //Don't look at grippers for now
- pr2GripperLeftIface->Lock(1);
- this->jointArray[id].actualPosition = ( pr2GripperLeftIface->data->actualFingerPosition[0] - 0.015) // FIXME: not debugged
- +(-pr2GripperLeftIface->data->actualFingerPosition[1] + 0.015);
- this->jointData[id].actualGap = ( pr2GripperLeftIface->data->actualFingerPosition[0] - 0.015) // FIXME: not debugged
- +(-pr2GripperLeftIface->data->actualFingerPosition[1] + 0.015);
- this->jointData[id].actualSpeed = ( pr2GripperLeftIface->data->actualFingerPositionRate[0]) // FIXME: not debugged
- +(-pr2GripperLeftIface->data->actualFingerPositionRate[1]);
- this->jointData[id].actualSpeed = pr2GripperLeftIface->data->actualFingerPositionRate[0]; // FIXME: temporary numbers
-
- this->jointData[id].actualEffectorForce = pr2GripperLeftIface->data->gripperForce;
- this->jointData[id].actualFingerPosition[0] = pr2GripperLeftIface->data->actualFingerPosition[0];
- this->jointData[id].actualFingerPosition[1] = pr2GripperLeftIface->data->actualFingerPosition[1];
- this->jointData[id].actualFingerPositionRate[0] = pr2GripperLeftIface->data->actualFingerPositionRate[0];
- this->jointData[id].actualFingerPositionRate[1] = pr2GripperLeftIface->data->actualFingerPositionRate[1];
-
- pr2GripperLeftIface->Unlock();
- */
- }
- else if(IsGripperRight((PR2::PR2_JOINT_ID)id))
- {
- /*
- pr2GripperRightIface->Lock(1);
- this->jointData[id].actualPosition = ( pr2GripperRightIface->data->actualFingerPosition[0] - 0.015) // FIXME: not debugged
- +(-pr2GripperRightIface->data->actualFingerPosition[1] + 0.015);
- this->jointData[id].actualGap = ( pr2GripperRightIface->data->actualFingerPosition[0] - 0.015) // FIXME: not debugged
- +(-pr2GripperRightIface->data->actualFingerPosition[1] + 0.015);
- this->jointData[id].actualSpeed = ( pr2GripperRightIface->data->actualFingerPositionRate[0]) // FIXME: not debugged
- +(-pr2GripperRightIface->data->actualFingerPositionRate[1]);
- this->jointData[id].actualSpeed = pr2GripperRightIface->data->actualFingerPositionRate[0]; // FIXME: temporary numbers
-
- this->jointData[id].actualEffectorForce = pr2GripperRightIface->data->gripperForce;
- this->jointData[id].actualFingerPosition[0] = pr2GripperRightIface->data->actualFingerPosition[0];
- this->jointData[id].actualFingerPosition[1] = pr2GripperRightIface->data->actualFingerPosition[1];
- this->jointData[id].actualFingerPositionRate[0] = pr2GripperRightIface->data->actualFingerPositionRate[0];
- this->jointData[id].actualFingerPositionRate[1] = pr2GripperRightIface->data->actualFingerPositionRate[1];
- pr2GripperRightIface->Unlock();
- */
- }
- else // corresponds to Pr2_Actarray from CASTER to ARM_R_GRIPPER
- {
- pr2Iface->Lock(1);
- jointArray[id]->position = pr2Iface->data->actuators[id].actualPosition ;
- jointArray[id]->velocity = pr2Iface->data->actuators[id].actualSpeed ;
- jointArray[id]->appliedEffort = pr2Iface->data->actuators[id].actualEffectorForce;
- pr2Iface->Unlock();
- }
+ pr2Iface->Lock(1);
+ jointArray[id]->position = pr2Iface->data->actuators[id].actualPosition ;
+ jointArray[id]->velocity = pr2Iface->data->actuators[id].actualSpeed ;
+ jointArray[id]->appliedEffort = pr2Iface->data->actuators[id].actualEffectorForce;
+ pr2Iface->Unlock();
}
@@ -1232,51 +948,19 @@
// send commands to hardware
for (int id = 0; id < PR2::HEAD_PTZ_R_TILT; id++)
{
- if(IsGripperLeft((PR2::PR2_JOINT_ID)id))
- {
- /*
- pr2GripperLeftIface->Lock(1);
- pr2GripperLeftIface->data->cmdEnableMotor = this->jointData[id].cmdEnableMotor ;
- pr2GripperLeftIface->data->controlMode = this->jointData[id].controlMode ;
- pr2GripperLeftIface->data->pGain = this->jointData[id].pGain ;
- pr2GripperLeftIface->data->iGain = this->jointData[id].iGain ;
- pr2GripperLeftIface->data->dGain = this->jointData[id].dGain ;
- pr2GripperLeftIface->data->cmdGap = this->jointData[id].cmdGap ;
- pr2GripperLeftIface->data->cmdForce = this->jointData[id].cmdEffectorForce;
- pr2GripperLeftIface->data->cmdPositionRate = this->jointData[id].cmdSpeed;
- pr2GripperLeftIface->Unlock();
- */
- }
- else if(IsGripperRight((PR2::PR2_JOINT_ID)id))
- {
- /*
- pr2GripperRightIface->Lock(1);
- pr2GripperRightIface->data->cmdEnableMotor = this->jointData[id].cmdEnableMotor ;
- pr2GripperRightIface->data->controlMode = this->jointData[id].controlMode ;
- pr2GripperRightIface->data->pGain = this->jointData[id].pGain ;
- pr2GripperRightIface->data->iGain = this->jointData[id].iGain ;
- pr2GripperRightIface->data->dGain = this->jointData[id].dGain ;
- pr2GripperRightIface->data->cmdGap = this->jointData[id].cmdGap ;
- pr2GripperRightIface->data->cmdForce = this->jointData[id].cmdEffectorForce;
- pr2GripperRightIface->data->cmdPositionRate = this->jointData[id].cmdSpeed;
- pr2GripperRightIface->Unlock();
- */
- }
- else // corresponds to Pr2_Actarray from CASTER to ARM_R_GRIPPER
- {
- pr2Iface->Lock(1);
- pr2Iface->data->actuators[id].cmdEnableMotor = jointArray[id]->initialized ;
- /*
- pr2Iface->data->actuators[id].controlMode = this->jointData[id].controlMode ;
- pr2Iface->data->actuators[id].pGain = this->jointData[id].pGain ;
- pr2Iface->data->actuators[id].iGain = this->jointData[id].iGain ;
- pr2Iface->data->actuators[id].dGain = this->jointData[id].dGain ;
- pr2Iface->data->actuators[id].cmdPosition = this->jointData[id].cmdPosition ;
- pr2Iface->data->actuators[id].cmdSpeed = this->jointData[id].cmdSpeed ;
- */
- pr2Iface->data->actuators[id].cmdEffectorForce = jointArray[id]->commandedEffort;
- pr2Iface->Unlock();
- }
+
+ pr2Iface->Lock(1);
+ pr2Iface->data->actuators[id].cmdEnableMotor = jointArray[id]->initialized ;
+ /*
+ pr2Iface->data->actuators[id].controlMode = this->jointData[id].controlMode ;
+ pr2Iface->data->actuators[id].pGain = this->jointData[id].pGain ;
+ pr2Iface->data->actuators[id].iGain = this->jointData[id].iGain ;
+ pr2Iface->data->actuators[id].dGain = this->jointData[id].dGain ;
+ pr2Iface->data->actuators[id].cmdPosition = this->jointData[id].cmdPosition ;
+ pr2Iface->data->actuators[id].cmdSpeed = this->jointData[id].cmdSpeed ;
+ */
+ pr2Iface->data->actuators[id].cmdEffectorForce = jointArray[id]->commandedEffort;
+ pr2Iface->Unlock();
}
Modified: pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h 2008-08-05 07:01:19 UTC (rev 2548)
+++ pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h 2008-08-05 07:46:49 UTC (rev 2549)
@@ -104,8 +104,6 @@
HEAD_YAW ,
HEAD_PITCH ,
HEAD_LASER_PITCH ,
- ARM_L_GRIPPER ,
- ARM_R_GRIPPER ,
HEAD_PTZ_L_PAN ,
HEAD_PTZ_L_TILT ,
HEAD_PTZ_R_PAN ,
@@ -299,9 +297,9 @@
};
// JointStart/JointEnd corresponds to the PR2_MODEL_ID, start and end id for each model
- enum PR2_MODEL_ID {PR2_BASE ,PR2_SPINE ,PR2_LEFT_ARM ,PR2_RIGHT_ARM ,PR2_LEFT_GRIPPER ,PR2_RIGHT_GRIPPER ,HEAD ,PR2_LEFT_PTZ ,PR2_RIGHT_PTZ , MAX_MODELS };
- const int JointStart[MAX_MODELS] = {CASTER_FL_STEER ,SPINE_ELEVATOR ,ARM_L_PAN ,ARM_R_PAN ,ARM_L_GRIPPER ,ARM_R_GRIPPER ,HEAD_YAW ,PR2_LEFT_PTZ ,PR2_RIGHT_PTZ };
- const int JointEnd[MAX_MODELS] = {CASTER_RR_DRIVE_R ,SPINE_ELEVATOR ,ARM_L_GRIPPER_GAP,ARM_R_GRIPPER_GAP,ARM_L_GRIPPER ,ARM_R_GRIPPER ,HEAD_LASER_PITCH ,PR2_LEFT_PTZ ,PR2_RIGHT_PTZ };
+ enum PR2_MODEL_ID {PR2_BASE ,PR2_SPINE ,PR2_LEFT_ARM ,PR2_RIGHT_ARM ,HEAD ,PR2_LEFT_PTZ ,PR2_RIGHT_PTZ , MAX_MODELS };
+ const int JointStart[MAX_MODELS] = {CASTER_FL_STEER ,SPINE_ELEVATOR ,ARM_L_PAN ,ARM_R_PAN ,HEAD_YAW ,PR2_LEFT_PTZ ,PR2_RIGHT_PTZ };
+ const int JointEnd[MAX_MODELS] = {CASTER_RR_DRIVE_R ,SPINE_ELEVATOR ,ARM_L_GRIPPER_GAP,ARM_R_GRIPPER_GAP,HEAD_LASER_PITCH ,PR2_LEFT_PTZ ,PR2_RIGHT_PTZ };
// Geometric description for the base
Modified: pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Misc.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Misc.h 2008-08-05 07:01:19 UTC (rev 2548)
+++ pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Misc.h 2008-08-05 07:46:49 UTC (rev 2549)
@@ -37,25 +37,6 @@
return q;
}
- inline bool IsHead(PR2_MODEL_ID id)
- {
- if(id == HEAD)
- return true;
- return false;
- }
- inline bool IsGripperLeft(PR2_MODEL_ID id)
- {
- if(id == PR2_LEFT_GRIPPER)
- return true;
- return false;
- }
- inline bool IsGripperRight(PR2_MODEL_ID id)
- {
- if(id == PR2_RIGHT_GRIPPER)
- return true;
- return false;
- }
-
inline bool IsPTZLeft(PR2_MODEL_ID id)
{
if(id == PR2_LEFT_PTZ)
@@ -70,26 +51,6 @@
return false;
}
- inline bool IsHead(PR2_JOINT_ID id)
- {
- if(id >= JointStart[HEAD] && id <= JointEnd[HEAD])
- return true;
- return false;
- }
-
- inline bool IsGripperLeft(PR2_JOINT_ID id)
- {
- if (id >= JointStart[PR2_LEFT_GRIPPER] && id <= JointEnd[PR2_LEFT_GRIPPER])
- return true;
- return false;
- }
- inline bool IsGripperRight(PR2_JOINT_ID id)
- {
- if (id >= JointStart[PR2_RIGHT_GRIPPER] && id <= JointEnd[PR2_RIGHT_GRIPPER])
- return true;
- return false;
- }
-
inline bool IsPTZRight(PR2_JOINT_ID id)
{
if (id >= JointStart[PR2_RIGHT_PTZ] && id <= JointEnd[PR2_RIGHT_PTZ])
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Camera.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Camera.hh 2008-08-05 07:01:19 UTC (rev 2548)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Camera.hh 2008-08-05 07:46:49 UTC (rev 2549)
@@ -109,8 +109,12 @@
// topic name
private: std::string topicName;
+ // A mutex to lock access to fields that are used in message callbacks
+ private: ros::thread::mutex lock;
+
+
};
/** /} */
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Camera.cc
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Camera.cc 2008-08-05 07:01:19 UTC (rev 2548)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Camera.cc 2008-08-05 07:46:49 UTC (rev 2549)
@@ -164,6 +164,7 @@
// Make sure there is room to store the image
assert (data->image_size <= sizeof(data->image));
+ this->lock.lock();
// Copy the pixel data to the interface
src = this->myParent->GetImageData(0);
dst = data->image;
@@ -195,6 +196,6 @@
// publish to ros
rosnode->publish(this->topicName,this->image);
-
+ this->lock.unlock();
}
Modified: pkg/trunk/manip/teleop_arm_keyboard/teleop_arm_keyboard.cc
===================================================================
--- pkg/trunk/manip/teleop_arm_keyboard/teleop_arm_keyboard.cc 2008-08-05 07:01:19 UTC (rev 2548)
+++ pkg/trunk/manip/teleop_arm_keyboard/teleop_arm_keyboard.cc 2008-08-05 07:46:49 UTC (rev 2549)
@@ -253,12 +253,12 @@
}
void TArmK_Node::openGripper(PR2_JOINT_ID jointID) {
- if(jointID != ARM_R_GRIPPER && jointID != ARM_L_GRIPPER) return;
+ if(jointID != ARM_R_GRIPPER_GAP && jointID != ARM_L_GRIPPER_GAP) return;
if(_leftInit == false || _rightInit == false) {
printf("No init, so not sending command.\n");
return;
}
- if(jointID == ARM_R_GRIPPER) {
+ if(jointID == ARM_R_GRIPPER_GAP) {
this->cmd_rightarmconfig.gripperForceCmd = 50;
this->cmd_rightarmconfig.gripperGapCmd = .2;
printf("Opening right gripper\n");
@@ -270,12 +270,12 @@
}
void TArmK_Node::closeGripper(PR2_JOINT_ID jointID) {
- if(jointID != ARM_R_GRIPPER && jointID != ARM_L_GRIPPER) return;
+ if(jointID != ARM_R_GRIPPER_GAP && jointID != ARM_L_GRIPPER_GAP) return;
if(_leftInit == false || _rightInit == false) {
printf("No init, so not sending command.\n");
return;
}
- if(jointID == ARM_R_GRIPPER) {
+ if(jointID == ARM_R_GRIPPER_GAP) {
this->cmd_rightarmconfig.gripperForceCmd = 50;
this->cmd_rightarmconfig.gripperGapCmd = 0;
} else {
@@ -325,7 +325,7 @@
case ARM_L_WRIST_ROLL:
this->cmd_leftarmconfig.wristRollAngle += jointCmdStep;
break;
- case ARM_L_GRIPPER:
+ case ARM_L_GRIPPER_GAP:
this->cmd_leftarmconfig.gripperGapCmd += gripperStep;
break;
case ARM_R_PAN:
@@ -349,7 +349,7 @@
case ARM_R_WRIST_ROLL:
this->cmd_rightarmconfig.wristRollAngle += jointCmdStep;
break;
- case ARM_R_GRIPPER:
+ case ARM_R_GRIPPER_GAP:
this->cmd_rightarmconfig.gripperGapCmd += gripperStep;
break;
default:
@@ -473,7 +473,7 @@
printf("left wrist roll\n");
break;
case '8':
- curr_jointID = ARM_L_GRIPPER;
+ curr_jointID = ARM_L_GRIPPER_GAP;
printf("left gripper\n");
break;
case '9':
@@ -516,7 +516,7 @@
printf("right wrist roll\n");
break;
case '8':
- curr_jointID = ARM_R_GRIPPER;
+ curr_jointID = ARM_R_GRIPPER_GAP;
printf("right gripper\n");
break;
case '9':
Modified: pkg/trunk/robot_control_loops/pr2_gazebo/src/RosGazeboControlsNode.cpp
===================================================================
--- pkg/trunk/robot_control_loops/pr2_gazebo/src/RosGazeboControlsNode.cpp 2008-08-05 07:01:19 UTC (rev 2548)
+++ pkg/trunk/robot_control_loops/pr2_gazebo/src/RosGazeboControlsNode.cpp 2008-08-05 07:46:49 UTC (rev 2549)
@@ -56,12 +56,9 @@
this->PR2Copy->hw.SetJointServoCmd(PR2::ARM_R_ELBOW_ROLL , this->rightarm.forearmRollAngle, 0);
this->PR2Copy->hw.SetJointServoCmd(PR2::ARM_R_WRIST_PITCH , this->rightarm.wristPitchAngle, 0);
this->PR2Copy->hw.SetJointServoCmd(PR2::ARM_R_WRIST_ROLL , this->rightarm.wristRollAngle, 0);
- this->PR2Copy->hw.SetJointServoCmd(PR2::ARM_R_GRIPPER , this->rightarm.gripperGapCmd, 0);
+ this->PR2Copy->hw.SetJointServoCmd(PR2::ARM_R_GRIPPER_GAP , this->rightarm.gripperGapCmd, 0);
}
- //Leave a way to communicate with the grippers
- this->PR2Copy->hw.CloseGripper(PR2::PR2_RIGHT_GRIPPER, this->rightarm.gripperGapCmd, this->rightarm.gripperForceCmd);
-
//*/
this->lock.unlock();
}
@@ -113,13 +110,9 @@
this->PR2Copy->hw.SetJointServoCmd(PR2::ARM_L_ELBOW_ROLL , this->leftarm.forearmRollAngle, 0);
this->PR2Copy->hw.SetJointServoCmd(PR2::ARM_L_WRIST_PITCH , this->leftarm.wristPitchAngle, 0);
this->PR2Copy->hw.SetJointServoCmd(PR2::ARM_L_WRIST_ROLL , this->leftarm.wristRollAngle, 0);
- this->PR2Copy->hw.SetJointServoCmd(PR2::ARM_L_GRIPPER , this->leftarm.gripperGapCmd, 0);
+ this->PR2Copy->hw.SetJointServoCmd(PR2::ARM_L_GRIPPER_GAP , this->leftarm.gripperGapCmd, 0);
}
- //Leave a way to communicate with the grippers
- this->PR2Copy->hw.CloseGripper(PR2::PR2_LEFT_GRIPPER, this->leftarm.gripperGapCmd, this->leftarm.gripperForceCmd);
-
-
this->lock.unlock();
}
@@ -857,12 +850,6 @@
// this->arm.wristRollAngle = 0.0;
// this->arm.gripperForceCmd = 1000.0;
// this->arm.gripperGapCmd = 0.0;
- //
- // // gripper test
- // this->PR2Copy->SetGripperGains(PR2::PR2_LEFT_GRIPPER ,10.0,0.0,0.0);
- // this->PR2Copy->SetGripperGains(PR2::PR2_RIGHT_GRIPPER ,10.0,0.0,0.0);
- // this->PR2Copy->OpenGripper(PR2::PR2_LEFT_GRIPPER ,this->arm.gripperGapCmd,this->arm.gripperForceCmd);
- // this->PR2Copy->CloseGripper(PR2::PR2_RIGHT_GRIPPER,this->arm.gripperGapCmd,this->arm.gripperForceCmd);
/***************************************************************/
/* */
Modified: pkg/trunk/robot_control_loops/pr2_gazebo/src/pr2_gazebo.cpp
===================================================================
--- pkg/trunk/robot_control_loops/pr2_gazebo/src/pr2_gazebo.cpp 2008-08-05 07:01:19 UTC (rev 2548)
+++ pkg/trunk/robot_control_loops/pr2_gazebo/src/pr2_gazebo.cpp 2008-08-05 07:46:49 UTC (rev 2549)
@@ -83,30 +83,7 @@
myPR2->InitializeRobot();
// Set control mode for the base
myPR2->SetBaseControlMode(PR2::PR2_CARTESIAN_CONTROL);
- // myPR2->SetJointControlMode(PR2::CASTER_FL_STEER, PR2::TORQUE_CONTROL);
- // myPR2->SetJointControlMode(PR2::CASTER_FR_STEER, PR2::TORQUE_CONTROL);
- // myPR2->SetJointControlMode(PR2::CASTER_RL_STEER, PR2::TORQUE_CONTROL);
- // myPR2->SetJointControlMode(PR2::CASTER_RR_STEER, PR2::TORQUE_CONTROL);
- myPR2->EnableGripperLeft();
- myPR2->EnableGripperRight();
-
- // Set control mode for the arms
- // FIXME: right now this just sets default to pd control
- //myPR2->SetArmControlMode(PR2::PR2_RIGHT_ARM, PR2::PR2_JOINT_CONTROL);
- //myPR2->SetArmControlMode(PR2::PR2_LEFT_ARM, PR2::PR2_JOINT_CONTROL);
- //------------------------------------------------------------
-
- // set torques for driving the robot wheels
- // myPR2->hw.SetJointTorque(PR2::CASTER_FL_DRIVE_L, 1000.0 );
- // myPR2->hw.SetJointTorque(PR2::CASTER_FR_DRIVE_L, 1000.0 );
- // myPR2->hw.SetJointTorque(PR2::CASTER_RL_DRIVE_L, 1000.0 );
- // myPR2->hw.SetJointTorque(PR2::CASTER_RR_DRIVE_L, 1000.0 );
- // myPR2->hw.SetJointTorque(PR2::CASTER_FL_DRIVE_R, 1000.0 );
- // myPR2->hw.SetJointTorque(PR2::CASTER_FR_DRIVE_R, 1000.0 );
- // myPR2->hw.SetJointTorque(PR2::CASTER_RL_DRIVE_R, 1000.0 );
- // myPR2->hw.SetJointTorque(PR2::CASTER_RR_DRIVE_R, 1000.0 );
-
/***************************************************************************************/
/* */
/* build actuators from pr2Actuators.xml */
@@ -142,6 +119,9 @@
signal(SIGQUIT, (&finalize));
signal(SIGTERM, (&finalize));
+ // let rosgazebonode read the xml data from pr2.xml in ros
+ rgn.LoadRobotModel();
+
// see if we can subscribe models needed
if (rgn.AdvertiseSubscribeMessages() != 0)
exit(-1);
Modified: pkg/trunk/robot_control_loops/pr2_gazebo/src/test2.cpp
===================================================================
--- pkg/trunk/robot_control_loops/pr2_gazebo/src/test2.cpp 2008-08-05 07:01:19 UTC (rev 2548)
+++ pkg/trunk/robot_control_loops/pr2_gazebo/src/test2.cpp 2008-08-05 07:46:49 UTC (rev 2549)
@@ -135,8 +135,6 @@
PR2::ARM_R_ELBOW_PITCH , PR2::ARM_R_ELBOW_ROLL , PR2::ARM_R_WRIST_PITCH ,
PR2::ARM_R_WRIST_ROLL , PR2::ARM_R_GRIPPER_GAP ,
PR2::HEAD_YAW , PR2::HEAD_PITCH , PR2::HEAD_LASER_PITCH ,
- PR2::ARM_L_GRIPPER ,
- PR2::ARM_R_GRIPPER ,
PR2::HEAD_PTZ_L_PAN - PR2::HEAD_PTZ_L_PAN ,
PR2::HEAD_PTZ_L_TILT - PR2::HEAD_PTZ_L_PAN ,
PR2::HEAD_PTZ_R_PAN - PR2::HEAD_PTZ_R_PAN ,
@@ -156,8 +154,6 @@
PR2::ARM_R_ELBOW_PITCH , PR2::ARM_R_ELBOW_ROLL , PR2::ARM_R_WRIST_PITCH ,
PR2::ARM_R_WRIST_ROLL , PR2::ARM_R_GRIPPER_GAP ,
PR2::HEAD_YAW , PR2::HEAD_PITCH , PR2::HEAD_LASER_PITCH ,
- PR2::ARM_L_GRIPPER ,
- PR2::ARM_R_GRIPPER ,
PR2::HEAD_PTZ_L_PAN ,
PR2::HEAD_PTZ_L_TILT ,
PR2::HEAD_PTZ_R_PAN ,
Modified: pkg/trunk/robot_control_loops/pr2_gazebo/src/test3.cpp
===================================================================
--- pkg/trunk/robot_control_loops/pr2_gazebo/src/test3.cpp 2008-08-05 07:01:19 UTC (rev 2548)
+++ pkg/trunk/robot_control_loops/pr2_gazebo/src/test3.cpp 2008-08-05 07:46:49 UTC (rev 2549)
@@ -122,9 +122,6 @@
// myPR2->SetJointControlMode(PR2::CASTER_RL_STEER, PR2::TORQUE_CONTROL);
// myPR2->SetJointControlMode(PR2::CASTER_RR_STEER, PR2::TORQUE_CONTROL);
- myPR2->EnableGripperLeft();
- myPR2->EnableGripperRight();
-
// Set control mode for the arms
// FIXME: right now this just sets default to pd control
//myPR2->SetArmControlMode(PR2::PR2_RIGHT_ARM, PR2::PR2_JOINT_CONTROL);
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml 2008-08-05 07:01:19 UTC (rev 2548)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml 2008-08-05 07:46:49 UTC (rev 2549)
@@ -6,83 +6,10 @@
<xml> <!-- The key attribute is needed noly if multiple <xml> tags are in the same <data> tag -->
- <!-- ========== Wrist Cameras =========== -->
- <body:empty name="wrist_cam_left_body">
- <xyz> 0.85 0.17 0.78 </xyz>
- <rpy> 0.00 0.0 0.00 </rpy>
- <sensor:camera name="wrist_cam_left_sensor">
- <imageSize>640 480</imageSize>
- <hfov>60</hfov>
- <nearClip>0.1</nearClip>
- <farClip>100</farClip>
- <updateRate>15.0</updateRate>
- <controller:generic_camera name="wrist_cam_left_controller">
- <updateRate>15.0</updateRate>
- <interface:camera name="wrist_cam_left_iface" />
- </controller:generic_camera>
- </sensor:camera>
- <geom:box name="wrist_cam_left_geom">
- <xyz> 0.00 0.00 0.00</xyz>
- <size>0.02 0.02 0.02</size>
- <mass>0.1</mass>
- <visual>
- <size>0.02 0.02 0.02</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/Black</material>
- </visual>
- </geom:box>
- </body:empty>
- <joint:hinge name="wrist_cam_left">
- <body1>wrist_cam_left_body</body1>
- <body2>gripper_roll_left</body2>
- <anchor>gripper_roll_left</anchor>
- <axis> 0.0 0.0 1.0 </axis>
- <lowStop> 0.0 </lowStop>
- <highStop> 0.0 </highStop>
- </joint:hinge>
- <body:empty name="wrist_cam_right_body">
- <xyz> 0.85 -0.17 0.78 </xyz>
- <rpy> 0.0 0.0 0.0 </rpy>
- <sensor:camera name="wrist_cam_right_sensor">
- <imageSize>640 480</imageSize>
- <hfov>60</hfov>
- <nearClip>0.1</nearClip>
- <farClip>100</farClip>
- <updateRate>15.0</updateRate>
- <controller:generic_camera name="wrist_cam_right_controller">
- <updateRate>15.0</updateRate>
- <interface:camera name="wrist_cam_right_iface" />
- </controller:generic_camera>
- </sensor:camera>
- <geom:box name="wrist_cam_right_geom">
- <xyz> 0.00 0.00 0.00</xyz>
- <size>0.02 0.02 0.02</size>
- <mass>0.1</mass>
- <visual>
- <size>0.02 0.02 0.02</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/Black</material>
- </visual>
- </geom:box>
- </body:empty>
- <joint:hinge name="wrist_cam_right">
- <body1>wrist_cam_right_body</body1>
- <body2>gripper_roll_right</body2>
- <anchor>gripper_roll_right</anchor>
- <axis> 0.0 0.0 1.0 </axis>
- <lowStop> 0.0 </lowStop>
- <highStop> 0.0 </highStop>
- </joint:hinge>
-
-
-
-
<!-- ptz left camera -->
<body:box name="ptz_cam_left_body">
<xyz> -0.1 0.30 1.04 </xyz>
@@ -201,7 +128,23 @@
<axis> 0.0 1.0 0.0 </axis>
</joint:hinge>
+ <!-- ptz camera controls -->
+ <controller:generic_ptz name="ptz_cam_left_controller">
+ <updateRate>15.0</updateRate> <!-- rate not implemented in gazebo yet-->
+ <panJoint>ptz_cam_left_pitch_joint</panJoint>
+ <tiltJoint>ptz_cam_left_yaw_joint</tiltJoint>
+ <interface:ptz name="ptz_left_iface" />
+ </controller:generic_ptz>
+ <controller:generic_ptz name="ptz_cam_right_controller">
+ <updateRate>15.0</updateRate> <!-- rate not implemented in gazebo yet-->
+ <panJoint>ptz_cam_right_pitch_joint</panJoint>
+ <tiltJoint>ptz_cam_right_yaw_joint</tiltJoint>
+ <interface:ptz name="ptz_right_iface" />
+ </controller:generic_ptz>
+
+
+
<!-- stereo camera -->
<body:empty name="stereo_camera_body">
<xyz> 0.03 0.0 1.30 </xyz>
@@ -247,102 +190,7 @@
<highStop> 0.0 </highStop>
</joint:hinge>
- <!-- ========================================== Hokuyo tilt laser ========================================== -->
- <body:box name="tilt_laser_body">
- <xyz>0.03 0.0 1.0</xyz>
- <rpy>0.0 0.0 0.0</rpy>
-
- <geom:box name="tilt_laser_box">
- <xyz>0.0 0.0 0.0</xyz>
- <rpy>0 0 0</rpy>
- <mass>0.12</mass>
- <size>.1 .1 .1</size>
- <visual>
- <xyz>0.0 0.0 0.0</xyz>
- <scale>.5 .5 .5</scale>
- <mesh>pr2/hokuyo.mesh</mesh>
- <material>Gazebo/Blue</material>
- </visual>
- </geom:box>
-
- <sensor:ray name="tilt_laser_1">
- <rayCount>683</rayCount>
- <rangeCount>683</rangeCount>
- <laserCount>1</laserCount>
- <origin>0.0 0.0 0.05</origin>
-
- <displayRays>false</displayRays>
-
- <minAngle>-45</minAngle>
- <maxAngle> 45</maxAngle>
-
- <minRange>0.05</minRange>
- <maxRange>10.0</maxRange>
- <updateRate>10</updateRate>
- <controller:sicklms200_laser name="tilt_laser_controller_1">
- <interface:laser name="tilt_laser_iface_1"/>
- <updateRate>10</updateRate>
- </controller:sicklms200_laser>
- </sensor:ray>
- </body:box>
-
- <!-- attach hokuyo to torso -->
- <joint:hinge name="hokuyo_pitch_joint">
- <body1>torso</body1>
- <body2>tilt_laser_body</body2>
- <anchor>tilt_laser_body</anchor>
- <axis> 0.0 1.0 0.0 </axis>
- </joint:hinge>
-
- <!-- ========================================== Hokuyo laser body ========================================== -->
- <body:box name="base_laser_body">
- <xyz>0.25 0.0 0.27</xyz> <!-- from the robot coordinate system (center bottom of base box)-->
- <rpy>0.0 0.0 0.0</rpy>
-
- <geom:box name="base_laser_box">
- <xyz>0.0 0.0 0.02</xyz>
- <rpy>0 0 0</rpy>
- <mass>0.12</mass>
- <size>.05 .05 .05</size>
- <visual>
- <scale>.05 .05 .05</scale>
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Red</material>
- </visual>
- </geom:box>
- <sensor:ray name="base_laser_1">
- <rayCount>683</rayCount>
- <rangeCount>683</rangeCount>
- <laserCount>1</laserCount>
- <origin>0.0 0.0 0.05</origin>
-
- <displayRays>false</displayRays>
-
- <minAngle>-90</minAngle>
- <maxAngle>90</maxAngle>
-
- <minRange>0.05</minRange>
- <maxRange>10.0</maxRange>
- <updateRate>10.0</updateRate>
- <controller:sicklms200_laser name="base_laser_controller_1">
- <interface:laser name="base_laser_iface_1"/>
- <updateRate>10</updateRate>
- </controller:sicklms200_laser>
- </sensor:ray>
- </body:box>
- <!-- attach hokuyo to torso -->
- <joint:hinge name="base_laser_base_attach_joint">
- <body1>base</body1>
- <body2>base_laser_body</body2>
- <anchor>base_laser_body</anchor>
- <axis> 1.0 0.0 0.0 </axis>
- <lowStop> 0.0 </lowStop>
- <highStop> 0.0 </highStop>
- </joint:hinge>
-
-
-
<!-- PR2_ACTARRAY -->
<controller:pr2_actarray name="pr2_controller" plugin="libPr2_Actarray.so">
<updateRate>100.0</updateRate>
@@ -643,7 +491,7 @@
<explicitDampingCoefficient>1.0</explicitDampingCoefficient>
<controlMode>PD_CONTROL</controlMode>
</joint>
- <joint name="hokuyo_pitch_joint">
+ <joint name="tilt_laser_joint">
<saturationTorque>500</saturationTorque>
<pGain>2</pGain>
<dGain>0</dGain>
@@ -675,140 +523,7 @@
<interface:position name="p3d_base_position"/>
</controller:P3D>
- <!-- ptz camera controls -->
- <controller:generic_ptz name="ptz_cam_left_controller">
- <updateRate>15.0</updateRate> <!-- rate not implemented in gazebo yet-->
- <panJoint>ptz_cam_left_pitch_joint</panJoint>
- <tiltJoint>ptz_cam_left_yaw_joint</tiltJoint>
- <interface:ptz name="ptz_left_iface" />
- </controller:generic_ptz>
- <controller:generic_ptz name="ptz_cam_right_controller">
- <updateRate>15.0</updateRate> <!-- rate not implemented in gazebo yet-->
- <panJoint>ptz_cam_right_pitch_joint</panJoint>
- <tiltJoint>ptz_cam_right_yaw_joint</tiltJoint>
- <interface:ptz name="ptz_right_iface" />
- </controller:generic_ptz>
-
-
-
- <!-- ========== Old Obsolete =========== -->
- <!-- ========== Right hand =========== -->
- <!--
- <body:box name="finger_1_right">
- <xyz>0.93 -0.172936 0.72</xyz>
- <rpy> 0.0 0.0 0.0 </rpy>
- <geom:box name="finger_1_right_geom">
- <kp>100000000.0</kp>
- <kd>1.0</kd>
- <size> 0.05 0.01 0.025</size>
- <mass>0.1</mass>
- <visual>
- <size> 0.05 0.01 0.025</size>
- <mesh>unit_box</mesh>
- <material>Gazebo/PioneerBody</material>
- </visual>
- </geom:box>
- </body:box>
- <body:box name="finger_2_right">
- <xyz>0.93 -0.132936 0.72</xyz>
- <rpy> 0.0 0.0 0.0 </rpy>
- <geom:box name="finger_2_right_geom">
- <kp>100000000.0</kp>
- <kd>1.0</kd>
- <size> 0.05 0.01 0.025</size>
- <mass>0.1</mass>
- <visual>
- <size> 0.05 0.01 0.025</size>
- <mesh>unit_box</mesh>
- <material>Gazebo/Blue</material>
- </visual>
- </geom:box>
- </body:box>
- <joint:slider name="finger_1_slider_right">
- <body2>finger_1_right</body2>
- <body1>gripper_roll_right</body1>
- <anchor>gripper_roll_right</anchor>
- <lowStop> -0.02 </lowStop>
- <highStop> 0.05 </highStop>
- <axis>0.0 1.0 0.0 </axis>
- </joint:slider>
- <joint:slider name="finger_2_slider_right">
- <body2>finger_2_right</body2>
- <body1>gripper_roll_right</body1>
- <anchor>gripper_roll_right</anchor>
- <lowStop> -0.05 </lowStop>
- <highStop> 0.02 </highStop>
- <axis>0.0 1.0 0.0 </axis>
- </joint:slider>
- <body:box name="finger_1_left">
- <xyz>0.93 0.132936 0.72</xyz>
- <rpy> 0.0 0.0 0.0 </rpy>
- <geom:box name="finger_1_left_geom">
- <kp>100000000.0</kp>
- <kd>1.0</kd>
- <size> 0.05 0.01 0.025</size>
- <mass>0.1</mass>
- <visual>
- <size> 0.05 0.01 0.025</size>
- <mesh>unit_box</mesh>
- <material>Gazebo/PioneerBody</material>
- </visual>
- </geom:box>
- </body:box>
-
- <body:box name="finger_2_left">
- <xyz>0.93 0.172936 0.72</xyz>
- <rpy> 0.0 0.0 0.0 </rpy>
- <geom:box name="finger_2_left_geom">
- <kp>100000000.0</kp>
- <kd>1.0</kd>
- <size> 0.05 0.01 0.025</size>
- <mass>0.1</mass>
- <visual>
- <size> 0.05 0.01 0.025</size>
- <mesh>unit_box</mesh>
- <material>Gazebo/Blue</material>
- </visual>
- </geom:box>
- </body:box>
- <joint:slider name="finger_1_slider_left">
- <body2>finger_1_left</body2>
- <body1>gripper_roll_left</body1>
- <anchor>gripper_roll_left</anchor>
- <lowStop> -0.02 </lowStop>
- <highStop> 0.05 </highStop>
- <axis>0.0 1.0 0.0 ...
[truncated message content] |
