Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[3362] pkg/trunk
|
From: <hsu...@us...> - 2008-08-20 22:30:33
|
Revision: 3362
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3362&view=rev
Author: hsujohnhsu
Date: 2008-08-20 22:30:40 +0000 (Wed, 20 Aug 2008)
Log Message:
-----------
updated experimental tester plugin.
Modified Paths:
--------------
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/test_actuators.h
pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp
pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo_test.xml
Added Paths:
-----------
pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo_joints.xml
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/test_actuators.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/test_actuators.h 2008-08-20 22:29:33 UTC (rev 3361)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/test_actuators.h 2008-08-20 22:30:40 UTC (rev 3362)
@@ -178,9 +178,17 @@
struct Gazebo_joint_
{
std::string* name_;
- gazebo::Joint* joint_;
+ std::vector<gazebo::Joint*> gaz_joints_;
+ mechanism::Joint* rmc_joint_;
+ double saturationTorque;
+ double explicitDampingCoefficient;
+ bool isGripper;
+ std::string* gripper_controller_name_;
+ std::vector<controller::Pid*> gaz_gripper_pids_;
};
std::vector<Gazebo_joint_*> gazebo_joints_;
+ double currentTime;
+ double lastTime;
// for storing transmission xml
// struct Robot_transmission_
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp 2008-08-20 22:29:33 UTC (rev 3361)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp 2008-08-20 22:30:40 UTC (rev 3362)
@@ -120,6 +120,10 @@
// TODO: mc_.init();
hw_.current_time_ = Simulator::Instance()->GetSimTime();
+
+ // for internal gripper pid use
+ currentTime = Simulator::Instance()->GetSimTime();
+ lastTime = Simulator::Instance()->GetSimTime();
}
void GazeboActuators::UpdateChild()
@@ -196,12 +200,112 @@
//-----------------------------------------------------------------------------------------
//
- // parse for joints
+ // parse for MechanismControl joints
//
//-----------------------------------------------------------------------------------------
mcn_.initXml(pr2_xml->FirstChildElement("robot"));
rmcn_.initXml(pr2_xml->FirstChildElement("robot"));
+ //-----------------------------------------------------------------------------------------
+ //
+ // how the mechanism joints relate to the gazebo_joints
+ //
+ //-----------------------------------------------------------------------------------------
+ // The gazebo joints and mechanism joints should match up.
+ std::cout << " Loading gazebo joints : " << std::endl;
+ for (XMLConfigNode *jNode = node->GetChild("robot")->GetChild("joint"); jNode; )
+ {
+ std::string *joint_name = new std::string(jNode->GetString("name","",1));
+ std::cout << "processing mech joint (" << *joint_name << ") map to gazebo joint. " << std::endl;
+
+ // joint exist in model, proceed to create gazebo joint and mapping
+ Gazebo_joint_* gj = new Gazebo_joint_();
+ gj->name_ = joint_name;
+
+ // check for special cases -- Gripper!
+ if (!rmc_.model_.getJoint(*joint_name))
+ {
+ // joint does not exist in MechanismControl
+ std::cout << std::endl << "mech joint (" << *joint_name << ") does not exist in mechanism control, check pr2.xml. " << std::endl;
+ // create this joint inside MechanismControl if it is an abstract type like a gripper
+ if (jNode->GetString("abstract","",0)== "gripper")
+ {
+ mechanism::Joint *joint = new mechanism::Joint();
+ joint->name_ = joint_name->c_str();
+ joint->type_ = mechanism::JOINT_ROTARY;
+ joint->position_ = 0; // from transmission
+ joint->velocity_ = 0; // from transmission
+ joint->applied_effort_ = 0; // from transmission
+ joint->commanded_effort_ = 0; // to transmission
+ joint->joint_limit_min_ = 0;
+ joint->joint_limit_max_ = 0;
+ joint->effort_limit_ = jNode->GetDouble("effortLimit",0.0,0);
+ joint->velocity_limit_ = jNode->GetDouble("velocityLimit",0.0,0);
+ mc_.addJoint(joint);
+ rmc_.addJoint(joint);
+ // get controller name from xml
+ gj->gripper_controller_name_ = new std::string(jNode->GetString("gripper_controller","",1));
+ std::cout << " gripper controller name is : " << *(gj->gripper_controller_name_) << std::endl;
+ }
+ }
+
+ // add a link to the mechanism control joint
+ gj->rmc_joint_ = rmc_.model_.getJoint(*joint_name);
+
+ // read gazebo specific joint properties
+ gj->saturationTorque = jNode->GetDouble("saturationTorque",0.0,0);
+ gj->explicitDampingCoefficient = jNode->GetDouble("explicitDampingCoefficient",0.0,0);
+
+ // deal with special case -- Gripper!
+ if (jNode->GetString("abstract","",0) == "gripper")
+ gj->isGripper = true;
+ else
+ gj->isGripper = false;
+
+ if (gj->isGripper)
+ {
+ std::string f_l_joint_name = jNode->GetString("left_proximal","",1);
+ std::string f_r_joint_name = jNode->GetString("right_proximal","",1);
+ std::string f_tip_l_joint_name = jNode->GetString("left_distal","",1);
+ std::string f_tip_r_joint_name = jNode->GetString("right_distal","",1);
+ gazebo::HingeJoint* gj_f_l = (gazebo::HingeJoint*)parent_model_->GetJoint(f_l_joint_name) ;
+ gazebo::HingeJoint* gj_f_r = (gazebo::HingeJoint*)parent_model_->GetJoint(f_r_joint_name) ;
+ gazebo::HingeJoint* gj_f_tip_l = (gazebo::HingeJoint*)parent_model_->GetJoint(f_tip_l_joint_name);
+ gazebo::HingeJoint* gj_f_tip_r = (gazebo::HingeJoint*)parent_model_->GetJoint(f_tip_r_joint_name);
+ gj->gaz_joints_.push_back(gj_f_l );
+ gj->gaz_joints_.push_back(gj_f_r );
+ gj->gaz_joints_.push_back(gj_f_tip_l);
+ gj->gaz_joints_.push_back(gj_f_tip_r);
+
+ gj->gaz_gripper_pids_.push_back( new controller::Pid() ); gj->gaz_gripper_pids_.back()->initPid( 1.0, 0.01, 0.0, 0.2, -0.2);
+ gj->gaz_gripper_pids_.push_back( new controller::Pid() ); gj->gaz_gripper_pids_.back()->initPid( 1.0, 0.01, 0.0, 0.2, -0.2);
+ gj->gaz_gripper_pids_.push_back( new controller::Pid() ); gj->gaz_gripper_pids_.back()->initPid( 1.0, 0.01, 0.0, 0.2, -0.2);
+ gj->gaz_gripper_pids_.push_back( new controller::Pid() ); gj->gaz_gripper_pids_.back()->initPid( 1.0, 0.01, 0.0, 0.2, -0.2);
+
+ // initialize for torque control mode
+ gj_f_l ->SetParam(dParamVel , 0);
+ gj_f_l ->SetParam(dParamFMax, 0);
+ gj_f_r ->SetParam(dParamVel , 0);
+ gj_f_r ->SetParam(dParamFMax, 0);
+ gj_f_tip_l->SetParam(dParamVel , 0);
+ gj_f_tip_l->SetParam(dParamFMax, 0);
+ gj_f_tip_r->SetParam(dParamVel , 0);
+ gj_f_tip_r->SetParam(dParamFMax, 0);
+
+ }
+ else
+ {
+ gazebo::Joint* ggj = (gazebo::Joint*)parent_model_->GetJoint(*joint_name);
+ gj->gaz_joints_.push_back(ggj);
+ // initialize for torque control mode
+ ggj->SetParam(dParamVel , 0);
+ ggj->SetParam(dParamFMax, 0);
+
+ }
+
+ gazebo_joints_.push_back(gj);
+ jNode = jNode->GetNext("joint");
+ }
//-----------------------------------------------------------------------------------------
//
// ACTUATOR XML
@@ -268,58 +372,6 @@
}
- //-----------------------------------------------------------------------------------------
- //
- // how the mechanism_joints relate to the gazebo_joints
- //
- //-----------------------------------------------------------------------------------------
- // The gazebo joints and mechanism joints should match up.
- std::cout << " Loading gazebo joints : " << std::endl;
- for (unsigned int i = 0; i < rmc_.model_.joints_.size(); ++i)
- {
- Gazebo_joint_* gj = new Gazebo_joint_();
-
- std::string *joint_name = &rmc_.model_.joints_[i]->name_;
-
- gj->name_ = joint_name;
-
- std::cout << "adding gazebo joint: " << *(gj->name_) << std::endl;
-
- gazebo::Joint *joint = parent_model_->GetJoint(*joint_name);
- if (joint)
- {
-
- gj->joint_ = joint;
-
- if (joint->GetType() == gazebo::Joint::HINGE)
- {
- // initialize for torque control mode
- joint->SetParam(dParamVel , 0);
- joint->SetParam(dParamFMax, 0);
- }
- else if (joint->GetType() == gazebo::Joint::SLIDER)
- {
- // initialize for torque control mode
- joint->SetParam(dParamVel , 0);
- joint->SetParam(dParamFMax, 0);
- }
- else
- {
- // initialize for torque control mode
- joint->SetParam(dParamVel , 0);
- joint->SetParam(dParamFMax, 0);
- }
- }
- else
- {
- fprintf(stderr, "Gazebo does not know about a joint named \"%s\"\n", joint_name->c_str());
- gj->joint_ = NULL;
- }
-
- gazebo_joints_.push_back(gj);
- }
-
-
}
@@ -336,6 +388,7 @@
{
// pass time to robot
hw_.current_time_ = Simulator::Instance()->GetSimTime();
+ currentTime = Simulator::Instance()->GetSimTime();
this->lock.lock();
@@ -430,16 +483,51 @@
// update joint status from hardware
for (std::vector<Gazebo_joint_*>::iterator gji = gazebo_joints_.begin(); gji != gazebo_joints_.end() ; gji++)
{
- mechanism::Joint* mech_joint = rmc_.model_.getJoint(*((*gji)->name_));
+ // gripper joint, is an ugly special case for now
+ if ((*gji)->isGripper)
+ {
+ for (std::vector<gazebo::Joint*>::iterator ggji = (*gji)->gaz_joints_.begin(); ggji != (*gji)->gaz_joints_.end() ; ggji++)
+ {
- // push gazebo joint to reverse joints
- if ((*gji)->joint_ && (*gji)->joint_->GetType() == gazebo::Joint::HINGE)
+ }
+ gazebo::HingeJoint* gj_f_l = dynamic_cast<gazebo::HingeJoint*>((*gji)->gaz_joints_[0]);
+ gazebo::HingeJoint* gj_f_r = dynamic_cast<gazebo::HingeJoint*>((*gji)->gaz_joints_[1]);
+ gazebo::HingeJoint* gj_f_tip_l = dynamic_cast<gazebo::HingeJoint*>((*gji)->gaz_joints_[2]);
+ gazebo::HingeJoint* gj_f_tip_r = dynamic_cast<gazebo::HingeJoint*>((*gji)->gaz_joints_[3]);
+
+ (*gji)->rmc_joint_->position_ = gj_f_l->GetAngle();
+ (*gji)->rmc_joint_->velocity_ = gj_f_l->GetAngleRate();
+ (*gji)->rmc_joint_->applied_effort_ = (*gji)->rmc_joint_->commanded_effort_;
+
+ }
+ else
{
- mech_joint->position_ = dynamic_cast<gazebo::HingeJoint*>((*gji)->joint_)->GetAngle(); // use one joint as reference
- mech_joint->velocity_ = dynamic_cast<gazebo::HingeJoint*>((*gji)->joint_)->GetAngleRate();
- mech_joint->applied_effort_ = mech_joint->commanded_effort_;
+ // normal joints
+ switch((*gji)->gaz_joints_[0]->GetType())
+ {
+ case gazebo::Joint::SLIDER:
+ {
+ gazebo::SliderJoint* gjs = dynamic_cast<gazebo::SliderJoint*>((*gji)->gaz_joints_[0]);
+ (*gji)->rmc_joint_->position_ = gjs->GetPosition();
+ (*gji)->rmc_joint_->velocity_ = gjs->GetPositionRate();
+ (*gji)->rmc_joint_->applied_effort_ = (*gji)->rmc_joint_->commanded_effort_;
+ break;
+ }
+ case gazebo::Joint::HINGE:
+ {
+ gazebo::HingeJoint* gjh = dynamic_cast<gazebo::HingeJoint*>((*gji)->gaz_joints_[0]);
+ (*gji)->rmc_joint_->position_ = gjh->GetAngle();
+ (*gji)->rmc_joint_->velocity_ = gjh->GetAngleRate();
+ (*gji)->rmc_joint_->applied_effort_ = (*gji)->rmc_joint_->commanded_effort_;
+ break;
+ }
+ case gazebo::Joint::HINGE2:
+ case gazebo::Joint::BALL:
+ case gazebo::Joint::UNIVERSAL:
+ break;
+ }
+ }
- }
}
// push reverse_mech_joint_ stuff back toward actuators
for (unsigned int i=0; i < rmc_.model_.transmissions_.size(); i++)
@@ -515,15 +603,65 @@
// -------------------------------------------------------------------------------------------------
for (std::vector<Gazebo_joint_*>::iterator gji = gazebo_joints_.begin(); gji != gazebo_joints_.end() ; gji++)
{
- mechanism::Joint* mech_joint = mc_.model_.getJoint(*((*gji)->name_));
+ // gripper joint, is an ugly special case for now
+ if ((*gji)->isGripper)
+ {
+ double gripperCmd , currentError, currentCmd ;
+ // FIXME: this restricts gripper to a position controller... not ideal
+ std::string jn = *((*gji)->gripper_controller_name_ );
+ controller::Controller* jc = mc_.getControllerByName(jn); // from actual mechanism control, not rmc_
+ controller::JointPositionControllerNode* jpc = dynamic_cast<controller::JointPositionControllerNode*>(jc);
+ gripperCmd = jpc->getCommand();
- // push gazebo joint to reverse joints
- if ((*gji)->joint_ && (*gji)->joint_->GetType() == gazebo::Joint::HINGE)
+ currentError = math_utils::shortest_angular_distance(dynamic_cast<gazebo::HingeJoint*>((*gji)->gaz_joints_[0])->GetAngle(),gripperCmd);
+ currentCmd = (*gji)->gaz_gripper_pids_[0]->updatePid(currentError,currentTime-lastTime);
+ dynamic_cast<gazebo::HingeJoint*>((*gji)->gaz_joints_[0])->SetParam( dParamVel, currentCmd );
+ dynamic_cast<gazebo::HingeJoint*>((*gji)->gaz_joints_[0])->SetParam( dParamFMax, (*gji)->saturationTorque );
+
+ currentError = math_utils::shortest_angular_distance(dynamic_cast<gazebo::HingeJoint*>((*gji)->gaz_joints_[1])->GetAngle(),-gripperCmd);
+ currentCmd = (*gji)->gaz_gripper_pids_[1]->updatePid(currentError,currentTime-lastTime);
+ dynamic_cast<gazebo::HingeJoint*>((*gji)->gaz_joints_[1])->SetParam( dParamVel, currentCmd );
+ dynamic_cast<gazebo::HingeJoint*>((*gji)->gaz_joints_[1])->SetParam( dParamFMax, (*gji)->saturationTorque );
+
+ currentError = math_utils::shortest_angular_distance(dynamic_cast<gazebo::HingeJoint*>((*gji)->gaz_joints_[2])->GetAngle(),-gripperCmd);
+ currentCmd = (*gji)->gaz_gripper_pids_[2]->updatePid(currentError,currentTime-lastTime);
+ dynamic_cast<gazebo::HingeJoint*>((*gji)->gaz_joints_[2])->SetParam( dParamVel, currentCmd );
+ dynamic_cast<gazebo::HingeJoint*>((*gji)->gaz_joints_[2])->SetParam( dParamFMax, (*gji)->saturationTorque );
+
+ currentError = math_utils::shortest_angular_distance(dynamic_cast<gazebo::HingeJoint*>((*gji)->gaz_joints_[3])->GetAngle(),gripperCmd);
+ currentCmd = (*gji)->gaz_gripper_pids_[3]->updatePid(currentError,currentTime-lastTime);
+ dynamic_cast<gazebo::HingeJoint*>((*gji)->gaz_joints_[3])->SetParam( dParamVel, currentCmd );
+ dynamic_cast<gazebo::HingeJoint*>((*gji)->gaz_joints_[3])->SetParam( dParamFMax, (*gji)->saturationTorque );
+
+ }
+ else
{
- dynamic_cast<gazebo::HingeJoint*>((*gji)->joint_)->SetTorque( mech_joint->commanded_effort_);
+ // normal joints
+ switch ((*gji)->gaz_joints_[0]->GetType())
+ {
+ case gazebo::Joint::SLIDER:
+ {
+ gazebo::SliderJoint* gjs = dynamic_cast<gazebo::SliderJoint*>((*gji)->gaz_joints_[0]);
+ gjs->SetSliderForce( (*gji)->rmc_joint_->commanded_effort_);
+ break;
+ }
+ case gazebo::Joint::HINGE:
+ {
+ gazebo::HingeJoint* gjh = dynamic_cast<gazebo::HingeJoint*>((*gji)->gaz_joints_[0]);
+ gjh->SetTorque( (*gji)->rmc_joint_->commanded_effort_);
+ break;
+ }
+ case gazebo::Joint::HINGE2:
+ case gazebo::Joint::BALL:
+ case gazebo::Joint::UNIVERSAL:
+ break;
+ }
}
+
}
+ lastTime = currentTime;
+
}
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml 2008-08-20 22:29:33 UTC (rev 3361)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml 2008-08-20 22:30:40 UTC (rev 3362)
@@ -3,10 +3,10 @@
<robot name="pr2"><!-- name of the robot-->
- <controller name="base_controller" type="BaseControllerNode">
- <controller name="caster_front_left_controller" type="JointVelocityControllerNode">
+ <controller name="base_controller" topic="base_controller" type="1BaseControllerNode">
+ <controller name="caster_front_left_controller" topic="caster_front_left_controller" type="JointVelocityControllerNode">
<joint name="caster_front_left_joint" >
- <controller_defaults p="1" d="0" i="0" iClamp="0" />
+ <pid p="1" d="0" i="0" iClamp="0" />
<map name="wheel_front_left_l_data" flag="gazebo">
<elem key="saturationTorque">1000</elem>
<elem key="explicitDampingCoefficient">1.0</elem>
@@ -15,27 +15,27 @@
</map>
</joint>
</controller>
- <controller name="wheel_front_left_l_controller" type="JointVelocityControllerNode">
+ <controller name="wheel_front_left_l_controller" topic="wheel_front_left_l_controller" type="JointVelocityControllerNode">
<joint name="wheel_front_left_l_joint" >
- <controller_defaults p="0.5" d="0" i="0" iClamp="0" />
+ <pid p="0.5" d="0" i="0" iClamp="0" />
<map name="wheel_front_left_l_data" flag="gazebo">
<elem key="saturationTorque">5</elem>
<elem key="jointType">Hinge</elem>
</map>
</joint>
</controller>
- <controller name="wheel_front_left_r_controller" type="JointVelocityControllerNode">
+ <controller name="wheel_front_left_r_controller" topic="wheel_front_left_r_controller" type="JointVelocityControllerNode">
<joint name="wheel_front_left_r_joint" >
- <controller_defaults p="0.5" d="0" i="0" iClamp="0" />
+ <pid p="0.5" d="0" i="0" iClamp="0" />
<map name="wheel_front_left_r_data" flag="gazebo">
<elem key="saturationTorque">5</elem>
<elem key="jointType">Hinge</elem>
</map>
</joint>
</controller>
- <controller name="caster_front_right_controller" type="JointVelocityControllerNode">
+ <controller name="caster_front_right_controller" topic="caster_front_right_controller" type="JointVelocityControllerNode">
<joint name="caster_front_right_joint" >
- <controller_defaults p="1" d="0" i="0" iClamp="0" />
+ <pid p="1" d="0" i="0" iClamp="0" />
<map name="wheel_front_left_l_data" flag="gazebo">
<elem key="saturationTorque">1000</elem>
<elem key="explicitDampingCoefficient">1.0</elem>
@@ -44,27 +44,27 @@
</map>
</joint>
</controller>
- <controller name="wheel_front_right_l_controller" type="JointVelocityControllerNode">
+ <controller name="wheel_front_right_l_controller" topic="wheel_front_right_l_controller" type="JointVelocityControllerNode">
<joint name="wheel_front_right_l_joint" >
- <controller_defaults p="0.5" d="0" i="0" iClamp="0" />
+ <pid p="0.5" d="0" i="0" iClamp="0" />
<map name="wheel_front_right_l_data" flag="gazebo">
<elem key="saturationTorque">5</elem>
<elem key="jointType">Hinge</elem>
</map>
</joint>
</controller>
- <controller name="wheel_front_right_r_controller" type="JointVelocityControllerNode">
+ <controller name="wheel_front_right_r_controller" topic="wheel_front_right_r_controller" type="JointVelocityControllerNode">
<joint name="wheel_front_right_r_joint" >
- <controller_defaults p="0.5" d="0" i="0" iClamp="0" />
+ <pid p="0.5" d="0" i="0" iClamp="0" />
<map name="wheel_front_right_r_data" flag="gazebo">
<elem key="saturationTorque">5</elem>
<elem key="jointType">Hinge</elem>
</map>
</joint>
</controller>
- <controller name="caster_rear_left_controller" type="JointVelocityControllerNode">
+ <controller name="caster_rear_left_controller" topic="caster_rear_left_controller" type="JointVelocityControllerNode">
<joint name="caster_rear_left_joint" >
- <controller_defaults p="1" d="0" i="0" iClamp="0" />
+ <pid p="1" d="0" i="0" iClamp="0" />
<map name="wheel_front_right_r_data" flag="gazebo">
<elem key="saturationTorque">1000</elem>
<elem key="explicitDampingCoefficient">1.0</elem>
@@ -73,27 +73,27 @@
</map>
</joint>
</controller>
- <controller name="wheel_rear_left_l_controller" type="JointVelocityControllerNode">
+ <controller name="wheel_rear_left_l_controller" topic="wheel_rear_left_l_controller" type="JointVelocityControllerNode">
<joint name="wheel_rear_left_l_joint" >
- <controller_defaults p="0.5" d="0" i="0" iClamp="0" />
+ <pid p="0.5" d="0" i="0" iClamp="0" />
<map name="wheel_rear_left_l_data" flag="gazebo">
<elem key="saturationTorque">5</elem>
<elem key="jointType">Hinge</elem>
</map>
</joint>
</controller>
- <controller name="wheel_rear_left_r_controller" type="JointVelocityControllerNode">
+ <controller name="wheel_rear_left_r_controller" topic="wheel_rear_left_r_controller" type="JointVelocityControllerNode">
<joint name="wheel_rear_left_r_joint" >
- <controller_defaults p="0.5" d="0" i="0" iClamp="0" />
+ <pid p="0.5" d="0" i="0" iClamp="0" />
<map name="wheel_rear_left_r_data" flag="gazebo">
<elem key="saturationTorque">5</elem>
<elem key="jointType">Hinge</elem>
</map>
</joint>
</controller>
- <controller name="caster_rear_right_controller" type="JointVelocityControllerNode">
+ <controller name="caster_rear_right_controller" topic="caster_rear_right_controller" type="JointVelocityControllerNode">
<joint name="caster_rear_right_joint" >
- <controller_defaults p="1" d="0" i="0" iClamp="0" />
+ <pid p="1" d="0" i="0" iClamp="0" />
<map name="wheel_rear_left_l_data" flag="gazebo">
<elem key="saturationTorque">1000</elem>
<elem key="explicitDampingCoefficient">1.0</elem>
@@ -102,18 +102,18 @@
</map>
</joint>
</controller>
- <controller name="wheel_rear_right_l_controller" type="JointVelocityControllerNode">
+ <controller name="wheel_rear_right_l_controller" topic="wheel_rear_right_l_controller" type="JointVelocityControllerNode">
<joint name="wheel_rear_right_l_joint" >
- <controller_defaults p="0.5" d="0" i="0" iClamp="0" />
+ <pid p="0.5" d="0" i="0" iClamp="0" />
<map name="wheel_rear_right_l_data" flag="gazebo">
<elem key="saturationTorque">5</elem>
<elem key="jointType">Hinge</elem>
</map>
</joint>
</controller>
- <controller name="wheel_rear_right_r_controller" type="JointVelocityControllerNode">
+ <controller name="wheel_rear_right_r_controller" topic="wheel_rear_right_r_controller" type="JointVelocityControllerNode">
<joint name="wheel_rear_right_r_joint" >
- <controller_defaults p="0.5" d="0" i="0" iClamp="0" />
+ <pid p="0.5" d="0" i="0" iClamp="0" />
<map name="wheel_rear_right_r_data" flag="gazebo">
<elem key="saturationTorque">5</elem>
<elem key="jointType">Hinge</elem>
@@ -124,9 +124,9 @@
<!-- ========================================= -->
<!-- torso array -->
- <controller name="torso_controller" type="JointPositionControllerNode">
+ <controller name="torso_controller" topic="torso_controller" type="JointPositionControllerNode">
<joint name="torso_joint">
- <controller_defaults p="1000" d="0" i="0" iClamp="0" />
+ <pid p="1000" d="0" i="0" iClamp="0" />
<map name="wheel_rear_right_l_data" flag="gazebo">
<elem key="saturationTorque">1000</elem>
<elem key="explicitDampingCoefficient">1.0</elem>
@@ -136,9 +136,9 @@
</controller>
<!-- ========================================= -->
<!-- left arm array -->
- <controller name="shoulder_pan_left_controller" type="JointPositionControllerNode">
+ <controller name="shoulder_pan_left_controller" topic="shoulder_pan_left_controller" type="JointPositionControllerNode">
<joint name="shoulder_pan_left_joint" >
- <controller_defaults p="1000" d="0" i="0" iClamp="0" />
+ <pid p="1000" d="0" i="0" iClamp="0" />
<map name="shoulder_pan_left_data" flag="gazebo">
<elem key="saturationTorque">1000</elem>
<elem key="explicitDampingCoefficient">1.0</elem>
@@ -146,9 +146,9 @@
</map>
</joint>
</controller>
- <controller name="shoulder_pitch_left_controller" type="JointPositionControllerNode">
+ <controller name="shoulder_pitch_left_controller" topic="shoulder_pitch_left_controller" type="JointPositionControllerNode">
<joint name="shoulder_pitch_left_joint" >
- <controller_defaults p="10000" d="0" i="0" iClamp="0" />
+ <pid p="10000" d="0" i="0" iClamp="0" />
<map name="shoulder_pitch_left_data" flag="gazebo">
<elem key="saturationTorque">1000</elem>
<elem key="explicitDampingCoefficient">1.0</elem>
@@ -156,9 +156,9 @@
</map>
</joint>
</controller>
- <controller name="upperarm_roll_left_controller" type="JointPositionControllerNode">
+ <controller name="upperarm_roll_left_controller" topic="upperarm_roll_left_controller" type="JointPositionControllerNode">
<joint name="upperarm_roll_left_joint" >
- <controller_defaults p="1000" d="0" i="0" iClamp="0" />
+ <pid p="1000" d="0" i="0" iClamp="0" />
<map name="upperarm_roll_left_data" flag="gazebo">
<elem key="saturationTorque">1000</elem>
<elem key="explicitDampingCoefficient">1.0</elem>
@@ -166,9 +166,9 @@
</map>
</joint>
</controller>
- <controller name="elbow_flex_left_controller" type="JointPositionControllerNode">
+ <controller name="elbow_flex_left_controller" topic="elbow_flex_left_controller" type="JointPositionControllerNode">
<joint name="elbow_flex_left_joint" >
- <controller_defaults p="5000" d="0" i="0" iClamp="0" />
+ <pid p="5000" d="0" i="0" iClamp="0" />
<map name="elbow_flex_left_data" flag="gazebo">
<elem key="saturationTorque">100</elem>
<elem key="explicitDampingCoefficient">1.0</elem>
@@ -176,9 +176,9 @@
</map>
</joint>
</controller>
- <controller name="forearm_roll_left_controller" type="JointPositionControllerNode">
+ <controller name="forearm_roll_left_controller" topic="forearm_roll_left_controller" type="JointPositionControllerNode">
<joint name="forearm_roll_left_joint" >
- <controller_defaults p="100" d="0" i="0" iClamp="0" />
+ <pid p="100" d="0" i="0" iClamp="0" />
<map name="forearm_roll_left_data" flag="gazebo">
<elem key="saturationTorque">100</elem>
<elem key="explicitDampingCoefficient">1.0</elem>
@@ -186,9 +186,9 @@
</map>
</joint>
</controller>
- <controller name="wrist_flex_left_controller" type="JointPositionControllerNode">
+ <controller name="wrist_flex_left_controller" topic="wrist_flex_left_controller" type="JointPositionControllerNode">
<joint name="wrist_flex_left_joint" >
- <controller_defaults p="100" d="0" i="0" iClamp="0" />
+ <pid p="100" d="0" i="0" iClamp="0" />
<map name="wrist_flex_left_data" flag="gazebo">
<elem key="saturationTorque">100</elem>
<elem key="explicitDampingCoefficient">1.0</elem>
@@ -196,9 +196,9 @@
</map>
</joint>
</controller>
- <controller name="gripper_roll_left_controller" type="JointPositionControllerNode">
+ <controller name="gripper_roll_left_controller" topic="gripper_roll_left_controller" type="JointPositionControllerNode">
<joint name="gripper_roll_left_joint" >
- <controller_defaults p="1" d="0" i="0" iClamp="0" />
+ <pid p="1" d="0" i="0" iClamp="0" />
<map name="gripper_roll_left_data" flag="gazebo">
<elem key="saturationTorque">100</elem>
<elem key="explicitDampingCoefficient">1.0</elem>
@@ -207,9 +207,9 @@
</joint>
</controller>
<!-- Special gripper joint -->
- <controller name="gripper_left_controller" type="JointPositionControllerNode">
+ <controller name="gripper_left_controller" topic="gripper_left_controller" type="JointPositionControllerNode">
<joint name="gripper_left_joint">
- <controller_defaults p="20" d="0" i="0" iClamp="0" />
+ <pid p="20" d="0" i="0" iClamp="0" />
<gripper_defaults effortLimit="100" velocityLimit="10" />
<map name="gripper_joints" flag="gazebo">
<elem key="saturationTorque">100</elem>
@@ -224,9 +224,9 @@
</controller>
<!-- ========================================= -->
<!-- right arm array -->
- <controller name="shoulder_pan_right_controller" type="JointPositionControllerNode">
+ <controller name="shoulder_pan_right_controller" topic="shoulder_pan_right_controller" type="JointPositionControllerNode">
<joint name="shoulder_pan_right_joint" >
- <controller_defaults p="1000" d="0" i="0" iClamp="0" />
+ <pid p="1000" d="0" i="0" iClamp="0" />
<map name="shoulder_pan_right_data" flag="gazebo">
<elem key="saturationTorque">1000</elem>
<elem key="explicitDampingCoefficient">1.0</elem>
@@ -234,9 +234,9 @@
</map>
</joint>
</controller>
- <controller name="shoulder_pitch_right_controller" type="JointPositionControllerNode">
+ <controller name="shoulder_pitch_right_controller" topic="shoulder_pitch_right_controller" type="JointPositionControllerNode">
<joint name="shoulder_pitch_right_joint" >
- <controller_defaults p="10000" d="0" i="0" iClamp="0" />
+ <pid p="10000" d="0" i="0" iClamp="0" />
<map name="shoulder_pitch_right_data" flag="gazebo">
<elem key="saturationTorque">1000</elem>
<elem key="explicitDampingCoefficient">1.0</elem>
@@ -244,9 +244,9 @@
</map>
</joint>
</controller>
- <controller name="upperarm_roll_right_controller" type="JointPositionControllerNode">
+ <controller name="upperarm_roll_right_controller" topic="upperarm_roll_right_controller" type="JointPositionControllerNode">
<joint name="upperarm_roll_right_joint" >
- <controller_defaults p="1000" d="0" i="0" iClamp="0" />
+ <pid p="1000" d="0" i="0" iClamp="0" />
<map name="upperarm_roll_right_data" flag="gazebo">
<elem key="saturationTorque">1000</elem>
<elem key="explicitDampingCoefficient">1.0</elem>
@@ -254,9 +254,9 @@
</map>
</joint>
</controller>
- <controller name="elbow_flex_right_controller" type="JointPositionControllerNode">
+ <controller name="elbow_flex_right_controller" topic="elbow_flex_right_controller" type="JointPositionControllerNode">
<joint name="elbow_flex_right_joint" >
- <controller_defaults p="5000" d="0" i="0" iClamp="0" />
+ <pid p="5000" d="0" i="0" iClamp="0" />
<map name="elbow_flex_right_data" flag="gazebo">
<elem key="saturationTorque">1000</elem>
<elem key="explicitDampingCoefficient">1.0</elem>
@@ -264,9 +264,9 @@
</map>
</joint>
</controller>
- <controller name="forearm_roll_right_controller" type="JointPositionControllerNode">
+ <controller name="forearm_roll_right_controller" topic="forearm_roll_right_controller" type="JointPositionControllerNode">
<joint name="forearm_roll_right_joint" >
- <controller_defaults p="100" d="0" i="0" iClamp="0" />
+ <pid p="100" d="0" i="0" iClamp="0" />
<map name="forearm_roll_right_data" flag="gazebo">
<elem key="saturationTorque">100</elem>
<elem key="explicitDampingCoefficient">1.0</elem>
@@ -274,9 +274,9 @@
</map>
</joint>
</controller>
- <controller name="wrist_flex_right_controller" type="JointPositionControllerNode">
+ <controller name="wrist_flex_right_controller" topic="wrist_flex_right_controller" type="JointPositionControllerNode">
<joint name="wrist_flex_right_joint" >
- <controller_defaults p="100" d="0" i="0" iClamp="0" />
+ <pid p="100" d="0" i="0" iClamp="0" />
<map name="wrist_flex_right_data" flag="gazebo">
<elem key="saturationTorque">100</elem>
<elem key="explicitDampingCoefficient">1.0</elem>
@@ -284,9 +284,9 @@
</map>
</joint>
</controller>
- <controller name="gripper_roll_right_controller" type="JointPositionControllerNode">
+ <controller name="gripper_roll_right_controller" topic="gripper_roll_right_controller" type="JointPositionControllerNode">
<joint name="gripper_roll_right_joint" >
- <controller_defaults p="1" d="0" i="0" iClamp="0" />
+ <pid p="1" d="0" i="0" iClamp="0" />
<map name="gripper_roll_right_data" flag="gazebo">
<elem key="saturationTorque">100</elem>
<elem key="explicitDampingCoefficient">1.0</elem>
@@ -295,9 +295,9 @@
</joint>
</controller>
<!-- Special gripper joint -->
- <controller name="gripper_right_controller" type="JointPositionControllerNode">
+ <controller name="gripper_right_controller" topic="gripper_right_controller" type="JointPositionControllerNode">
<joint name="gripper_right_joint">
- <controller_defaults p="20" d="0" i="0" iClamp="0" />
+ <pid p="20" d="0" i="0" iClamp="0" />
<gripper_defaults effortLimit="100" velocityLimit="10" />
<map name="gripper_joints" flag="gazebo">
<elem key="saturationTorque">100</elem>
@@ -311,9 +311,9 @@
</joint>
</controller>
<!-- head and above array -->
- <controller name="head_pan_controller" type="JointPositionControllerNode">
+ <controller name="head_pan_controller" topic="head_pan_controller" type="JointPositionControllerNode">
<joint name="head_pan_joint" >
- <controller_defaults p="100" d="0" i="0" iClamp="0" />
+ <pid p="100" d="0" i="0" iClamp="0" />
<map name="head_pan_data" flag="gazebo">
<elem key="saturationTorque">100</elem>
<elem key="explicitDampingCoefficient">1.0</elem>
@@ -321,9 +321,9 @@
</map>
</joint>
</controller>
- <controller name="head_tilt_controller" type="JointPositionControllerNode">
+ <controller name="head_tilt_controller" topic="head_tilt_controller" type="JointPositionControllerNode">
<joint name="head_tilt_joint" >
- <controller_defaults p="100" d="0" i="0" iClamp="0" />
+ <pid p="100" d="0" i="0" iClamp="0" />
<map name="head_tilt_data" flag="gazebo">
<elem key="saturationTorque">10</elem>
<elem key="explicitDampingCoefficient">1.0</elem>
@@ -331,9 +331,9 @@
</map>
</joint>
</controller>
- <controller name="tilt_laser_controller" type="JointPositionControllerNode">
+ <controller name="tilt_laser_controller" topic="tilt_laser_controller" type="JointPositionControllerNode">
<joint name="tilt_laser_joint" >
- <controller_defaults p="0.5" d="0" i="0" iClamp="20" />
+ <pid p="0.5" d="0" i="0" iClamp="20" />
<map name="tilt_laser_data" flag="gazebo">
<elem key="saturationTorque">500</elem>
<elem key="explicitDampingCoefficient">1.0</elem>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo_test.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo_test.xml 2008-08-20 22:29:33 UTC (rev 3361)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo_test.xml 2008-08-20 22:30:40 UTC (rev 3362)
@@ -12,9 +12,7 @@
<controller:test_actuators name="test_actuators" plugin="libtest_actuators.so">
<updateRate>100.0</updateRate>
- <include>transmissions.xml</include>
- <include>actuators.xml</include>
- <include>controllers.xml</include>
+ <include>gazebo_joints.xml</include>
<robot_filename>./world/pr2_test.xml</robot_filename>
<controller_filename>./world/controllers.xml</controller_filename>
Added: pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo_joints.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo_joints.xml (rev 0)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo_joints.xml 2008-08-20 22:30:40 UTC (rev 3362)
@@ -0,0 +1,146 @@
+<robot name="pr2">
+ <joint name="caster_front_left_joint" >
+ <saturationTorque>1000</saturationTorque>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ <kp_speed>1.0</kp_speed>
+ </joint>
+ <joint name="wheel_front_left_l_joint" >
+ <saturationTorque>5</saturationTorque>
+ </joint>
+ <joint name="wheel_front_left_r_joint" >
+ <saturationTorque>5</saturationTorque>
+ </joint>
+ <joint name="caster_front_right_joint" >
+ <saturationTorque>1000</saturationTorque>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ <kp_speed>1.0</kp_speed>
+ </joint>
+ <joint name="wheel_front_right_l_joint" >
+ <saturationTorque>5</saturationTorque>
+ </joint>
+ <joint name="wheel_front_right_r_joint" >
+ <saturationTorque>5</saturationTorque>
+ </joint>
+ <joint name="caster_rear_left_joint" >
+ <saturationTorque>1000</saturationTorque>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ <kp_speed>1.0</kp_speed>
+ </joint>
+ <joint name="wheel_rear_left_l_joint" >
+ <saturationTorque>5</saturationTorque>
+ </joint>
+ <joint name="wheel_rear_left_r_joint" >
+ <saturationTorque>5</saturationTorque>
+ </joint>
+ <joint name="caster_rear_right_joint" >
+ <saturationTorque>1000</saturationTorque>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ <kp_speed>1.0</kp_speed>
+ </joint>
+ <joint name="wheel_rear_right_l_joint" >
+ <saturationTorque>5</saturationTorque>
+ </joint>
+ <joint name="wheel_rear_right_r_joint" >
+ <saturationTorque>5</saturationTorque>
+ </joint>
+
+ <!-- ========================================= -->
+ <!-- torso array -->
+ <joint name="torso_joint">
+ <saturationTorque>1000</saturationTorque>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ </joint>
+ <!-- ========================================= -->
+ <!-- left arm array -->
+ <joint name="shoulder_pan_left_joint" >
+ <saturationTorque>1000</saturationTorque>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ </joint>
+ <joint name="shoulder_pitch_left_joint" >
+ <saturationTorque>1000</saturationTorque>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ </joint>
+ <joint name="upperarm_roll_left_joint" >
+ <saturationTorque>1000</saturationTorque>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ </joint>
+ <joint name="elbow_flex_left_joint" >
+ <saturationTorque>100</saturationTorque>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ </joint>
+ <joint name="forearm_roll_left_joint" >
+ <saturationTorque>100</saturationTorque>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ </joint>
+ <joint name="wrist_flex_left_joint" >
+ <saturationTorque>100</saturationTorque>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ </joint>
+ <joint name="gripper_roll_left_joint" >
+ <saturationTorque>100</saturationTorque>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ </joint>
+ <!-- Special gripper joint -->
+ <joint name="gripper_left_joint" gripper_controller="gripper_left_controller" abstract="gripper" effortLimit="1000" velocityLimit="100">
+ <saturationTorque>100</saturationTorque>
+ <explicitDampingCoefficient>0.1</explicitDampingCoefficient>
+ <left_proximal>finger_l_left_joint</left_proximal>
+ <left_distal>finger_tip_l_left_joint</left_distal>
+ <right_proximal>finger_r_left_joint</right_proximal>
+ <right_distal>finger_tip_r_left_joint</right_distal>
+ </joint>
+ <!-- ========================================= -->
+ <!-- right arm array -->
+ <joint name="shoulder_pan_right_joint" >
+ <saturationTorque>1000</saturationTorque>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ </joint>
+ <joint name="shoulder_pitch_right_joint" >
+ <saturationTorque>1000</saturationTorque>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ </joint>
+ <joint name="upperarm_roll_right_joint" >
+ <saturationTorque>1000</saturationTorque>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ </joint>
+ <joint name="elbow_flex_right_joint" >
+ <saturationTorque>1000</saturationTorque>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ </joint>
+ <joint name="forearm_roll_right_joint" >
+ <saturationTorque>100</saturationTorque>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ </joint>
+ <joint name="wrist_flex_right_joint" >
+ <saturationTorque>100</saturationTorque>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ </joint>
+ <joint name="gripper_roll_right_joint" >
+ <saturationTorque>100</saturationTorque>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ </joint>
+ <!-- Special gripper joint -->
+ <joint name="gripper_right_joint" gripper_controller="gripper_right_controller" abstract="gripper" effortLimit="1000" velocityLimit="100">
+ <saturationTorque>100</saturationTorque>
+ <explicitDampingCoefficient>0.1</explicitDampingCoefficient>
+ <left_proximal>finger_l_right_joint</left_proximal>
+ <left_distal>finger_tip_l_right_joint</left_distal>
+ <right_proximal>finger_r_right_joint</right_proximal>
+ <right_distal>finger_tip_r_right_joint</right_distal>
+ </joint>
+ <!-- head and above array -->
+ <joint name="head_pan_joint" >
+ <saturationTorque>100</saturationTorque>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ </joint>
+ <joint name="head_tilt_joint" >
+ <saturationTorque>10</saturationTorque>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ </joint>
+ <joint name="tilt_laser_joint" >
+ <saturationTorque>500</saturationTorque>
+ <explicitDampingCoefficient>1.0</explicitDampingCoefficient>
+ </joint>
+
+
+</robot>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
