Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots: [800] pkg/trunk
|
From: <hsu...@us...> - 2008-06-13 02:12:52
|
Revision: 800
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=800&view=rev
Author: hsujohnhsu
Date: 2008-06-12 19:13:00 -0700 (Thu, 12 Jun 2008)
Log Message:
-----------
Update GetBasePosition in rosgazebo and libpr2API.
Update roslaunch codes in 2dnav-gazebo.
Move world into 2dnav-gazebo.
Move Pid.h into controllers directory.
Modified Paths:
--------------
pkg/trunk/controllers/src/Pid.cpp
pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo.xml
pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
pkg/trunk/simulators/gazebo_plugin/src/Pr2_Actarray.cc
pkg/trunk/simulators/rosgazebo/rosgazebo.cc
Added Paths:
-----------
pkg/trunk/controllers/include/controllers/
pkg/trunk/controllers/include/controllers/Pid.h
Removed Paths:
-------------
pkg/trunk/controllers/include/Pid.h
pkg/trunk/simulators/rosgazebo/world/map3.png
pkg/trunk/simulators/rosgazebo/world/pr2.model
pkg/trunk/simulators/rosgazebo/world/robot.world
Deleted: pkg/trunk/controllers/include/Pid.h
===================================================================
--- pkg/trunk/controllers/include/Pid.h 2008-06-13 01:35:12 UTC (rev 799)
+++ pkg/trunk/controllers/include/Pid.h 2008-06-13 02:13:00 UTC (rev 800)
@@ -1,92 +0,0 @@
-#pragma once
-
-
-/***************************************************/
-/*! \brief A basic pid class.
-
- This class implements a generic structure that
- can be used to create a wide range of pid
- controllers. It can function independently or
- be subclassed to provide more specific controls
- based on a particular control loop.
-
- In particular, this class implements the standard
- pid equation:
-
- command = Pterm + Iterm + Dterm
-
- where: <br>
- <UL TYPE="none">
- <LI> Pterm = pGain * pError
- <LI> Iterm = iGain * iError
- <LI> Dterm = dGain * dError
- <LI> iError = iError + pError * dT
- <LI> dError = dError + pError / dT
- </UL>
-
- given:<br>
- <UL TYPE="none">
- <LI> pError = pTarget - pState.
- </UL>
-
- If the fixedTime input of UpdatePid is set to alpha,
- dT = alpha. Otherwise the time step is computed when
- the function is called.
-
-*/
-/***************************************************/
-class Pid
-{
- public:
-
- /*!
- * \brief Constructor, zeros out Pid values when created and
- * initialize Pid-gains and integral term limits:[iMax:iMin]-[I1:I2].
- *
- * \param P The proportional gain.
- * \param I The integral gain.
- * \param D The derivative gain.
- * \param I1 The integral upper limit.
- * \param I2 The integral lower limit.
- */
- Pid(double P = 0.8,double I = 0.5, double D = 0.0, double I1 = 1.0, double I2 =-1.0);
-
- /*!
- * \brief Destructor of Pid class.
- */
- ~Pid( );
-
- /*!
- * \brief Update the Pid loop with nonuniform time step size.
- *
- * \param pState This is the current measured state or position of the object
- * being controlled.
- * \param pTarget This is the set point the controller is trying to reach.
- * \param fixedTime Set to a value for fixed time step of that value
- */
- double UpdatePid( double pError, double dt );
-
- /*!
- * \brief Initialize PID-gains and integral term limits:[iMax:iMin]-[I1:I2]
- *
- * \param P The proportional gain.
- * \param I The integral gain.
- * \param D The derivative gain.
- * \param I1 The integral upper limit.
- * \param I2 The integral lower limit.
- */
- void InitPid( double P,double I, double D, double I1, double I2 );
-
-
- private:
- double pError; /**< Derivative state. */
- double dError; /**< Derivative state. */
- double iError; /**< Integrator state. */
- double pGain; /**< Proportional gain. */
- double iGain; /**< Integral gain. */
- double dGain; /**< Derivative gain. */
- double iMax; /**< Maximum allowable integrator state. */
- double iMin; /**< Minimum allowable integrator state. */
- double currentCommand; /**< Current position command. */
-};
-
Copied: pkg/trunk/controllers/include/controllers/Pid.h (from rev 793, pkg/trunk/controllers/include/Pid.h)
===================================================================
--- pkg/trunk/controllers/include/controllers/Pid.h (rev 0)
+++ pkg/trunk/controllers/include/controllers/Pid.h 2008-06-13 02:13:00 UTC (rev 800)
@@ -0,0 +1,92 @@
+#pragma once
+
+
+/***************************************************/
+/*! \brief A basic pid class.
+
+ This class implements a generic structure that
+ can be used to create a wide range of pid
+ controllers. It can function independently or
+ be subclassed to provide more specific controls
+ based on a particular control loop.
+
+ In particular, this class implements the standard
+ pid equation:
+
+ command = Pterm + Iterm + Dterm
+
+ where: <br>
+ <UL TYPE="none">
+ <LI> Pterm = pGain * pError
+ <LI> Iterm = iGain * iError
+ <LI> Dterm = dGain * dError
+ <LI> iError = iError + pError * dT
+ <LI> dError = dError + pError / dT
+ </UL>
+
+ given:<br>
+ <UL TYPE="none">
+ <LI> pError = pTarget - pState.
+ </UL>
+
+ If the fixedTime input of UpdatePid is set to alpha,
+ dT = alpha. Otherwise the time step is computed when
+ the function is called.
+
+*/
+/***************************************************/
+class Pid
+{
+ public:
+
+ /*!
+ * \brief Constructor, zeros out Pid values when created and
+ * initialize Pid-gains and integral term limits:[iMax:iMin]-[I1:I2].
+ *
+ * \param P The proportional gain.
+ * \param I The integral gain.
+ * \param D The derivative gain.
+ * \param I1 The integral upper limit.
+ * \param I2 The integral lower limit.
+ */
+ Pid(double P = 0.8,double I = 0.5, double D = 0.0, double I1 = 1.0, double I2 =-1.0);
+
+ /*!
+ * \brief Destructor of Pid class.
+ */
+ ~Pid( );
+
+ /*!
+ * \brief Update the Pid loop with nonuniform time step size.
+ *
+ * \param pState This is the current measured state or position of the object
+ * being controlled.
+ * \param pTarget This is the set point the controller is trying to reach.
+ * \param fixedTime Set to a value for fixed time step of that value
+ */
+ double UpdatePid( double pError, double dt );
+
+ /*!
+ * \brief Initialize PID-gains and integral term limits:[iMax:iMin]-[I1:I2]
+ *
+ * \param P The proportional gain.
+ * \param I The integral gain.
+ * \param D The derivative gain.
+ * \param I1 The integral upper limit.
+ * \param I2 The integral lower limit.
+ */
+ void InitPid( double P,double I, double D, double I1, double I2 );
+
+
+ private:
+ double pError; /**< Derivative state. */
+ double dError; /**< Derivative state. */
+ double iError; /**< Integrator state. */
+ double pGain; /**< Proportional gain. */
+ double iGain; /**< Integral gain. */
+ double dGain; /**< Derivative gain. */
+ double iMax; /**< Maximum allowable integrator state. */
+ double iMin; /**< Minimum allowable integrator state. */
+ double currentCommand; /**< Current position command. */
+};
+
Modified: pkg/trunk/controllers/src/Pid.cpp
===================================================================
--- pkg/trunk/controllers/src/Pid.cpp 2008-06-13 01:35:12 UTC (rev 799)
+++ pkg/trunk/controllers/src/Pid.cpp 2008-06-13 02:13:00 UTC (rev 800)
@@ -1,4 +1,4 @@
-#include "Pid.h"
+#include <controllers/Pid.h>
#include<iostream>
using namespace std;
Modified: pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo.xml 2008-06-13 01:35:12 UTC (rev 799)
+++ pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo.xml 2008-06-13 02:13:00 UTC (rev 800)
@@ -1,10 +1,9 @@
<launch>
<group name="wg">
- <!--node pkg="rosgazebo" type="gazebo" args="world/robot.world" respawn="false" /-->
- <node pkg="rosgazebo" type="run-gazebo.sh" args="$(find rosgazebo)/world/robot.world" respawn="false" />
+ <node pkg="rosgazebo" type="run-gazebo.sh" args="$(find 2dnav-gazebo)/world/robot.world" respawn="false" />
<node pkg="map_server" type="map_server" args="$(find 2dnav-gazebo)/map3.png 0.1" respawn="false" />
- <node pkg="rosgazebo" type="run-rosgazebo.sh" args="" respawn="false" />
+ <node pkg="rosgazebo" type="run-rosgazebo.sh" args="" respawn="true" />
<node pkg="amcl_player" type="amcl_player" respawn="false" />
<node pkg="wavefront_player" type="wavefront_player" respawn="false" />
<node pkg="nav_view" type="nav_view" respawn="true" />
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h 2008-06-13 01:35:12 UTC (rev 799)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h 2008-06-13 02:13:00 UTC (rev 800)
@@ -582,7 +582,7 @@
\param y - sideways (left)
\param w - upward
*/
- public: PR2_ERROR_CODE GetBasePositionActual(double* x, double* y, double* z);
+ public: PR2_ERROR_CODE GetBasePositionActual(double* x, double* y, double *z, double *roll, double *pitch, double *yaw);
/*! \fn
\brief - Run the robot
Modified: pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-06-13 01:35:12 UTC (rev 799)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp 2008-06-13 02:13:00 UTC (rev 800)
@@ -23,6 +23,7 @@
static gazebo::PositionIface *pr2LeftWristIface;
static gazebo::PositionIface *pr2RightWristIface;
+static gazebo::PositionIface *pr2BaseIface;
point Rot2D(point p,double theta)
@@ -113,7 +114,8 @@
pr2CameraHeadRightIface = new gazebo::CameraIface();
pr2LeftWristIface = new gazebo::PositionIface();
- pr2RightWristIface = new gazebo::PositionIface();
+ pr2RightWristIface = new gazebo::PositionIface();
+ pr2BaseIface = new gazebo::PositionIface();
int serverId = 0;
@@ -274,6 +276,17 @@
pr2RightWristIface = NULL;
}
+ try
+ {
+ pr2BaseIface->Open(client, "p3d_base_position");
+ }
+ catch (std::string e)
+ {
+ std::cout << "Gazebo error: Unable to connect to the base position interface\n"
+ << e << "\n";
+ pr2BaseIface = NULL;
+ }
+
return PR2_ALL_OK;
}
@@ -1317,13 +1330,16 @@
return PR2_ALL_OK;
};
-PR2_ERROR_CODE PR2Robot::GetBasePositionActual(double* x, double* y, double* th)
+PR2_ERROR_CODE PR2Robot::GetBasePositionActual(double* x, double* y, double *z, double *roll, double *pitch, double *yaw)
{
- simIface->Lock(1);
- *x = simIface->data->model_pose.pos.x;
- *y = simIface->data->model_pose.pos.y;
- *th = simIface->data->model_pose.yaw;
- simIface->Unlock();
+ pr2BaseIface->Lock(1);
+ *x = pr2BaseIface->data->pose.pos.x;
+ *y = pr2BaseIface->data->pose.pos.y;
+ *z = pr2BaseIface->data->pose.pos.z;
+ *roll = pr2BaseIface->data->pose.roll;
+ *pitch = pr2BaseIface->data->pose.pitch;
+ *yaw = pr2BaseIface->data->pose.yaw;
+ pr2BaseIface->Unlock();
return PR2_ALL_OK;
};
Modified: pkg/trunk/simulators/gazebo_plugin/src/Pr2_Actarray.cc
===================================================================
--- pkg/trunk/simulators/gazebo_plugin/src/Pr2_Actarray.cc 2008-06-13 01:35:12 UTC (rev 799)
+++ pkg/trunk/simulators/gazebo_plugin/src/Pr2_Actarray.cc 2008-06-13 02:13:00 UTC (rev 800)
@@ -149,7 +149,7 @@
this->myIface->data->actuators_count = this->numJoints;
-// printf("numJoints: %d\n",this->numJoints);
+ //printf("numJoints: %d\n",this->numJoints);
for (int i=0; i < this->numJoints; i++)
{
if(this->joints[i]->GetType() == Joint::SLIDER)
@@ -178,7 +178,7 @@
sjoint->SetParam( dParamVel, this->myIface->data->actuators[i].pGain * positionError + this->myIface->data->actuators[i].dGain * speedError);
sjoint->SetParam( dParamFMax, this->myIface->data->actuators[i].saturationTorque );
}
-// printf("SLIDER:: pErr: %f, pGain: %f, dGain: %f, sT: %f\n",positionError,this->myIface->data->actuators[i].pGain,this->myIface->data->actuators[i].dGain,this->myIface->data->actuators[i].saturationTorque);
+ // printf("SLIDER:: pErr: %f, pGain: %f, dGain: %f, sT: %f\n",positionError,this->myIface->data->actuators[i].pGain,this->myIface->data->actuators[i].dGain,this->myIface->data->actuators[i].saturationTorque);
break;
case PR2::SPEED_CONTROL :
sjoint->SetParam( dParamVel, cmdSpeed);
Modified: pkg/trunk/simulators/rosgazebo/rosgazebo.cc
===================================================================
--- pkg/trunk/simulators/rosgazebo/rosgazebo.cc 2008-06-13 01:35:12 UTC (rev 799)
+++ pkg/trunk/simulators/rosgazebo/rosgazebo.cc 2008-06-13 02:13:00 UTC (rev 800)
@@ -377,10 +377,10 @@
this->odomMsg.vel.th = vw;
// Get position
- double x,y,th;
- this->myPR2->GetBasePositionActual(&x,&y,&th);
- this->odomMsg.pos.x = x + 256.5;
- this->odomMsg.pos.y = y + 256.5;
+ double x,y,z,roll,pitch,th;
+ this->myPR2->GetBasePositionActual(&x,&y,&z,&roll,&pitch,&th);
+ this->odomMsg.pos.x = x;
+ this->odomMsg.pos.y = y;
this->odomMsg.pos.th = th;
// this->odomMsg.stall = this->positionmodel->Stall();
@@ -496,8 +496,6 @@
int
main(int argc, char** argv)
{
-
- usleep(1000000);
ros::init(argc,argv);
GazeboNode gn(argc,argv,argv[1]);
Deleted: pkg/trunk/simulators/rosgazebo/world/map3.png
===================================================================
(Binary files differ)
Deleted: pkg/trunk/simulators/rosgazebo/world/pr2.model
===================================================================
--- pkg/trunk/simulators/rosgazebo/world/pr2.model 2008-06-13 01:35:12 UTC (rev 799)
+++ pkg/trunk/simulators/rosgazebo/world/pr2.model 2008-06-13 02:13:00 UTC (rev 800)
@@ -1,1426 +0,0 @@
-<?xml version="1.0"?>
-
-<!-- PR2 model -->
-<model:physical name="pr2_model"
- xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
- xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
- xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
- xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
- xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
- xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
- xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
- >
-
- <xyz>0.0 0.0 0.0</xyz>
- <rpy>0.0 0.0 0.0</rpy>
-
- <canonicalBody>base_body</canonicalBody>
-
- <body:box name="base_body">
- <xyz>0.175 0.0 -0.006</xyz>
- <rpy>0.0 0.0 0.0 </rpy>
-
- <!-- report when this body collides with static bodies -->
- <reportStaticCollision>true</reportStaticCollision>
-
- <geom:box name="base_body_geom">
- <xyz> 0.0 0.00 0.000</xyz>
- <size>0.65 0.65 0.012</size>
- <mass>100.0</mass>
- <mul>1</mul>
- <!--color> 0.0 1.0 0.0</color-->
- <visual>
- <size>0.65 0.65 0.012</size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/Grey</material>
- </visual>
- </geom:box>
-
- <geom:box name="base_spine_geom">
- <xyz> -0.175 0.00 0.5 </xyz>
- <size> 0.01 0.05 1.0 </size>
- <mass>0.01</mass>
- <mul>1</mul>
- <visual>
- <size> 0.01 0.05 1.0 </size>
- <rpy>0.0 0.0 0.0 </rpy>
- <mesh>unit_box</mesh>
- <material>Gazebo/Grey</material>
- </visual>
- </geom:box>
-
- </body:box>
-
-
- <!-- Hokuyo laser body -->
- <canonicalBody>base_laser_body</canonicalBody>
- <body:box name="base_laser_body">
- <xyz>0.5 0.0 0.00</xyz>
- <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>
- <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>
-
- </geom:box>
- <geom:cylinder name="base_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>
- <scale>0.04 0.04 0.013</scale>
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Black</material>
- </visual>
-
- </geom:cylinder>
-
- <geom:cylinder name="base_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.036 0.036 0.009</size>
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Black</material>
- </visual>
-
- </geom:cylinder>
-
- <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/Grey</material>
- </visual>
-
- </geom:cylinder>
-
-
- <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>
- <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 -->
- <joint:hinge name="hokuyo_base_y_joint">
- <body1>base_body</body1>
- <body2>base_laser_body</body2>
- <anchor>base_laser_body</anchor>
- <axis> 0.0 1.0 0.0 </axis>
- </joint:hinge>
- <joint:hinge name="hokuyo_base_z_joint">
- <body1>base_body</body1>
- <body2>base_laser_body</body2>
- <anchor>base_laser_body</anchor>
- <axis> 0.0 0.0 1.0 </axis>
- </joint:hinge>
-
-
-
- <!-- neck body -->
- <canonicalBody>neck_body</canonicalBody>
- <body:box name="neck_body">
- <xyz>0.15 0.0 1.08</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>
- <mass>0</mass>
- <visual>
- <mesh>unit_box</mesh>
- <size>.20 .38 .14</size>
- <material>Gazebo/Black</material>
- </visual>
- </geom:box>
- </body:box>
- <!-- attach neck to torso -->
- <!--
- <attach>
- <parentBody>torso_body</parentBody>
- <myBody>neck_body</myBody>
- </attach>
- -->
- <joint:hinge name="fixed_neck_body_1">
- <body1>torso_body</body1>
- <body2>neck_body</body2>
- <anchor>torso_body</anchor>
- <axis> 1.0 0.0 0.0 </axis>
- </joint:hinge>
- <joint:hinge name="fixed_neck_body_2">
- <body1>torso_body</body1>
- <body2>neck_body</body2>
- <anchor>torso_body</anchor>
- <axis> 0.0 1.0 0.0 </axis>
- </joint:hinge>
- <joint:hinge name="fixed_neck_body_3">
- <body1>torso_body</body1>
- <body2>neck_body</body2>
- <anchor>torso_body</anchor>
- <axis> 0.0 0.0 1.0 </axis>
- </joint:hinge>
-
- <canonicalBody>head_base_body</canonicalBody>
- <body:box name="head_base_body">
- <xyz>0.15 0.0 1.18 </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>
- <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.15 0.0 1.12 </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>
- <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/Grey</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/Grey</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 to neck -->
- <joint:hinge name="neck_yaw_joint">
- <body1>neck_body</body1>
- <body2>head_base_body</body2>
- <anchor>neck_body</anchor>
- <axis> 0.0 0.0 1.0 </axis>
- </joint:hinge>
-
-
- <!-- left camera -->
- <body:box name="head_cam_left_body">
- <xyz> 0.20 0.23 1.12 </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>
- <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>
- <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.20 0.20 1.12 </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/Grey</material>
- </visual>
- </geom:box>
- </body:box>
-
- <!-- right camera -->
- <body:empty name="head_cam_right_body">
- <xyz> 0.20 -0.23 1.12 </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>
- <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>
- <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.20 -0.20 1.12 </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>
- <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/Grey</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>
-
-
- <!-- stereo camera -->
- <body:empty name="stereo_camera_body">
- <xyz> 0.15 0.0 1.3 </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>
- <controller:stereo_camera name="stereo_camera_controller">
- <updateRate>15.0</updateRate>
- <interface:stereocamera name="stereo_iface_0" />
- <interface:camera name="camera_iface_0" />
- </controller:stereo_camera>
- </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>
- <!--color> 0.0 1.0 0.0</color-->
- <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>
-
- <!-- 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>
-
-
-
- <!-- ========== dummy torso for prismatic joint =========== -->
- <canonicalBody>dummytorso_body</canonicalBody>
-
- <body:box name="dummytorso_body">
- <xyz>0.056 0.0 0.5</xyz>
- <rpy>0.0 0.0 0.0</rpy>
-
- <!-- report when this body collides with static bodies -->
- <reportStaticCollision>true</reportStaticCollision>
-
- <geom:box name="dummy_torso_geom">
- <xyz> 0.0 0.0 0.0 </xyz>
- <size> 0.01 0.01 0.01 </size>
- <mass>1.0</mass>
- <visual>
- <size> 0.01 0.01 0.01</size>
- <mesh>unit_box</mesh>
- <material>Gazebo/PioneerBody</material>
- </visual>
- </geom:box>
-
- </body:box>
-
- <joint:hinge name="tmp1dummy">
- <body1>base_body</body1>
- <body2>dummytorso_body</body2>
- <anchor>dummytorso_body</anchor>
- <axis> 1.0 0.0 0.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
- </joint:hinge>
-
- <joint:hinge name="tmp2dummy">
- <body1>base_body</body1>
- <body2>dummytorso_body</body2>
- <anchor>dummytorso_body</anchor>
- <axis> 0.0 1.0 0.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
- </joint:hinge>
-
- <joint:hinge name="tmp3dummy">
- <body1>base_body</body1>
- <body2>dummytorso_body</body2>
- <anchor>dummytorso_body</anchor>
- <axis> 0.0 0.0 1.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
- </joint:hinge>
-
-
-
- <!-- ========== spine and shoulder =========== -->
- <canonicalBody>torso_body</canonicalBody>
-
- <body:box name="torso_body">
- <xyz>0.056 0.0 0.5</xyz>
- <rpy>0.0 0.0 0.0</rpy>
-
- <!-- report when this body collides with static bodies -->
- <reportStaticCollision>true</reportStaticCollision>
-
- <geom:box name="torso_geom">
- <xyz> 0.0 0.0 0.0 </xyz>
- <size>0.1 0.2 1.0 </size>
- <mass>1</mass>
- <visual>
- <size>0.1 0.2 1.0 </size>
- <mesh>unit_box</mesh>
- <material>Gazebo/Grey</material>
- </visual>
- </geom:box>
-
- <geom:box name="shoulder_geom">
- <xyz> 0.1 0.00 0.51</xyz>
- <size>0.3 0.65 0.02</size>
- <mass>1</mass>
- <visual>
- <size>0.3 0.65 0.02</size>
- <mesh>unit_box</mesh>
- <material>Gazebo/Grey</material>
- </visual>
- </geom:box>
-
- </body:box>
-
- <joint:slider name="torso_spine_slider">
- <body2>dummytorso_body</body2>
- <body1>torso_body</body1>
- <anchor>torso_body</anchor>
- <lowStop> 0.0 </lowStop>
- <hiStop> 0.5 </hiStop>
- <axis>0.0 0.0 1.0 </axis>
- </joint:slider>
-
- <!-- ========== Right Arm =========== -->
- <body:cylinder name="shoulder_pan_body_right">
- <xyz> 0.185 -0.15 0.68 </xyz>
- <rpy> 0.0 0.0 0.0 </rpy>
- <geom:cylinder name="shoulder_pan_body_right_geom">
- <size> 0.13 0.6</size> <!-- radius and length -->
- <mass>1.0</mass>
- <visual>
- <xyz> 0.0 0.0 0.0 </xyz>
- <rpy> 0.0 0.0 0.0 </rpy>
- <size> 0.26 0.26 0.6</size> <!-- bounding box -->
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Grey</material>
- </visual>
- </geom:cylinder>
- </body:cylinder>
-
- <body:cylinder name="upperarm_joint_right">
- <xyz> 0.285 -0.15 0.68 </xyz>
- <rpy> 90.0 0.0 0.0 </rpy>
- <geom:cylinder name="upperarm_joint_right_geom">
- <size> 0.10 0.05</size> <!-- radius and length -->
- <mass>1.0</mass>
- <visual>
- <xyz> 0.0 0.0 0.0 </xyz>
- <rpy> 0.0 0.0 0.0 </rpy>
- <size> 0.20 0.20 0.1</size>
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Grey</material>
- </visual>
- </geom:cylinder>
- </body:cylinder>
-
- <body:box name="upperarm_right">
- <xyz> 0.485 -0.15 0.68 </xyz>
- <rpy> 0.0 0.0 0.0 </rpy>
- <geom:box name="upperarm_right_geom">
- <size> 0.25 0.1 0.1</size>
- <mass>1.0</mass>
- <visual>
- <size> 0.25 0.1 0.1</size>
- <mesh>unit_box</mesh>
- <material>Gazebo/Grey</material>
- </visual>
- </geom:box>
- </body:box>
-
- <body:cylinder name="elbow_right">
- <xyz> 0.6850 -0.15 0.68 </xyz>
- <rpy> 90.0 0.0 0.0 </rpy>
- <geom:cylinder name="elbow_right_geom">
- <size> 0.04 0.1</size> <!-- radius and length -->
- <mass>1.0</mass>
- <visual>
- <size> 0.08 0.08 0.1</size> <!-- bounding box -->
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Grey</material>
- </visual>
- </geom:cylinder>
- </body:cylinder>
-
- <body:box name="forearm_right">
- <xyz> 0.84225 -0.15 0.68 </xyz>
- <rpy> 0.0 0.0 0.0 </rpy>
- <geom:box name="forearm_right_geom">
- <size> 0.2 0.1 0.1</size>
- <mass>1.0</mass>
- <visual>
- <size> 0.2 0.1 0.1</size>
- <mesh>unit_box</mesh>
- <material>Gazebo/Grey</material>
- </visual>
- </geom:box>
- </body:box>
-
- <body:cylinder name="wrist_right">
- <xyz> 0.9995 -0.15 0.68 </xyz>
- <rpy> 90.0 0.0 0.0 </rpy>
- <geom:cylinder name="wrist_right_geom">
- <size> 0.025 0.1</size> <!-- radius and length -->
- <mass>1.0</mass>
- <visual>
- <size>0.05 0.05 0.1</size> <!-- bounding box -->
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Grey</material>
- </visual>
- </geom:cylinder>
- </body:cylinder>
-
- <body:box name="palm_right">
- <xyz> 1.05 -0.15 0.68</xyz>
- <rpy> 0.0 0.0 0.0 </rpy>
- <geom:box name="palm_right_geom">
- <size> 0.05 0.05 0.05</size>
- <mass>1.0</mass>
- <visual>
- <size> 0.05 0.05 0.05</size>
- <mesh>unit_box</mesh>
- <material>Gazebo/Grey</material>
- </visual>
- </geom:box>
- </body:box>
-
- <body:box name="finger_1_right">
- <xyz> 1.15 -0.17 0.68 </xyz>
- <rpy> 0.0 0.0 0.0 </rpy>
- <geom:box name="finger_1_right_geom">
- <size> 0.05 0.01 0.025</size>
- <mass>1.0</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> 1.15 -0.13 0.68 </xyz>
- <rpy> 0.0 0.0 0.0 </rpy>
- <geom:box name="finger_2_right_geom">
- <size> 0.05 0.01 0.025</size>
- <mass>1.0</mass>
- <visual>
- <size> 0.05 0.01 0.025</size>
- <mesh>unit_box</mesh>
- <material>Gazebo/White</material>
- </visual>
- </geom:box>
- </body:box>
-
-
-
-
-
-
- <joint:hinge name="shoulder_pan_right">
- <body1>shoulder_pan_body_right</body1>
- <body2>torso_body</body2>
- <anchor>shoulder_pan_body_right</anchor>
- <axis> 0.0 0.0 1.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
- </joint:hinge>
-
- <joint:hinge name="upperarm_pitch_joint_right">
- <body1>upperarm_joint_right</body1>
- <body2>shoulder_pan_body_right</body2>
- <anchor>upperarm_joint_right</anchor>
- <axis> 0.0 1.0 0.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
- </joint:hinge>
-
- <joint:hinge name="upperarm_roll_joint_right">
- <body1>upperarm_right</body1>
- <body2>upperarm_joint_right</body2>
- <anchor>upperarm_joint_right</anchor>
- <axis> 1.0 0.0 0.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
- </joint:hinge>
-
- <joint:hinge name="elbow_pitch_joint_right">
- <body1>elbow_right</body1>
- <body2>upperarm_right</body2>
- <anchor>elbow_right</anchor>
- <axis> 0.0 1.0 0.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
- </joint:hinge>
-
- <joint:hinge name="forearm_roll_joint_right">
- <body1>forearm_right</body1>
- <body2>elbow_right</body2>
- <anchor>elbow_right</anchor>
- <axis> 1.0 0.0 0.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
- </joint:hinge>
-
- <joint:hinge name="wrist_pitch_joint_right">
- <body1>wrist_right</body1>
- <body2>forearm_right</body2>
- <anchor>wrist_right</anchor>
- <axis> 0.0 1.0 0.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
- </joint:hinge>
-
- <joint:hinge name="palm_roll_joint_right">
- <body1>palm_right</body1>
- <body2>wrist_right</body2>
- <anchor>wrist_right</anchor>
- <axis> 1.0 0.0 0.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
- </joint:hinge>
-
- <joint:slider name="finger_1_slider_right">
- <body2>finger_1_right</body2>
- <body1>palm_right</body1>
- <anchor>palm_right</anchor>
- <lowStop> -0.02 </lowStop> <!-- red close -->
- <hiStop> 0.05 </hiStop> <!-- red open -->
- <axis>0.0 1.0 0.0 </axis>
- </joint:slider>
-
- <joint:slider name="finger_2_slider_right">
- <body2>finger_2_right</body2>
- <body1>palm_right</body1>
- <anchor>palm_right</anchor>
- <lowStop> -0.05 </lowStop> <!-- white open -->
- <hiStop> 0.02 </hiStop> <!-- white close -->
- <axis>0.0 1.0 0.0 </axis>
- </joint:slider>
-
- <!-- ========== Left Arm =========== -->
-
- <body:cylinder name="shoulder_pan_body_left">
- <xyz> 0.185 0.15 0.68 </xyz>
- <rpy> 0.0 0.0 0.0 </rpy>
- <geom:cylinder name="shoulder_pan_body_left_geom">
- <size> 0.13 0.6</size> <!-- radius and length -->
- <mass>1.0</mass>
- <visual>
- <xyz> 0.0 0.0 0.0 </xyz>
- <rpy> 0.0 0.0 0.0 </rpy>
- <size> 0.26 0.26 0.6</size> <!-- bounding box -->
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Grey</material>
- </visual>
- </geom:cylinder>
- </body:cylinder>
-
- <body:cylinder name="upperarm_joint_left">
- <xyz> 0.285 0.15 0.68 </xyz>
- <rpy> 90.0 0.0 0.0 </rpy>
- <geom:cylinder name="upperarm_joint_left_geom">
- <size> 0.10 0.05</size> <!-- radius and length -->
- <mass>1.0</mass>
- <visual>
- <xyz> 0.0 0.0 0.0 </xyz>
- <rpy> 0.0 0.0 0.0 </rpy>
- <size> 0.20 0.20 0.1</size>
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Grey</material>
- </visual>
- </geom:cylinder>
- </body:cylinder>
-
- <body:box name="upperarm_left">
- <xyz> 0.485 0.15 0.68 </xyz>
- <rpy> 0.0 0.0 0.0 </rpy>
- <geom:box name="upperarm_left_geom">
- <size> 0.25 0.1 0.1</size>
- <mass>1.0</mass>
- <visual>
- <size> 0.25 0.1 0.1</size>
- <mesh>unit_box</mesh>
- <material>Gazebo/Grey</material>
- </visual>
- </geom:box>
- </body:box>
-
- <body:cylinder name="elbow_left">
- <xyz> 0.685 0.15 0.68 </xyz>
- <rpy> 90.0 0.0 0.0 </rpy>
- <geom:cylinder name="elbow_left_geom">
- <size> 0.04 0.1</size> <!-- radius and length -->
- <mass>1.0</mass>
- <visual>
- <size> 0.08 0.08 0.1</size> <!-- bounding box -->
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Grey</material>
- </visual>
- </geom:cylinder>
- </body:cylinder>
-
- <body:box name="forearm_left">
- <xyz> 0.84225 0.15 0.68 </xyz>
- <rpy> 0.0 0.0 0.0 </rpy>
- <geom:box name="forearm_left_geom">
- <size> 0.2 0.1 0.1</size>
- <mass>1.0</mass>
- <visual>
- <size> 0.2 0.1 0.1</size>
- <mesh>unit_box</mesh>
- <material>Gazebo/Grey</material>
- </visual>
- </geom:box>
- </body:box>
-
- <body:cylinder name="wrist_left">
- <xyz> 0.9995 0.15 0.68 </xyz>
- <rpy> 90.0 0.0 0.0 </rpy>
- <geom:cylinder name="wrist_left_geom">
- <size> 0.025 0.1</size> <!-- radius and length -->
- <mass>1.0</mass>
- <visual>
- <size>0.05 0.05 0.1</size> <!-- bounding box -->
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Grey</material>
- </visual>
- </geom:cylinder>
- </body:cylinder>
-
- <body:box name="palm_left">
- <xyz> 1.05 0.15 0.68 </xyz>
- <rpy> 0.0 0.0 0.0 </rpy>
- <geom:box name="palm_left_geom">
- <size> 0.05 0.05 0.05</size>
- <mass>1.0</mass>
- <visual>
- <size> 0.05 0.05 0.05</size>
- <mesh>unit_box</mesh>
- <material>Gazebo/Grey</material>
- </visual>
- </geom:box>
- </body:box>
-
- <body:box name="finger_1_left">
- <xyz> 1.15 0.13 0.68 </xyz>
- <rpy> 0.0 0.0 0.0 </rpy>
- <geom:box name="finger_1_left_geom">
- <size> 0.05 0.01 0.025</size>
- <mass>1.0</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> 1.15 0.17 0.68 </xyz>
- <rpy> 0.0 0.0 0.0 </rpy>
- <geom:box name="finger_2_left_geom">
- <size> 0.05 0.01 0.025</size>
- <mass>1.0</mass>
- <visual>
- <size> 0.05 0.01 0.025</size>
- <mesh>unit_box</mesh>
- <material>Gazebo/White</material>
- </visual>
- </geom:box>
- </body:box>
-
-
- <joint:hinge name="shoulder_pan_left">
- <body1>shoulder_pan_body_left</body1>
- <body2>torso_body</body2>
- <anchor>shoulder_pan_body_left</anchor>
- <axis> 0.0 0.0 1.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
- </joint:hinge>
-
- <joint:hinge name="upperarm_pitch_joint_left">
- <body1>upperarm_joint_left</body1>
- <body2>shoulder_pan_body_left</body2>
- <anchor>upperarm_joint_left</anchor>
- <axis> 0.0 1.0 0.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
- </joint:hinge>
-
- <joint:hinge name="upperarm_roll_joint_left">
- <body1>upperarm_left</body1>
- <body2>upperarm_joint_left</body2>
- <anchor>upperarm_joint_left</anchor>
- <axis> 1.0 0.0 0.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
- </joint:hinge>
-
- <joint:hinge name="elbow_pitch_joint_left">
- <body1>elbow_left</body1>
- <body2>upperarm_left</body2>
- <anchor>elbow_left</anchor>
- <axis> 0.0 1.0 0.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
- </joint:hinge>
-
- <joint:hinge name="forearm_roll_joint_left">
- <body1>forearm_left</body1>
- <body2>elbow_left</body2>
- <anchor>elbow_left</anchor>
- <axis> 1.0 0.0 0.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
- </joint:hinge>
-
- <joint:hinge name="wrist_pitch_joint_left">
- <body1>wrist_left</body1>
- <body2>forearm_left</body2>
- <anchor>wrist_left</anchor>
- <axis> 0.0 1.0 0.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
- </joint:hinge>
-
- <joint:hinge name="palm_roll_joint_left">
- <body1>palm_left</body1>
- <body2>wrist_left</body2>
- <anchor>wrist_left</anchor>
- <axis> 1.0 0.0 0.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
- </joint:hinge>
-
- <joint:slider name="finger_1_slider_left">
- <body2>finger_1_left</body2>
- <body1>palm_left</body1>
- <anchor>palm_left</anchor>
- <lowStop> -0.02 </lowStop> <!-- red close -->
- <hiStop> 0.05 </hiStop> <!-- red open -->
- <axis>0.0 1.0 0.0 </axis>
- </joint:slider>
-
- <joint:slider name="finger_2_slider_left">
- <body2>finger_2_left</body2>
- <body1>palm_left</body1>
- <anchor>palm_left</anchor>
- <lowStop> -0.05 </lowStop> <!-- white open -->
- <hiStop> 0.02 </hiStop> <!-- white close -->
- <axis>0.0 1.0 0.0 </axis>
- </joint:slider>
-
-
-
- <!--=========================== 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.062 </xyz>
- <rpy> 0.0 0.0 0.0 </rpy>
-
- <!-- report when this body collides with static bodies -->
- <reportStaticCollision>true</reportStaticCollision>
-
- <geom:box name="front_left_caster_geom">
- <xyz> 0.0 0.0 0.0</xyz>
- <size>0.1 0.08 0.1</size> <!-- dx, dy, dz -->
- <mass>1.0</mass>
- <visual>
- <size>0.1 0.08 0.1</size> <!-- dx, dy, dz -->
- <mesh>unit_box</mesh>
- <material>Gazebo/Grey</material>
- </visual>
- </geom:box>
- </body:box>
-
- <body:box name="front_right_caster_body">
- <xyz> 0.425 -0.25 -0.062</xyz>
- <rpy> 0.0 0.0 0.0</rpy>
-
- <!-- report when this body collides with static bodies -->
- <reportStaticCollision>true</reportStaticCollision>
-
- <geom:box name="front_right_caster_geom">
- <xyz>0 0 0.0</xyz>
- <size>0.1 0.08 0.1</size> <!-- dx, dy, dz -->
- <mass>1.0</mass>
- <visual>
- <size>0.1 0.08 0.1</size> <!-- dx, dy, dz -->
- <mesh>unit_box</mesh>
- <material>Gazebo/Grey</material>
- </visual>
- </geom:box>
- </body:box>
-
- <body:box name="rear_left_caster_body">
- <xyz>-0.075 0.25 -0.062</xyz>
- <rpy> 0.0 0.0 0.0</rpy>
-
- <!-- report when this body collides with static bodies -->
- <reportStaticCollision>true</reportStaticCollision>
-
- <geom:box name="rear_left_caster_geom">
- <xyz>0 0 0.0</xyz>
- <size>0.1 0.08 0.1</size> <!-- dx, dy, dz -->
- <mass>1.0</mass>
- <visual>
- <size>0.1 0.08 0.1</size> <!-- dx, dy, dz -->
- <mesh>unit_box</mesh>
- <material>Gazebo/Grey</material>
- </visual>
- </geom:box>
- </body:box>
-
- <body:box name="rear_right_caster_body">
- <xyz>-0.075 -0.25 -0.062</xyz>
- <rpy> 0.0 0.0 0.0</rpy>
- <!-- report when this body collides with static bodies -->
- <reportStaticCollision>true</reportStaticCollision>
-
- <geom:box name="rear_right_caster_geom">
- <xyz>0 0 0.0</xyz>
- <size>0.1 0.08 0.1</size> <!-- dx, dy, dz -->
- <mass>1.0</mass>
- <visual>
- <size>0.1 0.08 0.1</size> <!-- dx, dy, dz -->
- <mesh>unit_box</mesh>
- <material>Gazebo/Grey</material>
- </visual>
- </geom:box>
- </body:box>
-
-
- <!--=========================== wheels ===========================-->
- <body:cylinder name="front_left_wheel_l">
- <xyz> 0.425 0.305 -0.17</xyz>
- <rpy> 90.0 0.0 1.0 </rpy>
- <geom:cylinder name="front_left_wheel_l_geom">
- <size> 0.08 0.03</size> <!-- radius and width -->
- <mass>0.5</mass>
- <visual>
- <size>0.16 0.16 0.03</size> <!-- bounding box -->
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Black</material>
- </visual>
- </geom:cylinder>
- </body:cylinder>
-
- <body:cylinder name="front_left_wheel_r">
- <xyz> 0.425 0.195 -0.17</xyz>
- <rpy> 90.0 0.0 1.0 </rpy>
- <geom:cylinder name="front_left_wheel_r_geom">
- <size> 0.08 0.03</size> <!-- radius and width -->
- <mass>0.5</mass>
- <visual>
- <size>0.16 0.16 0.03</size> <!-- bounding box -->
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Black</material>
- </visual>
- </geom:cylinder>
- </body:cylinder>
-
- <body:cylinder name="front_right_wheel_l">
- <xyz> 0.425 -0.195 -0.17</xyz>
- <rpy> 90.0 0.0 0.0 </rpy>
- <geom:cylinder name="front_right_wheel_l_geom">
- <size> 0.08 0.03</size> <!-- radius and width -->
- <mass>0.5</mass>
- <visual>
- <size>0.16 0.16 0.03</size> <!-- bounding box -->
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Black</material>
- </visual>
- </geom:cylinder>
- </body:cylinder>
-
- <body:cylinder name="front_right_wheel_r">
- <xyz> 0.425 -0.305 -0.17</xyz>
- <rpy> 90.0 0.0 0.0 </rpy>
- <geom:cylinder name="front_right_wheel_r_geom">
- <size> 0.08 0.03</size> <!-- radius and width -->
- <mass>0.5</mass>
- <visual>
- <size>0.16 0.16 0.03</size> <!-- bounding box -->
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Black</material>
- </visual>
- </geom:cylinder>
- </body:cylinder>
-
- <body:cylinder name="rear_left_wheel_l">
- <xyz> -0.075 0.305 -0.17</xyz>
- <rpy> 90.0 0.0 0.0 </rpy>
- <geom:cylinder name="rear_left_wheel_l_geom">
- <size> 0.08 0.03</size> <!-- radius and width -->
- <mass>0.5</mass>
- <visual>
- <size>0.16 0.16 0.03</size> <!-- bounding box -->
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Black</material>
- </visual>
- </geom:cylinder>
- </body:cylinder>
-
- <body:cylinder name="rear_left_wheel_r">
- <xyz> -0.075 0.195 -0.17</xyz>
- <rpy> 90.0 0.0 0.0 </rpy>
- <geom:cylinder name="rear_left_wheel_r_geom">
- <size> 0.08 0.03</size> <!-- radius and width -->
- <mass>0.5</mass>
- <visual>
- <size>0.16 0.16 0.03</size> <!-- bounding box -->
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Black</material>
- </visual>
- </geom:cylinder>
- </body:cylinder>
-
- <body:cylinder name="rear_right_wheel_l">
- <xyz> -0.075 -0.195 -0.17</xyz>
- <rpy> 90.0 0.0 0.0 </rpy>
- <geom:cylinder name="rear_right_wheel_l_geom">
- <size> 0.08 0.03</size> <!-- radius and width -->
- <mass>0.5</mass>
- <visual>
- <size>0.16 0.16 0.03</size> <!-- bounding box -->
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Black</material>
- </visual>
- </geom:cylinder>
- </body:cylinder>
-
- <body:cylinder name="rear_right_wheel_r">
- <xyz> -0.075 -0.305 -0.17</xyz>
- <rpy> 90.0 0.0 0.0 </rpy>
- <geom:cylinder name="rear_right_wheel_r_geom">
- <size> 0.08 0.03</size> <!-- radius and width -->
- <mass>0.5</mass>
- <visual>
- <size>0.16 0.16 0.03</size> <!-- bounding box -->
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Black</material>
- </visual>
- </geom:cylinder>
- </body:cylinder>
-
-
- <!--=========================== joints ===========================-->
-
- <joint:hinge name="front_left_wheel_l_drive">
- <body1>front_left_caster_body</body1>
- <body2>front_left_wheel_l</body2>
- <anchor>front_left_wheel_l</anchor>
- <axis>0 1 0</axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
- </joint:hinge>
-
- <joint:hinge name="front_left_wheel_r_drive">
- <body1>front_left_caster_body</body1>
- <body2>front_left_wheel_r</body2>
- <anchor>front_left_wheel_r</anchor>
- <axis>0 1 0</axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
- </joint:hinge>
-
- <joint:hinge name="front_right_wheel_l_drive">
- <body1>front_right_caster_body</body1>
- <body2>front_right_wheel_l</body2>
- <anchor>front_right_wheel_l</anchor>
- <axis>0 1 0</axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
- </joint:hinge>
-
- <joint:hinge name="front_right_wheel_r_drive">
- <body1>front_right_caster_body</body1>
- <body2>front_right_wheel_r</body2>
- <anchor>front_right_wheel_r</anchor>
- <axis>0 1 0</axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
- </joint:hinge>
-
- <joint:hinge name="rear_left_wheel_l_drive">
- <body1>rear_left_caster_body</body1>
- <body2>rear_left_wheel_l</body2>
- <anchor>rear_left_wheel_l</anchor>
- <axis>0 1 0</axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
- </joint:hinge>
-
- <joint:hinge name="rear_left_wheel_r_drive">
- <body1>rear_left_caster_body</body1>
- <body2>rear_left_wheel_r</body2>
- <anchor>rear_left_wheel_r</anchor>
- <axis>0 1 0</axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
- </joint:hinge>
-
- <joint:hinge name="rear_right_wheel_l_drive">
- <body1>rear_right_caster_body</body1>
- <body2>rear_right_wheel_l</body2>
- <anchor>rear_right_wheel_l</anchor>
- <axis>0 1 0</axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
- </joint:hinge>
-
- <joint:hinge name="rear_right_wheel_r_drive">
- <body1>rear_right_caster_body</body1>
- <body2>rear_right_wheel_r</body2>
- <anchor>rear_right_wheel_r</anchor>
- <axis>0 1 0</axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
- </joint:hinge>
-
-
-
-
- <joint:hinge name="front_left_caster_steer">
- <body1>front_left_caster_body</body1>
- <body2>base_body</body2>
- <anchor>front_left_caster_body</anchor>
- <axis> 0.0 0.0 1.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
- </joint:hinge>
-
- <joint:hinge name="front_right_caster_steer">
- <body1>front_right_caster_body</body1>
- <body2>base_body</body2>
- <anchor>front_right_caster_body</anchor>
- <axis> 0.0 0.0 1.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
- </joint:hinge>
-
- <joint:hinge name="rear_left_caster_steer">
- <body1>rear_left_caster_body</body1>
- <body2>base_body</body2>
- <anchor>rear_left_caster_body</anchor>
- <axis> 0.0 0.0 1.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
- </joint:hinge>
-
- <joint:hinge name="rear_right_caster_steer">
- <body1>rear_right_caster_body</body1>
- <body2>base_body</body2>
- <anchor>rear_right_caster_body</anchor>
- <axis> 0.0 0.0 1.0 </axis>
- <cfm>0.001</cfm>
- <erp>0.6</erp>
- </joint:hinge>
-
-
-
- <!-- Hokuyo laser body -->
- <canonicalBody>laser_body</canonicalBody>
- <body:box name="laser_body">
- <xyz>0.3 0.0 1.05</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>
-
- </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>
- <scale>0.04 0.04 0.013</scale>
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Black</material>
- </visual>
-
- </geom:cylinder>
-
- <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.036 0.036 0.009</size>
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Black</material>
- </visual>
-
- </geom:cylinder>
-
- <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.035 0.035 0.008</size>
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/Grey</material>
- </visual>
-
- </geom:cylinder>
-
-
- <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>
-
- <minRange>0.05</minRange>
- <maxRange>10.0</maxRange>
- <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>
-
- <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 -->
- <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>
-
-</model:physical>
Deleted: pkg/trunk/simulators/rosgazebo/world/robot.world
===================================================================
--- pkg/trunk/simulators/rosgazebo/world/robot.world 2008-06-13 01:35:12 UTC (rev 799)
+++ pkg/trunk/simulators/rosgazebo/world/robot.world 2008-06-13 02:13:00 UTC (rev 800)
@@ -1,634 +0,0 @@
-<?xml version="1.0"?>
-
-<gazebo:world
- xmlns:xi="http://www.w3.org/2001/XInclude"
- xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz"
- xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model"
- xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
- xmlns:window="http://playerstage.sourceforge.net/gazebo/xmlschema/#window"
- xmlns:param="http://playerstage.sourceforge.net/gazebo/xmlschema/#param"
- xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
- xmlns:geo="http://willowgarage.com/xmlschema/#geo"
- xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
- xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
- xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
- xmlns:ui="http://playerstage.sourceforge.net/gazebo/xmlschema/#ui"
- xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering"
- xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable"
- xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
- xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics" >
-
- <verbosity>5</verbosity>
-
-<!-- cfm is 1e-5 for single precision -->
-<!-- erp is typically .1-.8 -->
- <physics:ode>
- <stepTime>0.01</stepTime>
- <gravity>0 0 -9.8</gravity>
- <cfm>0.001</cfm>
- <erp>0.8</erp>
- </physics:ode>
-
- <geo:origin>
- <lat>37.4270909558</lat><lon>-122.077919338</lon>
- </geo:origin>
-
- <rendering:gui>
- <type>fltk</type>
- <size>1024 800</size>
- <pos>0 0</pos>
- </rendering:gui>
-
-
- <rendering:ogre>
- <ambient>1.0 1.0 1.0 1.0</ambient>
- <sky>
- <material>Gazebo/CloudySky</material>
- </sky>
- <gazeboPath>media</gazeboPath>
- <grid>off</grid>
- <desiredFPS>10</desiredFPS>
- </rendering:ogre>
-
-
-<!--
- <model:physical name="gbuilding">
- <body:heightmap name="building">
- <geom:heightmap name="building">
- <image>map.png</image>
- <worldTexture>map2.jpg</worldTexture>
- <size>2 2 0.5</size>
- </geom:heightmap>
- </body:heightmap>
- </model:physical>
--->
- <model:physical name="gplane">
- <xyz>0 0 0</xyz>
- <rpy>0 0 0</rpy>
- <static>true</static>
-
- <body:plane name="plane">
- <geom:plane name="plane">
- <normal>0 0 1</normal>
- <size>51.3 51.3</size>
- <!-- map3.png -->
- <material>PR2/floor_texture</material>
- </geom:plane>
- </body:plane>
- </model:physical>
-<!--
--->
-
-
-
- <!-- The camera -->
- <model:physical name="cam1_model">
- <xyz> 3.0 0.0 1.0</xyz>
- <rpy> 0.0 0.0 180.0</rpy>
- <static>true</static>
-
- <body:empty name="cam1_body">
- <sensor:camera name="cam1_sensor">
- <imageSize>1024 800</imageSize>
- <hfov>90</hfov>
- ...
[truncated message content] |
