Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[1985] pkg/trunk
|
From: <hsu...@us...> - 2008-07-23 19:25:36
|
Revision: 1985
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1985&view=rev
Author: hsujohnhsu
Date: 2008-07-23 19:25:45 +0000 (Wed, 23 Jul 2008)
Log Message:
-----------
changed actarray into one list.
added PTZ controls as implemented in Gazebo.
Modified Paths:
--------------
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/robot_descriptions/gazebo_robot_description/world/pr2.model
Modified: pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp 2008-07-23 19:24:37 UTC (rev 1984)
+++ pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp 2008-07-23 19:25:45 UTC (rev 1985)
@@ -34,15 +34,16 @@
gazebo::Client *client;
gazebo::SimulationIface *simIface;
gazebo::PR2ArrayIface *pr2Iface;
-gazebo::PR2ArrayIface *pr2HeadIface;
gazebo::PR2GripperIface *pr2GripperLeftIface;
gazebo::PR2GripperIface *pr2GripperRightIface;
gazebo::LaserIface *pr2LaserIface;
gazebo::LaserIface *pr2BaseLaserIface;
gazebo::CameraIface *pr2CameraIface;
gazebo::CameraIface *pr2CameraGlobalIface;
-gazebo::CameraIface *pr2CameraHeadLeftIface;
-gazebo::CameraIface *pr2CameraHeadRightIface;
+gazebo::CameraIface *pr2PTZCameraLeftIface;
+gazebo::CameraIface *pr2PTZCameraRightIface;
+gazebo::PTZIface *pr2PTZLeftIface;
+gazebo::PTZIface *pr2PTZRightIface;
gazebo::PositionIface *pr2LeftWristIface;
gazebo::PositionIface *pr2RightWristIface;
gazebo::PositionIface *pr2BaseIface;
@@ -74,14 +75,15 @@
client = new gazebo::Client();
simIface = new gazebo::SimulationIface();
pr2Iface = new gazebo::PR2ArrayIface();
- pr2HeadIface = new gazebo::PR2ArrayIface();
pr2GripperLeftIface = new gazebo::PR2GripperIface();
pr2GripperRightIface = new gazebo::PR2GripperIface();
pr2LaserIface = new gazebo::LaserIface();
pr2BaseLaserIface = new gazebo::LaserIface();
pr2CameraGlobalIface = new gazebo::CameraIface();
- pr2CameraHeadLeftIface = new gazebo::CameraIface();
- pr2CameraHeadRightIface = new gazebo::CameraIface();
+ pr2PTZCameraLeftIface = new gazebo::CameraIface();
+ pr2PTZCameraRightIface = new gazebo::CameraIface();
+ pr2PTZLeftIface = new gazebo::PTZIface();
+ pr2PTZRightIface = new gazebo::PTZIface();
pr2LeftWristIface = new gazebo::PositionIface();
pr2RightWristIface = new gazebo::PositionIface();
@@ -120,17 +122,28 @@
<< e << "\n";
}
- /// Open the Position interface
+ /// Open the PTZ Position interface
try
{
- pr2HeadIface->Open(client, "pr2_head_iface");
+ pr2PTZLeftIface->Open(client, "ptz_left_iface");
}
catch (std::string e)
{
- std::cout << "Gazebo error: Unable to connect to the pr2 head interface\n"
+ std::cout << "Gazebo error: Unable to connect to the pr2 Left PTZ interface\n"
<< e << "\n";
}
+ /// Open the PTZ Position interface
+ try
+ {
+ pr2PTZRightIface->Open(client, "ptz_right_iface");
+ }
+ catch (std::string e)
+ {
+ std::cout << "Gazebo error: Unable to connect to the pr2 Right PTZ interface\n"
+ << e << "\n";
+ }
+
/// Open the Position interface for gripper left
try
{
@@ -179,7 +192,7 @@
}
- /// Open the camera interface for hokuyo
+ /// Open the camera interfaces
try
{
pr2CameraGlobalIface->Open(client, "cam1_iface_0");
@@ -193,24 +206,24 @@
try
{
- pr2CameraHeadLeftIface->Open(client, "head_cam_left_iface_0");
+ pr2PTZCameraLeftIface->Open(client, "ptz_cam_left_iface");
}
catch (std::string e)
{
std::cout << "Gazebo error: Unable to connect to the pr2 camera interface\n"
<< e << "\n";
- pr2CameraHeadLeftIface = NULL;
+ pr2PTZCameraLeftIface = NULL;
}
try
{
- pr2CameraHeadRightIface->Open(client, "head_cam_right_iface_0");
+ pr2PTZCameraRightIface->Open(client, "ptz_cam_right_iface");
}
catch (std::string e)
{
std::cout << "Gazebo error: Unable to connect to the pr2 camera interface\n"
<< e << "\n";
- pr2CameraHeadRightIface = NULL;
+ pr2PTZCameraRightIface = NULL;
}
try
@@ -248,23 +261,10 @@
std::cout << "initial HW reads\n" << std::endl;
// fill in actuator data
- for (int id = PR2::CASTER_FL_STEER; id < PR2::HEAD_PTZ_R_TILT; id++)
+ for (int id = PR2::HEAD_YAW; id < PR2::HEAD_PTZ_R_TILT; id++)
{
- if(IsHead((PR2::PR2_JOINT_ID)id))
+ if(IsGripperLeft((PR2::PR2_JOINT_ID)id))
{
- pr2HeadIface->Lock(1);
- this->jointData[id].cmdEnableMotor = pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdEnableMotor ;
- this->jointData[id].controlMode = pr2HeadIface->data->actuators[id-JointStart[HEAD]].controlMode ;
- this->jointData[id].pGain = pr2HeadIface->data->actuators[id-JointStart[HEAD]].pGain ;
- this->jointData[id].iGain = pr2HeadIface->data->actuators[id-JointStart[HEAD]].iGain ;
- this->jointData[id].dGain = pr2HeadIface->data->actuators[id-JointStart[HEAD]].dGain ;
- this->jointData[id].cmdPosition = pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdPosition ;
- this->jointData[id].cmdSpeed = pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdSpeed ;
- this->jointData[id].cmdEffectorForce = pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdEffectorForce ;
- pr2HeadIface->Unlock();
- }
- else if(IsGripperLeft((PR2::PR2_JOINT_ID)id))
- {
pr2GripperLeftIface->Lock(1);
this->jointData[id].cmdEnableMotor = pr2GripperLeftIface->data->cmdEnableMotor ;
this->jointData[id].controlMode = pr2GripperLeftIface->data->controlMode ;
@@ -361,9 +361,9 @@
PR2_ERROR_CODE PR2HW::SetJointGains(PR2_JOINT_ID id, double pGain, double iGain, double dGain)
{
- this->jointData[id-JointStart[HEAD]].pGain = pGain;
- this->jointData[id-JointStart[HEAD]].iGain = iGain;
- this->jointData[id-JointStart[HEAD]].dGain = dGain;
+ this->jointData[id].pGain = pGain;
+ this->jointData[id].iGain = iGain;
+ this->jointData[id].dGain = dGain;
return PR2_ALL_OK;
};
@@ -677,10 +677,10 @@
pr2CameraIface = pr2CameraGlobalIface;
break;
case CAMERA_HEAD_LEFT:
- pr2CameraIface = pr2CameraHeadLeftIface;
+ pr2CameraIface = pr2PTZCameraLeftIface;
break;
case CAMERA_HEAD_RIGHT:
- pr2CameraIface = pr2CameraHeadRightIface;
+ pr2CameraIface = pr2PTZCameraRightIface;
break;
default:
pr2CameraIface = pr2CameraGlobalIface;
@@ -827,18 +827,10 @@
{
//std::cout << "updating HW receive\n" << std::endl;
// receive data from hardware
- for (int id = PR2::CASTER_FL_STEER; id < PR2::HEAD_PTZ_R_TILT; id++)
+ for (int id = PR2::HEAD_YAW; id < PR2::HEAD_PTZ_R_TILT; id++)
{
- if(IsHead((PR2::PR2_JOINT_ID)id))
+ if(IsGripperLeft((PR2::PR2_JOINT_ID)id))
{
- pr2HeadIface->Lock(1);
- this->jointData[id].actualPosition = pr2HeadIface->data->actuators[id-JointStart[HEAD]].actualPosition ;
- this->jointData[id].actualSpeed = pr2HeadIface->data->actuators[id-JointStart[HEAD]].actualSpeed ;
- this->jointData[id].actualEffectorForce = pr2HeadIface->data->actuators[id-JointStart[HEAD]].actualEffectorForce;
- pr2HeadIface->Unlock();
- }
- else if(IsGripperLeft((PR2::PR2_JOINT_ID)id))
- {
pr2GripperLeftIface->Lock(1);
this->jointData[id].actualPosition = ( pr2GripperLeftIface->data->actualFingerPosition[0] - 0.015) // FIXME: not debugged
+(-pr2GripperLeftIface->data->actualFingerPosition[1] + 0.015);
@@ -886,23 +878,10 @@
//std::cout << "updating HW send\n" << std::endl;
// send commands to hardware
- for (int id = PR2::CASTER_FL_STEER; id < PR2::HEAD_PTZ_R_TILT; id++)
+ for (int id = PR2::HEAD_YAW; id < PR2::HEAD_PTZ_R_TILT; id++)
{
- if(IsHead((PR2::PR2_JOINT_ID)id))
+ if(IsGripperLeft((PR2::PR2_JOINT_ID)id))
{
- pr2HeadIface->Lock(1);
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdEnableMotor = this->jointData[id].cmdEnableMotor ;
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].controlMode = this->jointData[id].controlMode ;
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].pGain = this->jointData[id].pGain ;
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].iGain = this->jointData[id].iGain ;
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].dGain = this->jointData[id].dGain ;
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdPosition = this->jointData[id].cmdPosition ;
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdSpeed = this->jointData[id].cmdSpeed ;
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdEffectorForce = this->jointData[id].cmdEffectorForce;
- pr2HeadIface->Unlock();
- }
- else if(IsGripperLeft((PR2::PR2_JOINT_ID)id))
- {
pr2GripperLeftIface->Lock(1);
pr2GripperLeftIface->data->cmdEnableMotor = this->jointData[id].cmdEnableMotor ;
pr2GripperLeftIface->data->controlMode = this->jointData[id].controlMode ;
@@ -951,20 +930,10 @@
{
// std::cout << "updating Joint receive\n" << std::endl;
// receive data from hardware
- for (int id = PR2::CASTER_FL_STEER; id < PR2::HEAD_PTZ_R_TILT; id++)
+ for (int id = PR2::HEAD_YAW; id < PR2::HEAD_PTZ_R_TILT; id++)
{
- if(IsHead((PR2::PR2_JOINT_ID)id))
+ if(IsGripperLeft((PR2::PR2_JOINT_ID)id))
{
-
- pr2HeadIface->Lock(1);
- jointArray[id]->position = pr2HeadIface->data->actuators[id-JointStart[HEAD]].actualPosition ;
- jointArray[id]->velocity = pr2HeadIface->data->actuators[id-JointStart[HEAD]].actualSpeed ;
- jointArray[id]->appliedEffort = pr2HeadIface->data->actuators[id-JointStart[HEAD]].actualEffectorForce;
- pr2HeadIface->Unlock();
-
- }
- else 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
@@ -1017,27 +986,11 @@
// std::cout << "updating Joint send\n" << std::endl;
// send commands to hardware
- for (int id = PR2::CASTER_FL_STEER; id < PR2::HEAD_PTZ_R_TILT; id++)
+ for (int id = PR2::HEAD_YAW; id < PR2::HEAD_PTZ_R_TILT; id++)
{
- if(IsHead((PR2::PR2_JOINT_ID)id))
+ if(IsGripperLeft((PR2::PR2_JOINT_ID)id))
{
- pr2HeadIface->Lock(1);
-
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdEnableMotor = jointArray[id]->initialized;
/*
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].controlMode = this->jointData[id].controlMode ;
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].pGain = this->jointData[id].pGain ;
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].iGain = this->jointData[id].iGain ;
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].dGain = this->jointData[id].dGain ;
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdPosition = this->jointData[id].cmdPosition ;
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdSpeed = this->jointData[id].cmdSpeed ;
- */
- pr2HeadIface->data->actuators[id-JointStart[HEAD]].cmdEffectorForce = jointArray[id]->commandedEffort;
- pr2HeadIface->Unlock();
- }
- else if(IsGripperLeft((PR2::PR2_JOINT_ID)id))
- {
- /*
pr2GripperLeftIface->Lock(1);
pr2GripperLeftIface->data->cmdEnableMotor = this->jointData[id].cmdEnableMotor ;
pr2GripperLeftIface->data->controlMode = this->jointData[id].controlMode ;
Modified: pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h 2008-07-23 19:24:37 UTC (rev 1984)
+++ pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Core.h 2008-07-23 19:25:45 UTC (rev 1985)
@@ -64,6 +64,9 @@
};
enum PR2_JOINT_ID{
+ HEAD_YAW ,
+ HEAD_PITCH ,
+ HEAD_LASER_PITCH ,
CASTER_FL_STEER , // 0
CASTER_FL_DRIVE_L , // 1
CASTER_FL_DRIVE_R , // 2
@@ -93,9 +96,6 @@
ARM_R_WRIST_ROLL ,
ARM_L_GRIPPER ,
ARM_R_GRIPPER ,
- HEAD_YAW ,
- HEAD_PITCH ,
- HEAD_LASER_PITCH ,
HEAD_PTZ_L_PAN ,
HEAD_PTZ_L_TILT ,
HEAD_PTZ_R_PAN ,
@@ -121,6 +121,8 @@
};
enum PR2_BODY_ID{
+ HEAD_PAN_BASE ,
+ HEAD_TILT_BASE ,
CASTER_FL_WHEEL_L ,
CASTER_FL_WHEEL_R ,
CASTER_FL_BODY ,
@@ -153,8 +155,6 @@
ARM_R_HAND ,
ARM_R_FINGER_1 ,
ARM_R_FINGER_2 ,
- HEAD_PAN_BASE ,
- HEAD_TILT_BASE ,
STEREO_BLOCK ,
TILT_LASER_BLOCK ,
BASE_LASER_BLOCK ,
@@ -287,9 +287,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 , 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 };
- const int JointEnd[MAX_MODELS] = {CASTER_RR_DRIVE_R ,SPINE_ELEVATOR ,ARM_L_WRIST_ROLL ,ARM_R_WRIST_ROLL ,ARM_L_GRIPPER ,ARM_R_GRIPPER ,HEAD_LASER_PITCH };
+ 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_WRIST_ROLL ,ARM_R_WRIST_ROLL ,ARM_L_GRIPPER ,ARM_R_GRIPPER ,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-07-23 19:24:37 UTC (rev 1984)
+++ pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Misc.h 2008-07-23 19:25:45 UTC (rev 1985)
@@ -54,6 +54,20 @@
return false;
}
+ inline bool IsPTZLeft(PR2_MODEL_ID id)
+ {
+ if(id == PR2_LEFT_PTZ)
+ return true;
+ return false;
+ }
+
+ inline bool IsPTZRight(PR2_MODEL_ID id)
+ {
+ if(id == PR2_RIGHT_PTZ)
+ return true;
+ return false;
+ }
+
inline bool IsHead(PR2_JOINT_ID id)
{
if(id >= JointStart[HEAD] && id <= JointEnd[HEAD])
@@ -74,6 +88,20 @@
return false;
}
+ inline bool IsPTZRight(PR2_JOINT_ID id)
+ {
+ if (id >= JointStart[PR2_RIGHT_PTZ] && id <= JointEnd[PR2_RIGHT_PTZ])
+ return true;
+ return false;
+ }
+ inline bool IsPTZLeft(PR2_JOINT_ID id)
+ {
+ if (id >= JointStart[PR2_LEFT_PTZ] && id <= JointEnd[PR2_LEFT_PTZ])
+ return true;
+ return false;
+ }
+
+
inline double GetMagnitude(double xl[], int num)
{
int ii;
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model 2008-07-23 19:24:37 UTC (rev 1984)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2.model 2008-07-23 19:25:45 UTC (rev 1985)
@@ -14,8 +14,6 @@
<xyz>0.0 0.0 0.02</xyz>
<rpy>0.0 0.0 0.0 </rpy>
- <canonicalBody>base_body</canonicalBody>
-
<body:box name="base_body">
<xyz>0.175 0.0 0.15 </xyz> <!-- base bottom is h/2 + some height for wheel clearance-->
<rpy>0.0 0.0 0.0 </rpy>
@@ -52,127 +50,86 @@
</visual>
</geom:box>
- </body:box>
+ <!-- Hokuyo laser body -->
+ <geom:box name="base_laser_box">
+ <xyz>0.275 0.0 0.14</xyz>
+ <rpy>0 0 0</rpy>
+ <size>0.05 0.05 0.041</size>
+ <mass>0.12</mass>
+ <visual>
+ <scale>0.05 0.05 0.041</scale>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Blue</material>
+ </visual>
- <!-- Hokuyo laser body -->
- <model:physical name="base_laser_model">
- <canonicalBody>base_laser_body</canonicalBody>
- <body:box name="base_laser_body">
- <xyz>0.45 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>
+ </geom:box>
+ <geom:cylinder name="base_laser_cylinder1">
+ <xyz>0.275 0.0 0.161</xyz>
+ <rpy>0 0 0</rpy>
+ <size>0.02 0.013</size>
+ <mass>0.02</mass>
+
+ <visual>
<rpy>0 0 0</rpy>
- <size>0.05 0.05 0.041</size>
- <mass>0.12</mass>
+ <scale>0.04 0.04 0.013</scale>
+ <mesh>unit_cylinder</mesh>
+ <material>Gazebo/Black</material>
+ </visual>
- <visual>
- <scale>0.05 0.05 0.041</scale>
- <mesh>unit_box</mesh>
- <material>Gazebo/Blue</material>
- </visual>
+ </geom:cylinder>
- </geom:box>
- <geom:cylinder name="base_laser_cylinder1">
- <xyz>0.0 0.0 0.041</xyz>
+ <geom:cylinder name="base_laser_cylinder2">
+ <xyz>0.275 0.0 0.174</xyz>
+ <rpy>0 0 0</rpy>
+ <size>0.018 0.009</size>
+ <mass>0.02</mass>
+
+ <visual>
<rpy>0 0 0</rpy>
- <size>0.02 0.013</size>
- <mass>0.02</mass>
-
- <visual>
- <rpy>0 0 0</rpy>
- <scale>0.04 0.04 0.013</scale>
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Black</material>
- </visual>
+ <size>0.036 0.036 0.009</size>
+ <mesh>unit_cylinder</mesh>
+ <material>Gazebo/Black</material>
+ </visual>
- </geom:cylinder>
+ </geom:cylinder>
- <geom:cylinder name="base_laser_cylinder2">
- <xyz>0.0 0.0 0.054</xyz>
+ <geom:cylinder name="base_laser_cylinder3">
+ <xyz>0.275 0.0 0.183</xyz>
+ <rpy>0 0 0</rpy>
+ <size>0.0175 0.008</size>
+ <mass>0.02</mass>
+
+ <visual>
<rpy>0 0 0</rpy>
- <size>0.018 0.009</size>
- <mass>0.02</mass>
-
- <visual>
- <rpy>0 0 0</rpy>
- <size>0.036 0.036 0.009</size>
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Black</material>
- </visual>
+ <size>0.035 0.035 0.008</size>
+ <mesh>unit_cylinder</mesh>
+ <material>Gazebo/Shell</material>
+ </visual>
+ </geom:cylinder>
- </geom:cylinder>
+ <sensor:ray name="base_laser_1">
+ <origin>0.275 0.0 0.17</origin> <!-- from the robot coordinate system (center bottom of base box)-->
+ <rayCount>68</rayCount>
+ <rangeCount>683</rangeCount>
+ <laserCount>1</laserCount>
- <geom:cylinder name="base_laser_cylinder3">
- <xyz>0.0 0.0 0.063</xyz>
- <rpy>0 0 0</rpy>
- <size>0.0175 0.008</size>
- <mass>0.02</mass>
-
- <visual>
- <rpy>0 0 0</rpy>
- <size>0.035 0.035 0.008</size>
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Shell</material>
- </visual>
+ <minAngle>-135</minAngle>
+ <maxAngle>135</maxAngle>
- </geom:cylinder>
+ <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>
- <sensor:ray name="base_laser_1">
- <rayCount>68</rayCount>
- <rangeCount>683</rangeCount>
- <laserCount>1</laserCount>
- <origin>0.0 0.0 0.05</origin>
- <minAngle>-135</minAngle>
- <maxAngle>135</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>
- <!--
- -->
- <!--
- <sensor:ray2 name="laser_1">
- <rayCount>683</rayCount>
- <rangeCount>683</rangeCount>
- <laserCount>1</laserCount>
- <origin>0.0 0.0 0.05</origin>
-
- <minAngle>-135</minAngle>
- <maxAngle>135</maxAngle>
-
- <minRange>0.05</minRange>
- <maxRange>10.0</maxRange>
- <controller:sick2lms200_laser name="laser_controller_1">
- <interface:laser name="laser_iface_1"/>
- <updateRate>10</updateRate>
- </controller:sick2lms200_laser>
- </sensor:ray2>
- -->
-
- </body:box>
-
- <!-- attach hokuyo to torso -->
- <attach>
- <parentBody>base_body</parentBody>
- <myBody>base_laser_body</myBody>
- </attach>
- </model:physical>
-
-
-
<!-- ========== spine and shoulder =========== -->
- <canonicalBody>torso_body</canonicalBody>
-
<body:box name="torso_body">
<xyz>0.056 0.0 0.45</xyz>
<rpy>0.0 0.0 0.0</rpy>
@@ -851,469 +808,399 @@
<!-- neck body -->
- <model:physical name="neck_model">
- <body:box name="neck_body">
- <xyz>0.10 0.0 1.03</xyz>
- <rpy>0.0 0.0 0.0</rpy>
- <!--xyz>0.0 0.0 0.0</xyz>
- <rpy>0.0 0.0 0.0</rpy-->
- <geom:box name="neck_geom">
+ <body:box name="neck_body">
+ <xyz>0.10 0.0 1.03</xyz>
+ <rpy>0.0 0.0 0.0</rpy>
+ <!--xyz>0.0 0.0 0.0</xyz>
+ <rpy>0.0 0.0 0.0</rpy-->
+ <geom:box name="neck_geom">
+ <!--size>.20 .38 .14</size-->
+ <size>0.01 0.01 0.01</size>
+ <mass>0.01</mass>
+ <visual>
+ <mesh>unit_box</mesh>
<!--size>.20 .38 .14</size-->
<size>0.01 0.01 0.01</size>
- <mass>0.01</mass>
- <visual>
- <mesh>unit_box</mesh>
- <!--size>.20 .38 .14</size-->
- <size>0.01 0.01 0.01</size>
- <material>Gazebo/Black</material>
- </visual>
- </geom:box>
- </body:box>
+ <material>Gazebo/Black</material>
+ </visual>
+ </geom:box>
+ </body:box>
- <!-- attach neck to torso -->
- <attach>
- <parentBody>torso_body</parentBody>
- <myBody>neck_body</myBody>
- </attach>
+ <!-- attach neck to torso -->
+ <joint:hinge name="neck_torso_attach_joint">
+ <body1>torso_body</body1>
+ <body2>neck_body</body2>
+ <anchor>neck_body</anchor>
+ <axis> 1.0 0.0 0.0 </axis>
+ <lowStop> 0.0 </lowStop>
+ <highStop> 0.0 </highStop>
+ </joint:hinge>
- <canonicalBody>head_base_body</canonicalBody>
- <body:box name="head_base_body">
- <xyz>0.10 0.0 1.13 </xyz>
- <rpy>0.0 0.0 0.0 </rpy>
- <!-- report when this body collides with static bodies -->
- <reportStaticCollision>true</reportStaticCollision>
- <geom:box name="head_base_geom">
- <xyz> 0.0 0.00 0.000</xyz>
+
+ <body:box name="head_base_body">
+ <xyz>0.10 0.0 1.13 </xyz>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <!-- report when this body collides with static bodies -->
+ <reportStaticCollision>true</reportStaticCollision>
+ <geom:box name="head_base_geom">
+ <xyz> 0.0 0.00 0.000</xyz>
+ <size>0.165 0.21 0.003</size>
+ <mass>0.1</mass>
+ <mul>1</mul>
+ <!--color> 0.0 1.0 0.0</color-->
+ <visual>
<size>0.165 0.21 0.003</size>
- <mass>0.1</mass>
- <mul>1</mul>
- <!--color> 0.0 1.0 0.0</color-->
- <visual>
- <size>0.165 0.21 0.003</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/PioneerBody</material>
- </visual>
- </geom:box>
- </body:box>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/PioneerBody</material>
+ </visual>
+ </geom:box>
+ </body:box>
- <canonicalBody>head_tilt_body</canonicalBody>
- <body:box name="head_tilt_body">
- <xyz>0.10 0.0 1.07 </xyz>
- <rpy>0.0 0.0 0.0 </rpy>
- <!-- report when this body collides with static bodies -->
- <reportStaticCollision>true</reportStaticCollision>
- <geom:box name="head_tilt_geom">
- <xyz> 0.0 0.00 0.150</xyz>
+ <body:box name="head_tilt_body">
+ <xyz>0.10 0.0 1.07 </xyz>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <!-- report when this body collides with static bodies -->
+ <reportStaticCollision>true</reportStaticCollision>
+ <geom:box name="head_tilt_geom">
+ <xyz> 0.0 0.00 0.150</xyz>
+ <size>0.165 0.21 0.003</size>
+ <mass>0.1</mass>
+ <mul>1</mul>
+ <!--color> 0.0 1.0 0.0</color-->
+ <visual>
<size>0.165 0.21 0.003</size>
- <mass>0.1</mass>
- <mul>1</mul>
- <!--color> 0.0 1.0 0.0</color-->
- <visual>
- <size>0.165 0.21 0.003</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/Black</material>
- </visual>
- </geom:box>
- <geom:box name="head_tilt_left_geom">
- <xyz> 0.0 -0.105 0.10</xyz>
- <size>0.165 0.003 0.10</size>
- <mass>0.1</mass>
- <mul>1</mul>
- <!--color> 0.0 1.0 0.0</color-->
- <visual>
- <size>0.165 0.003 0.10</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/Shell</material>
- </visual>
- </geom:box>
- <geom:box name="head_tilt_right_geom">
- <xyz> 0.0 0.105 0.10</xyz>
- <size>0.165 0.003 0.10</size>
- <mass>0.1</mass>
- <mul>1</mul>
- <!--color> 0.0 1.0 0.0</color-->
- <visual>
- <size>0.165 0.003 0.10</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/Shell</material>
- </visual>
- </geom:box>
- </body:box>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Black</material>
+ </visual>
+ </geom:box>
+ <geom:box name="head_tilt_left_geom">
+ <xyz> 0.0 -0.105 0.10</xyz>
+ <size>0.165 0.003 0.10</size>
+ <mass>0.1</mass>
+ <mul>1</mul>
+ <!--color> 0.0 1.0 0.0</color-->
+ <visual>
+ <size>0.165 0.003 0.10</size>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Shell</material>
+ </visual>
+ </geom:box>
+ <geom:box name="head_tilt_right_geom">
+ <xyz> 0.0 0.105 0.10</xyz>
+ <size>0.165 0.003 0.10</size>
+ <mass>0.1</mass>
+ <mul>1</mul>
+ <!--color> 0.0 1.0 0.0</color-->
+ <visual>
+ <size>0.165 0.003 0.10</size>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Shell</material>
+ </visual>
+ </geom:box>
+ </body:box>
- <!-- attach head_base to head_tilt -->
- <joint:hinge name="head_tilt_joint">
- <body2>head_tilt_body</body2>
- <body1>head_base_body</body1>
- <anchor>head_tilt_body</anchor>
- <axis> 0.0 1.0 0.0 </axis>
- </joint:hinge>
+ <!-- attach head_base to head_tilt -->
+ <joint:hinge name="head_tilt_joint">
+ <body2>head_tilt_body</body2>
+ <body1>head_base_body</body1>
+ <anchor>head_tilt_body</anchor>
+ <axis> 0.0 1.0 0.0 </axis>
+ </joint:hinge>
- <!-- attach head to neck -->
- <joint:hinge name="neck_yaw_joint">
- <body1>neck_body</body1>
- <body2>head_base_body</body2>
- <anchor>head_base_body</anchor>
- <axis> 0.0 0.0 1.0 </axis>
- </joint:hinge>
+ <!-- attach head to neck -->
+ <joint:hinge name="neck_yaw_joint">
+ <body1>neck_body</body1>
+ <body2>head_base_body</body2>
+ <anchor>head_base_body</anchor>
+ <axis> 0.0 0.0 1.0 </axis>
+ </joint:hinge>
- <!-- left camera -->
- <body:box name="head_cam_left_body">
- <xyz> 0.15 0.23 1.07 </xyz>
- <rpy> 0.00 0.0 0.00 </rpy>
- <sensor:camera name="head_cam_left_sensor">
- <imageSize>640 480</imageSize>
- <hfov>60</hfov>
- <nearClip>0.1</nearClip>
- <farClip>100</farClip>
+ <!-- left camera -->
+ <body:box name="ptz_cam_left_body">
+ <xyz> 0.13 0.30 1.04 </xyz>
+ <rpy> 0.00 0.0 0.00 </rpy>
+ <sensor:camera name="ptz_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="ptz_cam_left_controller">
<updateRate>15.0</updateRate>
- <controller:generic_camera name="head_cam_left_controller">
- <updateRate>15.0</updateRate>
- <interface:camera name="head_cam_left_iface_0" />
- </controller:generic_camera>
- </sensor:camera>
- <geom:box name="head_cam_left_geom">
- <xyz> 0.00 0.00 0.00</xyz>
+ <interface:camera name="ptz_cam_left_iface" />
+ </controller:generic_camera>
+ <!-- PTZ camera controllers -->
+ </sensor:camera>
+ <geom:box name="ptz_cam_left_geom">
+ <xyz> 0.00 0.00 0.00</xyz>
+ <size>0.05 0.03 0.04</size>
+ <mass>0.1</mass>
+ <!--color> 0.0 1.0 0.0</color-->
+ <visual>
<size>0.05 0.03 0.04</size>
- <mass>0.1</mass>
- <!--color> 0.0 1.0 0.0</color-->
- <visual>
- <size>0.05 0.03 0.04</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/PioneerBody</material>
- </visual>
- </geom:box>
- </body:box>
- <body:box name="head_cam_base_left_body">
- <xyz> 0.15 0.20 1.07 </xyz>
- <rpy> 0.00 0.0 0.00 </rpy>
- <geom:box name="head_cam_base_left_geom">
- <xyz> 0.00 0.00 0.00</xyz>
- <size>0.05 0.02 0.02</size>
- <mass>0.1</mass>
- <!--color> 0.0 1.0 0.0</color-->
- <visual>
- <size>0.05 0.02 0.02</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/Shell</material>
- </visual>
- </geom:box>
- </body:box>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Black</material>
+ </visual>
+ </geom:box>
+ </body:box>
- <!-- right camera -->
- <body:empty name="head_cam_right_body">
- <xyz> 0.15 -0.23 1.07 </xyz>
- <rpy> 0.00 0.0 0.00 </rpy>
- <sensor:camera name="head_cam_right_sensor">
- <imageSize>640 480</imageSize>
- <hfov>60</hfov>
- <nearClip>0.1</nearClip>
- <farClip>100</farClip>
+ <body:box name="ptz_cam_base_left_body">
+ <xyz> 0.13 0.27 1.04 </xyz>
+ <rpy> 0.00 0.0 0.00 </rpy>
+ <geom:box name="ptz_cam_base_left_geom">
+ <xyz> 0.00 0.00 0.00</xyz>
+ <size>0.05 0.02 0.02</size>
+ <mass>0.1</mass>
+ <!--color> 0.0 1.0 0.0</color-->
+ <visual>
+ <size>0.05 0.02 0.02</size>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Shell</material>
+ </visual>
+ </geom:box>
+ </body:box>
+ <!-- right camera -->
+ <body:empty name="ptz_cam_right_body">
+ <xyz> 0.13 -0.30 1.04 </xyz>
+ <rpy> 0.00 0.0 0.00 </rpy>
+ <sensor:camera name="ptz_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="ptz_cam_right_controller">
<updateRate>15.0</updateRate>
- <controller:generic_camera name="head_cam_right_controller">
- <updateRate>15.0</updateRate>
- <interface:camera name="head_cam_right_iface_0" />
- </controller:generic_camera>
- </sensor:camera>
- <geom:box name="head_cam_right_geom">
- <xyz> 0.00 0.00 0.00</xyz>
+ <interface:camera name="ptz_cam_right_iface" />
+ </controller:generic_camera>
+ <!-- PTZ camera controllers -->
+ </sensor:camera>
+ <geom:box name="ptz_cam_right_geom">
+ <xyz> 0.00 0.00 0.00</xyz>
+ <size>0.05 0.03 0.04</size>
+ <mass>0.1</mass>
+ <!--color> 0.0 1.0 0.0</color-->
+ <visual>
<size>0.05 0.03 0.04</size>
- <mass>0.1</mass>
- <!--color> 0.0 1.0 0.0</color-->
- <visual>
- <size>0.05 0.03 0.04</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/PioneerBody</material>
- </visual>
- </geom:box>
- </body:empty>
- <body:box name="head_cam_base_right_body">
- <xyz> 0.15 -0.20 1.07 </xyz>
- <rpy> 0.00 0.0 0.00 </rpy>
- <geom:box name="head_cam_base_right_geom">
- <xyz> 0.00 0.00 0.00</xyz>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Black</material>
+ </visual>
+ </geom:box>
+ </body:empty>
+ <body:box name="ptz_cam_base_right_body">
+ <xyz> 0.13 -0.27 1.04 </xyz>
+ <rpy> 0.00 0.0 0.00 </rpy>
+ <geom:box name="ptz_cam_base_right_geom">
+ <xyz> 0.00 0.00 0.00</xyz>
+ <size>0.05 0.02 0.02</size>
+ <mass>0.1</mass>
+ <!--color> 0.0 1.0 0.0</color-->
+ <visual>
<size>0.05 0.02 0.02</size>
- <mass>0.1</mass>
- <!--color> 0.0 1.0 0.0</color-->
- <visual>
- <size>0.05 0.02 0.02</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/Shell</material>
- </visual>
- </geom:box>
- </body:box>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Shell</material>
+ </visual>
+ </geom:box>
+ </body:box>
- <!-- attach neck_body to ptz cameras -->
- <joint:hinge name="head_cam_left_yaw_joint">
- <body1>head_cam_left_body</body1>
- <body2>head_cam_base_left_body</body2>
- <anchor>head_cam_left_body</anchor>
- <axis> 0.0 0.0 1.0 </axis>
- </joint:hinge>
- <joint:hinge name="head_cam_left_pitch_joint">
- <body1>head_cam_base_left_body</body1>
- <body2>neck_body</body2>
- <anchor>head_cam_base_left_body</anchor>
- <axis> 0.0 1.0 0.0 </axis>
- </joint:hinge>
- <joint:hinge name="head_cam_right_yaw_joint">
- <body2>head_cam_right_body</body2>
- <body1>head_cam_base_right_body</body1>
- <anchor>head_cam_right_body</anchor>
- <axis> 0.0 0.0 1.0 </axis>
- </joint:hinge>
- <joint:hinge name="head_cam_right_pitch_joint">
- <body2>head_cam_base_right_body</body2>
- <body1>neck_body</body1>
- <anchor>head_cam_base_right_body</anchor>
- <axis> 0.0 1.0 0.0 </axis>
- </joint:hinge>
+ <!-- attach neck_body to ptz cameras -->
+ <joint:hinge name="ptz_cam_left_yaw_joint">
+ <body1>ptz_cam_left_body</body1>
+ <body2>ptz_cam_base_left_body</body2>
+ <anchor>ptz_cam_left_body</anchor>
+ <axis> 0.0 0.0 1.0 </axis>
+ </joint:hinge>
+ <joint:hinge name="ptz_cam_left_pitch_joint">
+ <body1>ptz_cam_base_left_body</body1>
+ <body2>neck_body</body2>
+ <anchor>ptz_cam_base_left_body</anchor>
+ <axis> 0.0 1.0 0.0 </axis>
+ </joint:hinge>
+ <joint:hinge name="ptz_cam_right_yaw_joint">
+ <body2>ptz_cam_right_body</body2>
+ <body1>ptz_cam_base_right_body</body1>
+ <anchor>ptz_cam_right_body</anchor>
+ <axis> 0.0 0.0 1.0 </axis>
+ </joint:hinge>
+ <joint:hinge name="ptz_cam_right_pitch_joint">
+ <body2>ptz_cam_base_right_body</body2>
+ <body1>neck_body</body1>
+ <anchor>ptz_cam_base_right_body</anchor>
+ <axis> 0.0 1.0 0.0 </axis>
+ </joint:hinge>
- <!-- stereo camera -->
- <body:empty name="stereo_camera_body">
- <xyz> 0.10 0.0 1.25 </xyz>
- <rpy> 0.0 0.0 0.0 </rpy>
- <sensor:stereocamera name="stereo_camera_sensor">
- <imageSize>640 480</imageSize>
- <hfov>60</hfov>
- <nearClip>0.1</nearClip>
- <farClip>100</farClip>
- <saveFrames>false</saveFrames>
- <saveFramePath>frames</saveFramePath>
- <baseline>0.2</baseline>
+ <!-- stereo camera -->
+ <body:empty name="stereo_camera_body">
+ <xyz> 0.10 0.0 1.25 </xyz>
+ <rpy> 0.0 0.0 0.0 </rpy>
+ <sensor:stereocamera name="stereo_camera_sensor">
+ <imageSize>640 480</imageSize>
+ <hfov>60</hfov>
+ <nearClip>0.1</nearClip>
+ <farClip>100</farClip>
+ <saveFrames>false</saveFrames>
+ <saveFramePath>frames</saveFramePath>
+ <baseline>0.2</baseline>
+ <updateRate>15.0</updateRate>
+ <controller:stereocamera name="stereo_camera_controller">
<updateRate>15.0</updateRate>
- <controller:stereocamera name="stereo_camera_controller">
- <updateRate>15.0</updateRate>
- <interface:stereocamera name="stereo_iface_0" />
- <interface:camera name="camera_iface_0" />
- <interface:camera name="camera_iface_1" />
- <leftcamera>camera_iface_0</leftcamera>
- <rightcamera>camera_iface_1</rightcamera>
- </controller:stereocamera>
- </sensor:stereocamera>
- <geom:box name="stereo_camera_geom">
- <xyz> 0.00 0.00 0.00</xyz>
+ <interface:stereocamera name="stereo_iface_0" />
+ <interface:camera name="camera_iface_0" />
+ <interface:camera name="camera_iface_1" />
+ <leftcamera>camera_iface_0</leftcamera>
+ <rightcamera>camera_iface_1</rightcamera>
+ </controller:stereocamera>
+ </sensor:stereocamera>
+ <geom:box name="stereo_camera_geom">
+ <xyz> 0.00 0.00 0.00</xyz>
+ <size>0.10 0.15 0.10</size>
+ <mass>0.1</mass>
+ <visual>
<size>0.10 0.15 0.10</size>
- <mass>0.1</mass>
- <visual>
- <size>0.10 0.15 0.10</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/PioneerBody</material>
- </visual>
- </geom:box>
- </body:empty>
+ <rpy>0.0 0.0 0.0 </rpy>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/PioneerBody</material>
+ </visual>
+ </geom:box>
+ </body:empty>
- <!-- attach stereo_camera to head_tilt -->
- <joint:hinge name="stereo_camera_roll_joint">
- <body2>head_tilt_body</body2>
- <body1>stereo_camera_body</body1>
- <anchor>head_tilt_body</anchor>
- <axis> 1.0 0.0 0.0 </axis>
- </joint:hinge>
- <joint:hinge name="stereo_camera_pitch_joint">
- <body2>head_tilt_body</body2>
- <body1>stereo_camera_body</body1>
- <anchor>head_tilt_body</anchor>
- <axis> 0.0 1.0 0.0 </axis>
- </joint:hinge>
- <joint:hinge name="stereo_camera_yaw_joint">
- <body2>head_tilt_body</body2>
- <body1>stereo_camera_body</body1>
- <anchor>head_tilt_body</anchor>
- <axis> 0.0 0.0 010 </axis>
- </joint:hinge>
+ <!-- attach stereo_camera to head_tilt -->
+ <joint:hinge name="stereo_camera_attach_joint">
+ <body2>head_tilt_body</body2>
+ <body1>stereo_camera_body</body1>
+ <anchor>head_tilt_body</anchor>
+ <axis> 1.0 0.0 0.0 </axis>
+ <lowStop> 0.0 </lowStop>
+ <highStop> 0.0 </highStop>
+ </joint:hinge>
- <!-- Hokuyo laser body -->
- <canonicalBody>laser_body</canonicalBody>
- <body:box name="laser_body">
- <xyz>0.23 0.0 1.0</xyz>
- <rpy>0.0 0.0 0.0</rpy>
-
- <geom:box name="laser_box">
- <xyz>0.0 0.0 0.02</xyz>
- <rpy>0 0 0</rpy>
- <size>0.05 0.05 0.041</size>
- <mass>0.12</mass>
+ <!-- Hokuyo laser body -->
+ <body:box name="laser_body">
+ <xyz>0.23 0.0 1.0</xyz>
+ <rpy>0.0 0.0 0.0</rpy>
+
+ <geom:box name="laser_box">
+ <xyz>0.0 0.0 0.02</xyz>
+ <rpy>0 0 0</rpy>
+ <size>0.05 0.05 0.041</size>
+ <mass>0.12</mass>
- <visual>
- <scale>0.05 0.05 0.041</scale>
- <mesh>unit_box</mesh>
- <material>Gazebo/Blue</material>
- </visual>
+ <visual>
+ <scale>0.05 0.05 0.041</scale>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Blue</material>
+ </visual>
- </geom:box>
- <geom:cylinder name="laser_cylinder1">
- <xyz>0.0 0.0 0.041</xyz>
+ </geom:box>
+ <geom:cylinder name="laser_cylinder1">
+ <xyz>0.0 0.0 0.041</xyz>
+ <rpy>0 0 0</rpy>
+ <size>0.02 0.013</size>
+ <mass>0.02</mass>
+
+ <visual>
<rpy>0 0 0</rpy>
- <size>0.02 0.013</size>
- <mass>0.02</mass>
-
- <visual>
- <rpy>0 0 0</rpy>
- <scale>0.04 0.04 0.013</scale>
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Black</material>
- </visual>
+ <scale>0.04 0.04 0.013</scale>
+ <mesh>unit_cylinder</mesh>
+ <material>Gazebo/Black</material>
+ </visual>
- </geom:cylinder>
+ </geom:cylinder>
- <geom:cylinder name="laser_cylinder2">
- <xyz>0.0 0.0 0.054</xyz>
+ <geom:cylinder name="laser_cylinder2">
+ <xyz>0.0 0.0 0.054</xyz>
+ <rpy>0 0 0</rpy>
+ <size>0.018 0.009</size>
+ <mass>0.02</mass>
+
+ <visual>
<rpy>0 0 0</rpy>
- <size>0.018 0.009</size>
- <mass>0.02</mass>
-
- <visual>
- <rpy>0 0 0</rpy>
- <size>0.036 0.036 0.009</size>
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Black</material>
- </visual>
+ <size>0.036 0.036 0.009</size>
+ <mesh>unit_cylinder</mesh>
+ <material>Gazebo/Black</material>
+ </visual>
- </geom:cylinder>
+ </geom:cylinder>
- <geom:cylinder name="laser_cylinder3">
- <xyz>0.0 0.0 0.063</xyz>
+ <geom:cylinder name="laser_cylinder3">
+ <xyz>0.0 0.0 0.063</xyz>
+ <rpy>0 0 0</rpy>
+ <size>0.0175 0.008</size>
+ <mass>0.02</mass>
+
+ <visual>
<rpy>0 0 0</rpy>
- <size>0.0175 0.008</size>
- <mass>0.02</mass>
-
- <visual>
- <rpy>0 0 0</rpy>
- <size>0.035 0.035 0.008</size>
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Shell</material>
- </visual>
+ <size>0.035 0.035 0.008</size>
+ <mesh>unit_cylinder</mesh>
+ <material>Gazebo/Shell</material>
+ </visual>
- </geom:cylinder>
+ </geom:cylinder>
- <sensor:ray name="laser_1">
- <rayCount>68</rayCount>
- <rangeCount>683</rangeCount>
- <laserCount>1</laserCount>
- <origin>0.0 0.0 0.05</origin>
+ <sensor:ray name="laser_1">
+ <rayCount>68</rayCount>
+ <rangeCount>683</rangeCount>
+ <laserCount>1</laserCount>
+ <origin>0.0 0.0 0.05</origin>
- <minAngle>-45</minAngle>
- <maxAngle>45</maxAngle>
+ <minAngle>-135</minAngle>
+ <maxAngle> 135</maxAngle>
- <minRange>0.05</minRange>
- <maxRange>10.0</maxRange>
+ <minRange>0.05</minRange>
+ <maxRange>10.0</maxRange>
+ <updateRate>10</updateRate>
+ <controller:sicklms200_laser name="laser_controller_1">
+ <interface:laser name="laser_iface_1"/>
<updateRate>10</updateRate>
- <controller:sicklms200_laser name="laser_controller_1">
- <interface:laser name="laser_iface_1"/>
- <updateRate>10</updateRate>
- </controller:sicklms200_laser>
- </sensor:ray>
- <!--
- -->
- <!--
- <sensor:ray2 name="laser_1">
- <rayCount>683</rayCount>
- <rangeCount>683</rangeCount>
- <laserCount>1</laserCount>
- <origin>0.0 0.0 0.05</origin>
+ </controller:sicklms200_laser>
+ </sensor:ray>
+ <!--
+ -->
+ <!--
+ <sensor:ray2 name="laser_1">
+ <rayCount>683</rayCount>
+ <rangeCount>683</rangeCount>
+ <laserCount>1</laserCount>
+ <origin>0.0 0.0 0.05</origin>
- <minAngle>-135</minAngle>
- <maxAngle>135</maxAngle>
+ <minAngle>-135</minAngle>
+ <maxAngle>135</maxAngle>
- <minRange>0.05</minRange>
- <maxRange>10.0</maxRange>
- <controller:sick2lms200_laser name="laser_controller_1">
- <interface:laser name="laser_iface_1"/>
- <updateRate>10</updateRate>
- </controller:sick2lms200_laser>
- </sensor:ray2>
- -->
+ <minRange>0.05</minRange>
+ <maxRange>10.0</maxRange>
+ <controller:sick2lms200_laser name="laser_controller_1">
+ <interface:laser name="laser_iface_1"/>
+ <updateRate>10</updateRate>
+ </controller:sick2lms200_laser>
+ </sensor:ray2>
+ -->
- </body:box>
+ </body:box>
- <!-- attach hokuyo to torso -->
- <joint:hinge name="hokuyo_pitch_joint">
- <body1>neck_body</body1>
- <body2>laser_body</body2>
- <anchor>laser_body</anchor>
- <axis> 0.0 1.0 0.0 </axis>
- </joint:hinge>
+ <!-- attach hokuyo to torso -->
+ <joint:hinge name="hokuyo_pitch_joint">
+ <body1>neck_body</body1>
+ <body2>laser_body</body2>
+ <anchor>laser_body</anchor>
+ <axis> 0.0 1.0 0.0 </axis>
+ </joint:hinge>
- <controller:pr2_actarray name="pr2_head_controller" plugin="libPr2_Actarray.so">
- <updateRate>100.0</updateRate>
- <joint name="neck_yaw_joint">
- <saturationTorque>100</saturationTorque>
- <pGain>1</pGain>
- <dGain>0</dGain>
- <iGain>0</iGain>
- <iClamp>0</iClamp>
- </joint>
- <joint name="head_tilt_joint">
- <saturationTorque>10</saturationTorque>
- <pGain>1</pGain>
- <dGain>0</dGain>
- <iGain>0</iGain>
- <iClamp>0</iClamp>
- </joint>
- <joint name="hokuyo_pitch_joint">
- <saturationTorque>500</saturationTorque>
- <pGain>2</pGain>
- <dGain>0</dGain>
- <iGain>1</iGain>
- <iClamp>20</iClamp>
- </joint>
- <joint name="head_cam_left_yaw_joint">
- <saturationTorque>100</saturationTorque>
- <pGain>1</pGain>
- <dGain>0</dGain>
- <iGain>0</iGain>
- <iClamp>0</iClamp>
- </joint>
- <joint name="head_cam_left_pitch_joint">
- <saturationTorque>100</saturationTorque>
- <pGain>1</pGain>
- <dGain>0</dGain>
- <iGain>0</iGain>
- <iClamp>0</iClamp>
- </joint>
- <joint name="head_cam_right_yaw_joint">
- <saturationTorque>100</saturationTorque>
- <pGain>1</pGain>
- <dGain>0</dGain>
- <iGain>0</iGain>
- <iClamp>0</iClamp>
- </joint>
- <joint name="head_cam_right_pitch_joint">
- <saturationTorque>100</saturationTorque>
- <pGain>1</pGain>
- <dGain>0</dGain>
- <iGain>0</iGain>
- <iClamp>0</iClamp>
- </joint>
- <interface:pr2array name="pr2_head_iface"/>
- </controller:pr2_actarray>
- </model:physical>
-
-
-
<!--=========================== caster ===========================-->
- <canonicalBody>front_left_caster_body</canonicalBody>
- <canonicalBody>front_right_caster_body</canonicalBody>
- <canonicalBody>rear_left_caster_body</canonicalBody>
- <canonicalBody>rear_right_caster_body</canonicalBody>
-
- <!--=========================== caster body ===========================-->
<body:box name="front_left_caster_body">
<xyz>0.425 0.25 0.16069 </xyz> <!-- caster height + height off ground of base bottom -->
<rpy> 0.0 0.0 0.0 </rpy>
@@ -1795,9 +1682,35 @@
+
+
+
<!-- PR2_ACTARRAY -->
<controller:pr2_actarray name="pr2_controller" plugin="libPr2_Actarray.so">
<updateRate>100.0</updateRate>
+ <!-- head and above array -->
+ <joint name="neck_yaw_joint">
+ <saturationTorque>100</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>0</iClamp>
+ </joint>
+ <joint name="head_tilt_joint">
+ <saturationTorque>10</saturationTorque>
+ <pGain>1</pGain>
+ <dGain>0</dGain>
+ <iGain>0</iGain>
+ <iClamp>0</iClamp>
+ </joint>
+ <joint name="hokuyo_pitch_joint">
+ <saturationTorque>500</saturationTorque>
+ <pGain>2</pGain>
+ <dGain>0</dGain>
+ <iGain>1</iGain>
+ <iClamp>20</iClamp>
+ </joint>
+ <!-- caster array -->
<joint name="front_left_caster_steer">
<saturationTorque>1000</saturationTorque>
<pGain>2</pGain>
@@ -1883,6 +1796,7 @@
<iClamp>0</iClamp>
</joint>
<!-- ========================================= -->
+ <!-- torso array -->
<joint name="torso_spine_slider">
<saturationTorque>1000</saturationTorque>
<pGain>1</pGain>
@@ -1891,6 +1805,7 @@
<iClamp>0</iClamp>
</joint>
<!-- ========================================= -->
+ <!-- left arm array -->
<joint name="shoulder_pan_left">
<saturationTorque>1000</saturationTorque>
<pGain>1</pGain>
@@ -1941,6 +1856,7 @@
<iClamp>0</iClamp>
</joint>
<!-- ========================================= -->
+ <!-- right arm array -->
<joint name="shoulder_pan_right">
<saturationTorque>1000</saturationTorque>
<pGain>1</pGain>
@@ -2035,6 +1951,20 @@
<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>
+
</model:physical>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
