Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[4062] pkg/trunk
|
From: <hsu...@us...> - 2008-09-08 23:46:17
|
Revision: 4062
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4062&view=rev
Author: hsujohnhsu
Date: 2008-09-08 23:46:26 +0000 (Mon, 08 Sep 2008)
Log Message:
-----------
final tweaks to get 2dnav-gazebo back online.
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/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-09-08 23:41:46 UTC (rev 4061)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/test_actuators.h 2008-09-08 23:46:26 UTC (rev 4062)
@@ -132,12 +132,14 @@
// for mechanism control
//---------------------------------------------------------------------
MechanismControl mc_;
- MechanismControl rmc_;
MechanismControlNode mcn_;
+ mechanism::RobotState *fake_state_;
+
void LoadMC(XMLConfigNode *node);
void UpdateMC();
+ void PublishROS();
void UpdateMCJoints();
void UpdateGazeboJoints();
@@ -200,7 +202,7 @@
{
std::string* name_;
std::vector<gazebo::Joint*> gaz_joints_;
- mechanism::JointState* rmc_joint_state_;
+ mechanism::JointState* fake_joint_state_;
double saturationTorque;
double explicitDampingCoefficient;
};
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp 2008-09-08 23:41:46 UTC (rev 4061)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp 2008-09-08 23:46:26 UTC (rev 4062)
@@ -53,7 +53,7 @@
GZ_REGISTER_DYNAMIC_CONTROLLER("test_actuators", TestActuators);
TestActuators::TestActuators(Entity *parent)
- : Controller(parent) , hw_(0), mc_(&hw_), rmc_(&hw_) , mcn_(&mc_)
+ : Controller(parent) , hw_(0), mc_(&hw_), fake_state_(NULL) , mcn_(&mc_)
{
this->parent_model_ = dynamic_cast<Model*>(this->parent);
@@ -167,22 +167,22 @@
//-----------------------------------------------------------------------------------------
TiXmlDocument *pr2_xml = new TiXmlDocument();
TiXmlDocument *controller_xml = new TiXmlDocument();
- TiXmlDocument *transmission_xml = new TiXmlDocument();
+ //TiXmlDocument *transmission_xml = new TiXmlDocument();
TiXmlDocument *actuator_xml = new TiXmlDocument();
std::string pr2_xml_filename = getenv("MC_RESOURCE_PATH"); pr2_xml_filename += "/"; pr2_xml_filename += node->GetString("robot_filename","",1);
std::string controller_xml_filename = getenv("MC_RESOURCE_PATH"); controller_xml_filename += "/"; controller_xml_filename += node->GetString("controller_filename","",1);
- std::string transmission_xml_filename = getenv("MC_RESOURCE_PATH"); transmission_xml_filename += "/"; transmission_xml_filename += node->GetString("transmission_filename","",1);
+ //std::string transmission_xml_filename = getenv("MC_RESOURCE_PATH"); transmission_xml_filename += "/"; transmission_xml_filename += node->GetString("transmission_filename","",1);
std::string actuator_xml_filename = getenv("MC_RESOURCE_PATH"); actuator_xml_filename += "/"; actuator_xml_filename += node->GetString("actuator_filename","",1);
std::cout << " pr2 robot xml file name: " << pr2_xml_filename << std::endl;
std::cout << " controller file name: " << controller_xml_filename << std::endl;
- std::cout << " transmission file name: " << transmission_xml_filename << std::endl;
+ //std::cout << " transmission file name: " << transmission_xml_filename << std::endl;
std::cout << " actuator file name: " << actuator_xml_filename << std::endl;
pr2_xml ->LoadFile(pr2_xml_filename);
controller_xml ->LoadFile(controller_xml_filename);
- transmission_xml->LoadFile(transmission_xml_filename);
+ //transmission_xml->LoadFile(transmission_xml_filename);
actuator_xml ->LoadFile(actuator_xml_filename);
urdf::normalizeXml( pr2_xml->RootElement() );
@@ -243,8 +243,9 @@
//
//-----------------------------------------------------------------------------------------
mcn_.initXml(pr2_xml->FirstChildElement("robot"));
- rmc_.initXml(pr2_xml->FirstChildElement("robot"));
+ fake_state_ = new mechanism::RobotState(&mc_.model_, &hw_);
+
//-----------------------------------------------------------------------------------------
//
// how the mechanism joints relate to the gazebo_joints
@@ -264,7 +265,7 @@
// add a link to the mechanism control joint
- gj->rmc_joint_state_ = rmc_.state_->getJointState(*joint_name);
+ gj->fake_joint_state_ = fake_state_->getJointState(*joint_name);
// read gazebo specific joint properties
gj->saturationTorque = jNode->GetDouble("saturationTorque",0.0,0);
@@ -291,16 +292,6 @@
//-----------------------------------------------------------------------------------------
//
- // TRANSMISSION XML
- //
- // make mc parse xml for transmissions
- //
- //-----------------------------------------------------------------------------------------
- mcn_.initXml(transmission_xml->FirstChildElement("robot"));
- rmc_.initXml(transmission_xml->FirstChildElement("robot"));
-
- //-----------------------------------------------------------------------------------------
- //
// CONTROLLER XML
//
// spawn controllers
@@ -320,12 +311,6 @@
mc_.spawnController(*controller_type,
*controller_name,
zit);
-
- std::cout << " adding to rmc_ " << *controller_name << "(" << *controller_type << ")" << std::endl;
- rmc_.spawnController(*controller_type,
- *controller_name,
- zit);
-
}
}
@@ -365,76 +350,18 @@
lastRealTime = currentRealTime;
}
- this->lock.lock();
- /***************************************************************/
- /* */
- /* publish time to ros */
- /* */
- /***************************************************************/
- timeMsg.rostime.sec = (unsigned long)floor(hw_.current_time_);
- timeMsg.rostime.nsec = (unsigned long)floor( 1e9 * ( hw_.current_time_ - timeMsg.rostime.sec) );
- rosnode_->publish("time",timeMsg);
+ PublishROS();
- /***************************************************************/
- /* */
- /* object position */
- /* FIXME: move this to the P3D plugin (update P3D required) */
- /* */
- /***************************************************************/
- //this->PR2Copy->GetObjectPositionActual(&x,&y,&z,&roll,&pitch,&yaw);
- //this->objectPosMsg.x = x;
- //this->objectPosMsg.y = y;
- //this->objectPosMsg.z = z;
- //rosnode_->publish("object_position", this->objectPosMsg);
-
-
-
- /* get left arm position */
- if( mc_.state_->getJointState("shoulder_pan_left_joint") ) larm.turretAngle = mc_.state_->getJointState("shoulder_pan_left_joint") ->position_;
- if( mc_.state_->getJointState("shoulder_pitch_left_joint") ) larm.shoulderLiftAngle = mc_.state_->getJointState("shoulder_pitch_left_joint")->position_;
- if( mc_.state_->getJointState("upperarm_roll_left_joint") ) larm.upperarmRollAngle = mc_.state_->getJointState("upperarm_roll_left_joint") ->position_;
- if( mc_.state_->getJointState("elbow_flex_left_joint") ) larm.elbowAngle = mc_.state_->getJointState("elbow_flex_left_joint") ->position_;
- if( mc_.state_->getJointState("forearm_roll_left_joint") ) larm.forearmRollAngle = mc_.state_->getJointState("forearm_roll_left_joint") ->position_;
- if( mc_.state_->getJointState("wrist_flex_left_joint") ) larm.wristPitchAngle = mc_.state_->getJointState("wrist_flex_left_joint") ->position_;
- if( mc_.state_->getJointState("gripper_roll_left_joint") ) larm.wristRollAngle = mc_.state_->getJointState("gripper_roll_left_joint") ->position_;
- if( mc_.state_->getJointState("gripper_left_joint") ) larm.gripperForceCmd = mc_.state_->getJointState("gripper_left_joint") ->applied_effort_;
- if( mc_.state_->getJointState("gripper_left_joint") ) larm.gripperGapCmd = mc_.state_->getJointState("gripper_left_joint") ->position_;
- rosnode_->publish("left_pr2arm_pos", larm);
- /* get right arm position */
- if( mc_.state_->getJointState("shoulder_pan_right_joint") ) rarm.turretAngle = mc_.state_->getJointState("shoulder_pan_right_joint") ->position_;
- if( mc_.state_->getJointState("shoulder_pitch_right_joint") ) rarm.shoulderLiftAngle = mc_.state_->getJointState("shoulder_pitch_right_joint")->position_;
- if( mc_.state_->getJointState("upperarm_roll_right_joint") ) rarm.upperarmRollAngle = mc_.state_->getJointState("upperarm_roll_right_joint") ->position_;
- if( mc_.state_->getJointState("elbow_flex_right_joint") ) rarm.elbowAngle = mc_.state_->getJointState("elbow_flex_right_joint") ->position_;
- if( mc_.state_->getJointState("forearm_roll_right_joint") ) rarm.forearmRollAngle = mc_.state_->getJointState("forearm_roll_right_joint") ->position_;
- if( mc_.state_->getJointState("wrist_flex_right_joint") ) rarm.wristPitchAngle = mc_.state_->getJointState("wrist_flex_right_joint") ->position_;
- if( mc_.state_->getJointState("gripper_roll_right_joint") ) rarm.wristRollAngle = mc_.state_->getJointState("gripper_roll_right_joint") ->position_;
- if( mc_.state_->getJointState("gripper_right_joint") ) rarm.gripperForceCmd = mc_.state_->getJointState("gripper_right_joint") ->applied_effort_;
- if( mc_.state_->getJointState("gripper_right_joint") ) rarm.gripperGapCmd = mc_.state_->getJointState("gripper_right_joint") ->position_;
- rosnode_->publish("right_pr2arm_pos", rarm);
-
- PublishFrameTransforms();
-
- this->lock.unlock();
-
//---------------------------------------------------------------------
// Real time update calls to mechanism control
// this is what the hard real time loop does,
// minus the tick() call to etherCAT
//---------------------------------------------------------------------
- //
- // step through all controllers in the Robot_controller
+ // fetch joint info into fake_state_ from gazebo joints
UpdateMCJoints();
-
-
// push reverse_mech_joint_ stuff back toward actuators
- for (unsigned int i=0; i < rmc_.model_.transmissions_.size(); i++)
- {
- rmc_.state_->propagateStateBackwards();
- rmc_.state_->propagateEffort();
- // std::cout << " applying reverse transmisison : "
- // << dynamic_cast<mechanism::SimpleTransmission*>(rmc_.model_.transmissions_[i])->name_
- // << " " << std::endl;
- }
+ fake_state_->propagateStateBackwards();
+ fake_state_->propagateEffort();
// -------------------------------------------------------------------------------------------------
@@ -497,13 +424,10 @@
// - -
// -------------------------------------------------------------------------------------------------
// propagate actuator data back to reverse-joints
- for (unsigned int i=0; i < rmc_.model_.transmissions_.size(); i++)
- {
- // assign reverse joint states from actuator states
- rmc_.state_->propagateState();
- // assign joint effort
- rmc_.state_->propagateEffortBackwards();
- }
+ // assign reverse joint states from actuator states
+ fake_state_->propagateState();
+ // assign joint effort
+ fake_state_->propagateEffortBackwards();
UpdateGazeboJoints();
@@ -512,7 +436,61 @@
}
+ void TestActuators::PublishROS()
+ {
+ this->lock.lock();
+ /***************************************************************/
+ /* */
+ /* publish time to ros */
+ /* */
+ /***************************************************************/
+ timeMsg.rostime.sec = (unsigned long)floor(hw_.current_time_);
+ timeMsg.rostime.nsec = (unsigned long)floor( 1e9 * ( hw_.current_time_ - timeMsg.rostime.sec) );
+ rosnode_->publish("time",timeMsg);
+ /***************************************************************/
+ /* */
+ /* object position */
+ /* FIXME: move this to the P3D plugin (update P3D required) */
+ /* */
+ /***************************************************************/
+ //this->PR2Copy->GetObjectPositionActual(&x,&y,&z,&roll,&pitch,&yaw);
+ //this->objectPosMsg.x = x;
+ //this->objectPosMsg.y = y;
+ //this->objectPosMsg.z = z;
+ //rosnode_->publish("object_position", this->objectPosMsg);
+
+
+
+ /* get left arm position */
+ if( mc_.state_->getJointState("shoulder_pan_left_joint") ) larm.turretAngle = mc_.state_->getJointState("shoulder_pan_left_joint") ->position_;
+ if( mc_.state_->getJointState("shoulder_pitch_left_joint") ) larm.shoulderLiftAngle = mc_.state_->getJointState("shoulder_pitch_left_joint")->position_;
+ if( mc_.state_->getJointState("upperarm_roll_left_joint") ) larm.upperarmRollAngle = mc_.state_->getJointState("upperarm_roll_left_joint") ->position_;
+ if( mc_.state_->getJointState("elbow_flex_left_joint") ) larm.elbowAngle = mc_.state_->getJointState("elbow_flex_left_joint") ->position_;
+ if( mc_.state_->getJointState("forearm_roll_left_joint") ) larm.forearmRollAngle = mc_.state_->getJointState("forearm_roll_left_joint") ->position_;
+ if( mc_.state_->getJointState("wrist_flex_left_joint") ) larm.wristPitchAngle = mc_.state_->getJointState("wrist_flex_left_joint") ->position_;
+ if( mc_.state_->getJointState("gripper_roll_left_joint") ) larm.wristRollAngle = mc_.state_->getJointState("gripper_roll_left_joint") ->position_;
+ if( mc_.state_->getJointState("gripper_left_joint") ) larm.gripperForceCmd = mc_.state_->getJointState("gripper_left_joint") ->applied_effort_;
+ if( mc_.state_->getJointState("gripper_left_joint") ) larm.gripperGapCmd = mc_.state_->getJointState("gripper_left_joint") ->position_;
+ rosnode_->publish("left_pr2arm_pos", larm);
+ /* get right arm position */
+ if( mc_.state_->getJointState("shoulder_pan_right_joint") ) rarm.turretAngle = mc_.state_->getJointState("shoulder_pan_right_joint") ->position_;
+ if( mc_.state_->getJointState("shoulder_pitch_right_joint") ) rarm.shoulderLiftAngle = mc_.state_->getJointState("shoulder_pitch_right_joint")->position_;
+ if( mc_.state_->getJointState("upperarm_roll_right_joint") ) rarm.upperarmRollAngle = mc_.state_->getJointState("upperarm_roll_right_joint") ->position_;
+ if( mc_.state_->getJointState("elbow_flex_right_joint") ) rarm.elbowAngle = mc_.state_->getJointState("elbow_flex_right_joint") ->position_;
+ if( mc_.state_->getJointState("forearm_roll_right_joint") ) rarm.forearmRollAngle = mc_.state_->getJointState("forearm_roll_right_joint") ->position_;
+ if( mc_.state_->getJointState("wrist_flex_right_joint") ) rarm.wristPitchAngle = mc_.state_->getJointState("wrist_flex_right_joint") ->position_;
+ if( mc_.state_->getJointState("gripper_roll_right_joint") ) rarm.wristRollAngle = mc_.state_->getJointState("gripper_roll_right_joint") ->position_;
+ if( mc_.state_->getJointState("gripper_right_joint") ) rarm.gripperForceCmd = mc_.state_->getJointState("gripper_right_joint") ->applied_effort_;
+ if( mc_.state_->getJointState("gripper_right_joint") ) rarm.gripperGapCmd = mc_.state_->getJointState("gripper_right_joint") ->position_;
+ rosnode_->publish("right_pr2arm_pos", rarm);
+
+ PublishFrameTransforms();
+
+ this->lock.unlock();
+ }
+
+
void TestActuators::UpdateGazeboJoints()
{
// -------------------------------------------------------------------------------------------------
@@ -531,7 +509,7 @@
if (gjs)
{
double dampForce = -(*gji)->explicitDampingCoefficient * gjs->GetPositionRate();
- gjs->SetSliderForce( (*gji)->rmc_joint_state_->commanded_effort_+dampForce);
+ gjs->SetSliderForce( (*gji)->fake_joint_state_->commanded_effort_+dampForce);
break;
}
}
@@ -541,8 +519,8 @@
if (gjh)
{
double dampForce = -(*gji)->explicitDampingCoefficient * gjh->GetAngleRate();
- gjh->SetTorque( (*gji)->rmc_joint_state_->commanded_effort_+dampForce);
- //std::cout << " hinge " << *((*gji)->name_) << " torque: " << (*gji)->rmc_joint_state_->commanded_effort_ << " damping " << dampForce << std::endl;
+ gjh->SetTorque( (*gji)->fake_joint_state_->commanded_effort_+dampForce);
+ //std::cout << " hinge " << *((*gji)->name_) << " torque: " << (*gji)->fake_joint_state_->commanded_effort_ << " damping " << dampForce << std::endl;
break;
}
}
@@ -565,18 +543,18 @@
case gazebo::Joint::SLIDER:
{
gazebo::SliderJoint* gjs = dynamic_cast<gazebo::SliderJoint*>((*gji)->gaz_joints_[0]);
- (*gji)->rmc_joint_state_->position_ = gjs->GetPosition();
- (*gji)->rmc_joint_state_->velocity_ = gjs->GetPositionRate();
- (*gji)->rmc_joint_state_->applied_effort_ = (*gji)->rmc_joint_state_->commanded_effort_;
+ (*gji)->fake_joint_state_->position_ = gjs->GetPosition();
+ (*gji)->fake_joint_state_->velocity_ = gjs->GetPositionRate();
+ (*gji)->fake_joint_state_->applied_effort_ = (*gji)->fake_joint_state_->commanded_effort_;
break;
}
case gazebo::Joint::HINGE:
{
gazebo::HingeJoint* gjh = dynamic_cast<gazebo::HingeJoint*>((*gji)->gaz_joints_[0]);
- (*gji)->rmc_joint_state_->position_ = gjh->GetAngle();
- (*gji)->rmc_joint_state_->velocity_ = gjh->GetAngleRate();
- (*gji)->rmc_joint_state_->applied_effort_ = (*gji)->rmc_joint_state_->commanded_effort_;
- //std::cout << " hinge " << *((*gji)->name_) << " angle " << (*gji)->rmc_joint_state_->position_ << std::endl;
+ (*gji)->fake_joint_state_->position_ = gjh->GetAngle();
+ (*gji)->fake_joint_state_->velocity_ = gjh->GetAngleRate();
+ (*gji)->fake_joint_state_->applied_effort_ = (*gji)->fake_joint_state_->commanded_effort_;
+ //std::cout << " hinge " << *((*gji)->name_) << " angle " << (*gji)->fake_joint_state_->position_ << std::endl;
break;
}
case gazebo::Joint::HINGE2:
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo_joints.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo_joints.xml 2008-09-08 23:41:46 UTC (rev 4061)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo_joints.xml 2008-09-08 23:46:26 UTC (rev 4062)
@@ -80,15 +80,23 @@
<saturationTorque>100</saturationTorque>
<explicitDampingCoefficient>1.0</explicitDampingCoefficient>
</joint>
- <!-- Special gripper joint -->
- <joint name="finger_l_left_joint" gripper_controller="gripper_left_controller" abstract="gripper" effortLimit="1000" velocityLimit="100">
+ <!-- Special gripper joint -->
+ <joint name="finger_l_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>
+ <joint name="finger_r_left_joint" gripper_controller="gripper_left_controller" abstract="gripper" effortLimit="1000" velocityLimit="100">
+ <saturationTorque>100</saturationTorque>
+ <explicitDampingCoefficient>0.1</explicitDampingCoefficient>
+ </joint>
+ <joint name="finger_tip_l_left_joint" gripper_controller="gripper_left_controller" abstract="gripper" effortLimit="1000" velocityLimit="100">
+ <saturationTorque>100</saturationTorque>
+ <explicitDampingCoefficient>0.1</explicitDampingCoefficient>
+ </joint>
+ <joint name="finger_tip_r_left_joint" gripper_controller="gripper_left_controller" abstract="gripper" effortLimit="1000" velocityLimit="100">
+ <saturationTorque>100</saturationTorque>
+ <explicitDampingCoefficient>0.1</explicitDampingCoefficient>
+ </joint>
<!-- ========================================= -->
<!-- right arm array -->
<joint name="shoulder_pan_right_joint" >
@@ -120,14 +128,22 @@
<explicitDampingCoefficient>1.0</explicitDampingCoefficient>
</joint>
<!-- Special gripper joint -->
- <joint name="finger_l_right_joint" gripper_controller="gripper_right_controller" abstract="gripper" effortLimit="1000" velocityLimit="100">
+ <joint name="finger_l_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>
+ <joint name="finger_r_right_joint" gripper_controller="gripper_right_controller" abstract="gripper" effortLimit="1000" velocityLimit="100">
+ <saturationTorque>100</saturationTorque>
+ <explicitDampingCoefficient>0.1</explicitDampingCoefficient>
+ </joint>
+ <joint name="finger_tip_l_right_joint" gripper_controller="gripper_right_controller" abstract="gripper" effortLimit="1000" velocityLimit="100">
+ <saturationTorque>100</saturationTorque>
+ <explicitDampingCoefficient>0.1</explicitDampingCoefficient>
+ </joint>
+ <joint name="finger_tip_r_right_joint" gripper_controller="gripper_right_controller" abstract="gripper" effortLimit="1000" velocityLimit="100">
+ <saturationTorque>100</saturationTorque>
+ <explicitDampingCoefficient>0.1</explicitDampingCoefficient>
+ </joint>
<!-- head and above array -->
<joint name="head_pan_joint" >
<saturationTorque>100</saturationTorque>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
