|
From: <stu...@us...> - 2008-09-29 20:18:10
|
Revision: 4779
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4779&view=rev
Author: stuglaser
Date: 2008-09-29 20:17:19 +0000 (Mon, 29 Sep 2008)
Log Message:
-----------
RealtimePublisher is now actually realtime safe.
Modified Paths:
--------------
pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/include/motor_qualification_controllers/motor_test1.h
pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/include/motor_qualification_controllers/motor_test2.h
pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/include/motor_qualification_controllers/motor_test3.h
pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/src/motor_test1.cpp
pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/src/motor_test2.cpp
pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/src/motor_test3.cpp
pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp
pkg/trunk/drivers/motor/ethercat_hardware/src/wg0x.cpp
pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
pkg/trunk/util/misc_utils/include/misc_utils/realtime_publisher.h
Modified: pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/include/motor_qualification_controllers/motor_test1.h
===================================================================
--- pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/include/motor_qualification_controllers/motor_test1.h 2008-09-29 20:03:36 UTC (rev 4778)
+++ pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/include/motor_qualification_controllers/motor_test1.h 2008-09-29 20:17:19 UTC (rev 4779)
@@ -38,7 +38,7 @@
/*! \class controller::MotorTest1
\brief Motor Test1 Controller
- This tests the motor enocder assembly to check if
+ This tests the motor enocder assembly to check if
the two are assembled properly.
*/
@@ -81,7 +81,7 @@
* \brief Get latest time..
*/
double getTime();
-
+
/*!
* \brief Perform the test analysis
*/
@@ -107,15 +107,13 @@
double fixture_joint_end_pos_; /**< End pos of the fixture joint. */
bool complete;
misc_utils::RealtimePublisher<robot_msgs::DiagnosticMessage> publisher_;
- robot_msgs::DiagnosticMessage diagnostic_message_;
-
};
/***************************************************/
/*! \class controller::MotorTest1Node
\brief Motor Test1 Controller
- This tests the motor enocder assembly to check if
+ This tests the motor enocder assembly to check if
the two are assembled properly.
*/
Modified: pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/include/motor_qualification_controllers/motor_test2.h
===================================================================
--- pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/include/motor_qualification_controllers/motor_test2.h 2008-09-29 20:03:36 UTC (rev 4778)
+++ pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/include/motor_qualification_controllers/motor_test2.h 2008-09-29 20:17:19 UTC (rev 4779)
@@ -38,7 +38,7 @@
/*! \class controller::MotorTest2
\brief Motor Test1 Controller
- This tests the motor enocder assembly to check if
+ This tests the motor enocder assembly to check if
the two are assembled properly.
*/
@@ -85,7 +85,7 @@
* \brief Get latest time..
*/
double getTime();
-
+
/*!
* \brief Perform the test analysis
*/
@@ -111,24 +111,20 @@
double initial_time_; /**< Start time of the test. */
bool complete;
misc_utils::RealtimePublisher<robot_msgs::DiagnosticMessage> publisher_;
- robot_msgs::DiagnosticMessage diagnostic_message_;
-
-
+
NEWMAT::ColumnVector test_voltage_;
- NEWMAT::ColumnVector test_velocity_;
+ NEWMAT::ColumnVector test_velocity_;
NEWMAT::ColumnVector test_current_;
NEWMAT::ColumnVector test_baseline_;
NEWMAT::UpperTriangularMatrix U_;
NEWMAT::ColumnVector M_;
-
-
};
/***************************************************/
/*! \class controller::MotorTest2Node
\brief Motor Test1 Controller
- This tests the motor enocder assembly to check if
+ This tests the motor enocder assembly to check if
the two are assembled properly.
*/
Modified: pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/include/motor_qualification_controllers/motor_test3.h
===================================================================
--- pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/include/motor_qualification_controllers/motor_test3.h 2008-09-29 20:03:36 UTC (rev 4778)
+++ pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/include/motor_qualification_controllers/motor_test3.h 2008-09-29 20:17:19 UTC (rev 4779)
@@ -38,7 +38,7 @@
/*! \class controller::MotorTest3
\brief Motor Test1 Controller
- This tests the motor enocder assembly to check if
+ This tests the motor enocder assembly to check if
the two are assembled properly.
*/
@@ -79,7 +79,7 @@
void init(double amplitude, std::string fixture_name, double time, std::string name ,mechanism::RobotState *robot);
bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
-
+
/*!
* \brief Perform the test analysis
*/
@@ -102,16 +102,13 @@
double initial_time_; /**< Start time of the test. */
bool complete;
misc_utils::RealtimePublisher<robot_msgs::DiagnosticMessage> publisher_;
- robot_msgs::DiagnosticMessage diagnostic_message_;
-
-
};
/***************************************************/
/*! \class controller::MotorTest3Node
\brief Motor Test1 Controller
- This tests the motor enocder assembly to check if
+ This tests the motor enocder assembly to check if
the two are assembled properly.
*/
Modified: pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/src/motor_test1.cpp
===================================================================
--- pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/src/motor_test1.cpp 2008-09-29 20:03:36 UTC (rev 4778)
+++ pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/src/motor_test1.cpp 2008-09-29 20:17:19 UTC (rev 4779)
@@ -43,7 +43,7 @@
{
robot_ = NULL;
joint_ = NULL;
-
+
duration_=0;
torque_=0;
initial_time_=0;
@@ -52,7 +52,7 @@
fixture_joint_end_pos_= 0;
test_joint_end_pos_= 0;
complete = false;
-
+
}
MotorTest1::~MotorTest1()
@@ -91,7 +91,7 @@
fprintf(stderr, "MotorTest1 could not find joint named \"%s\"\n", joint_name);
return false;
}
-
+
TiXmlElement *cd = j->FirstChildElement("controller_defaults");
if (cd)
{
@@ -116,7 +116,7 @@
void MotorTest1::update()
{
double time = robot_->hw_->current_time_;
-
+
if((time-initial_time_)<duration_)
{
joint_->commanded_effort_ = torque_;
@@ -132,7 +132,7 @@
analysis();
complete = true;
}
- else
+ else
return;
}
@@ -142,8 +142,12 @@
double t_delta = test_joint_end_pos_-test_joint_start_pos_;
double error = fabs(fabs(f_delta)-fabs(t_delta))/((fabs(f_delta)+fabs(t_delta))/2);
printf("f: %f , t: %f\n", f_delta, t_delta);
- diagnostic_message_.set_status_size(1);
- robot_msgs::DiagnosticStatus *status = diagnostic_message_.status;
+
+ // Screw realtime
+ publisher_.lock();
+
+ publisher_.msg_.set_status_size(1);
+ robot_msgs::DiagnosticStatus *status = publisher_.msg_.status;
status->name = "MotorTest";
printf("error: %f\n", error);
if (f_delta==0 && t_delta==0)
@@ -157,7 +161,7 @@
//the motor doesn't have an encoder
status->level = 2;
status->message = "ERROR: The motor encoder is not attached or not powered.";
-
+
}
else if(error<0.005)
{
@@ -167,23 +171,24 @@
}
else if(f_delta>0 && t_delta<0)
{
- //the encoder is reversed
+ //the encoder is reversed
status->level = 2;
status->message = "ERROR: The motor encoder is reversed.";
}
- else if(f_delta<0 && t_delta<0)
+ else if(f_delta<0 && t_delta<0)
{
//motor reversed
status->level = 2;
status->message = "ERROR: The motor wiring is reversed.";
- }
+ }
else
{
//test passed
status->level = 0;
status->message = "OK: Passed.";
- }
- publisher_.publish(diagnostic_message_);
+ }
+
+ publisher_.unlockAndPublish();
return;
}
Modified: pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/src/motor_test2.cpp
===================================================================
--- pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/src/motor_test2.cpp 2008-09-29 20:03:36 UTC (rev 4778)
+++ pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/src/motor_test2.cpp 2008-09-29 20:17:19 UTC (rev 4779)
@@ -45,7 +45,7 @@
robot_ = NULL;
actuator_ = NULL;
joint_ = NULL;
-
+
count_=1;
duration_=4;
torque_=0;
@@ -53,8 +53,8 @@
resistance_=10;
initial_time_=0;
complete = false;
-
-
+
+
}
MotorTest2::~MotorTest2()
@@ -94,7 +94,7 @@
fprintf(stderr, "MotorTest2 could not find joint named \"%s\"\n", joint_name);
return false;
}
-
+
TiXmlElement *cd = j->FirstChildElement("controller_defaults");
if (cd)
{
@@ -123,11 +123,11 @@
double time = robot_->hw_->current_time_;
static int first_time = 1;
static double zero_offset = 0;
-
-
+
+
if ((time-initial_time_)<duration_) {
- test_baseline_(count_) = actuator_->state_.motor_voltage_;
-
+ test_baseline_(count_) = actuator_->state_.motor_voltage_;
+
count_++;
}
else if((time-initial_time_)<2*duration_)
@@ -145,35 +145,38 @@
else if((time-initial_time_)<3*duration_)
{
joint_->commanded_effort_ = torque_;
- test_voltage_(count_) =actuator_->state_.motor_voltage_- zero_offset;
+ test_voltage_(count_) =actuator_->state_.motor_voltage_- zero_offset;
test_current_(count_) = actuator_->state_.last_measured_current_;
test_velocity_(count_) =joint_->velocity_;
count_++;
}
else if (!complete)
{
- joint_->commanded_effort_ = 0.0;
+ joint_->commanded_effort_ = 0.0;
analysis();
complete = true;
}
- else
+ else
return;
}
void MotorTest2::analysis()
{
- diagnostic_message_.set_status_size(1);
- robot_msgs::DiagnosticStatus *status = diagnostic_message_.status;
+ // Screw realtime
+ publisher_.lock();
+
+ publisher_.msg_.set_status_size(1);
+ robot_msgs::DiagnosticStatus *status = publisher_.msg_.status;
status->set_values_size(2);
status->name = "MotorTest";
NEWMAT::Matrix test_matrix =test_velocity_ | test_current_;
- QRZ(test_matrix, U_);
+ QRZ(test_matrix, U_);
QRZ(test_matrix, test_voltage_, M_);
NEWMAT::Matrix solution=U_.i()*M_;
double speed_const_meas = fabs(1/solution(1,1));
//ouble resistance_meas =fabs(solution(2,1)); this was all over the place no idea why??
-
+
if (fabs(speed_const_meas-speed_constant_)/speed_constant_ > 0.05)
{
//the motor isn't moving
@@ -194,8 +197,10 @@
status->values[0].value = speed_const_meas;
status->values[1].label = "expected speed constant";
status->values[1].value = speed_constant_;
- }
- publisher_.publish(diagnostic_message_);
+ }
+
+ publisher_.unlockAndPublish();
+
return;
}
Modified: pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/src/motor_test3.cpp
===================================================================
--- pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/src/motor_test3.cpp 2008-09-29 20:03:36 UTC (rev 4778)
+++ pkg/trunk/controllers/qualification_controllers/motor_qualification_controllers/src/motor_test3.cpp 2008-09-29 20:17:19 UTC (rev 4779)
@@ -44,14 +44,14 @@
robot_ = NULL;
joint_ = NULL;
-
-
+
+
duration_=20;
amplitude_=0;
initial_time_=0;
complete = false;
-
+
}
MotorTest3::~MotorTest3()
@@ -66,7 +66,7 @@
duration_=20;
sweep_controller_ = new SineSweepController();
sweep_controller_->init(1.0, 40, duration_, amplitude,robot->hw_->current_time_ ,name,robot);
-
+
amplitude_=amplitude;
initial_time_=time;
@@ -91,10 +91,10 @@
fprintf(stderr, "MotorTest3 could not find joint named \"%s\"\n", joint_name);
return false;
}
-
+
TiXmlElement *cd = j->FirstChildElement("controller_defaults");
if (cd)
- {
+ {
double amplitude = atof(cd->Attribute("amplitude"));
std::string fixture_name = cd->Attribute("fixture_name");
init(amplitude, fixture_name, robot->hw_->current_time_,j->Attribute("name"), robot);
@@ -111,34 +111,35 @@
void MotorTest3::update()
{
double time = robot_->hw_->current_time_;
-
-
+
+
if((time-initial_time_)<duration_)
{
sweep_controller_->update();
}
else if (!complete)
{
-
+
analysis();
complete = true;
}
- else
+ else
return;
}
void MotorTest3::analysis()
{
- diagnostic_message_.set_status_size(1);
- robot_msgs::DiagnosticStatus *status = diagnostic_message_.status;
+ publisher_.lock(); // Screw realtime
+ publisher_.msg_.set_status_size(1);
+ robot_msgs::DiagnosticStatus *status = publisher_.msg_.status;
status->name = "MotorTest";
-
+
//test passed
status->level = 0;
status->message = "OK: Passed.";
- publisher_.publish(diagnostic_message_);
+ publisher_.unlockAndPublish();
return;
}
Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp 2008-09-29 20:03:36 UTC (rev 4778)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp 2008-09-29 20:17:19 UTC (rev 4779)
@@ -236,8 +236,11 @@
}
// Publish status of each EtherCAT device
- diagnostic_message_.set_status_vec(statuses);
- publisher_.publish(diagnostic_message_);
+ if (publisher_.trylock())
+ {
+ publisher_.msg_.set_status_vec(statuses);
+ publisher_.unlockAndPublish();
+ }
}
void EthercatHardware::update()
Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/wg0x.cpp
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/wg0x.cpp 2008-09-29 20:03:36 UTC (rev 4778)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/wg0x.cpp 2008-09-29 20:17:19 UTC (rev 4779)
@@ -196,7 +196,7 @@
unsigned int minor = revision & 0xff;
printf("Device #%02d: WG0%d (%#08x) Firmware Revision %d.%02d, PCB Revision %c.%02d\n", sh_->get_ring_position(),
- sh_->get_product_code() == WG05::PRODUCT_CODE ? 5 : 6,
+ sh_->get_product_code() == WG05::PRODUCT_CODE ? 5 : 6,
sh_->get_product_code(), major, minor,
'A' + ((revision >> 24) & 0xff) - 1, (revision >> 16) & 0xff);
@@ -323,14 +323,16 @@
if (p->timestamp_ != last_pressure_time_)
{
- ethercat_hardware::PressureState pressure_;
- pressure_.set_data0_size(22);
- pressure_.set_data1_size(22);
- for (int i = 0; i < 22; ++i ) {
- pressure_.data0[i] = ((p->data0_[i] >> 8) & 0xff) | ((p->data0_[i] << 8) & 0xff00);
- pressure_.data1[i] = ((p->data1_[i] >> 8) & 0xff) | ((p->data1_[i] << 8) & 0xff00);
+ if (publisher_.trylock())
+ {
+ publisher_.msg_.set_data0_size(22);
+ publisher_.msg_.set_data1_size(22);
+ for (int i = 0; i < 22; ++i ) {
+ publisher_.msg_.data0[i] = ((p->data0_[i] >> 8) & 0xff) | ((p->data0_[i] << 8) & 0xff00);
+ publisher_.msg_.data1[i] = ((p->data1_[i] >> 8) & 0xff) | ((p->data1_[i] << 8) & 0xff00);
+ }
+ publisher_.unlockAndPublish();
}
- publisher_.publish(pressure_);
}
WG0X::convertState(state, current_buffer, last_buffer);
Modified: pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
===================================================================
--- pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2008-09-29 20:03:36 UTC (rev 4778)
+++ pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2008-09-29 20:17:19 UTC (rev 4779)
@@ -123,15 +123,10 @@
static const double STATE_PUBLISHING_PERIOD = 0.01; // this translates to about 100Hz
- mechanism_control::MechanismState mechanism_state_;
const char* const mechanism_state_topic_;
misc_utils::RealtimePublisher<mechanism_control::MechanismState> publisher_;
- rosTF::TransformArray transform_array_msg_;
- misc_utils::RealtimePublisher<rosTF::TransformArray> transform_array_publisher_;
-
- rostools::Time time_msg_;
- misc_utils::RealtimePublisher<rostools::Time> time_publisher_;
+ misc_utils::RealtimePublisher<rosTF::TransformArray> transform_publisher_;
};
#endif /* MECHANISM_CONTROL_H */
Modified: pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-09-29 20:03:36 UTC (rev 4778)
+++ pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-09-29 20:17:19 UTC (rev 4779)
@@ -145,7 +145,7 @@
controller::Controller *c = controller::ControllerFactory::instance().create(type);
if (c == NULL)
return false;
- printf("Spawning %s: %08x\n", name.c_str(), &model_);
+ printf("Spawning %s: %08x\n", name.c_str(), (unsigned int)&model_);
if (!c->initXml(state_, config) ||
@@ -189,7 +189,7 @@
MechanismControlNode::MechanismControlNode(MechanismControl *mc)
: mc_(mc), mechanism_state_topic_("mechanism_state"), publisher_(mechanism_state_topic_, 1),
- transform_array_publisher_("TransformArray", 1), time_publisher_("time", 1)
+ transform_publisher_("TransformArray", 1)
{
assert(mc != NULL);
assert(mechanism_state_topic_);
@@ -229,9 +229,9 @@
{
if (!mc_->initXml(config))
return false;
- mechanism_state_.set_joint_states_size(mc_->model_.joints_.size());
- mechanism_state_.set_actuator_states_size(mc_->hw_->actuators_.size());
- transform_array_msg_.set_quaternions_size(mc_->model_.links_.size());
+ publisher_.msg_.set_joint_states_size(mc_->model_.joints_.size());
+ publisher_.msg_.set_actuator_states_size(mc_->hw_->actuators_.size());
+ transform_publisher_.msg_.set_quaternions_size(mc_->model_.links_.size());
return true;
}
@@ -242,77 +242,80 @@
static double last_publish_time = 0.0;
if (mc_->hw_->current_time_ - last_publish_time > STATE_PUBLISHING_PERIOD)
{
- assert(mc_->model_.joints_.size() == mechanism_state_.get_joint_states_size());
- for (unsigned int i = 0; i < mc_->model_.joints_.size(); ++i)
+ if (publisher_.trylock())
{
- mechanism_control::JointState *out = &mechanism_state_.joint_states[i];
- mechanism::JointState *in = &mc_->state_->joint_states_[i];
- out->name = mc_->model_.joints_[i]->name_;
- out->position = in->position_;
- out->velocity = in->velocity_;
- out->applied_effort = in->applied_effort_;
- out->commanded_effort = in->commanded_effort_;
- }
+ assert(mc_->model_.joints_.size() == publisher_.msg_.get_joint_states_size());
+ for (unsigned int i = 0; i < mc_->model_.joints_.size(); ++i)
+ {
+ mechanism_control::JointState *out = publisher_.msg_.joint_states + i;
+ mechanism::JointState *in = &mc_->state_->joint_states_[i];
+ out->name = mc_->model_.joints_[i]->name_;
+ out->position = in->position_;
+ out->velocity = in->velocity_;
+ out->applied_effort = in->applied_effort_;
+ out->commanded_effort = in->commanded_effort_;
+ }
- for (unsigned int i = 0; i < mc_->hw_->actuators_.size(); ++i)
- {
- mechanism_control::ActuatorState *out = &mechanism_state_.actuator_states[i];
- ActuatorState *in = &mc_->hw_->actuators_[i]->state_;
- out->name = mc_->hw_->actuators_[i]->name_;
- out->encoder_count = in->encoder_count_;
- out->position = in->position_;
- out->timestamp = in->timestamp_;
- out->encoder_velocity = in->encoder_velocity_;
- out->velocity = in->velocity_;
- out->calibration_reading = in->calibration_reading_;
- out->last_calibration_high_transition = in->last_calibration_high_transition_;
- out->last_calibration_low_transition = in->last_calibration_low_transition_;
- out->is_enabled = in->is_enabled_;
- out->run_stop_hit = in->run_stop_hit_;
- out->last_requested_current = in->last_requested_current_;
- out->last_commanded_current = in->last_commanded_current_;
- out->last_measured_current = in->last_measured_current_;
- out->last_requested_effort = in->last_requested_effort_;
- out->last_commanded_effort = in->last_commanded_effort_;
- out->last_measured_effort = in->last_measured_effort_;
- out->motor_voltage = in->motor_voltage_;
- out->num_encoder_errors = in->num_encoder_errors_;
+ for (unsigned int i = 0; i < mc_->hw_->actuators_.size(); ++i)
+ {
+ mechanism_control::ActuatorState *out = publisher_.msg_.actuator_states + i;
+ ActuatorState *in = &mc_->hw_->actuators_[i]->state_;
+ out->name = mc_->hw_->actuators_[i]->name_;
+ out->encoder_count = in->encoder_count_;
+ out->position = in->position_;
+ out->timestamp = in->timestamp_;
+ out->encoder_velocity = in->encoder_velocity_;
+ out->velocity = in->velocity_;
+ out->calibration_reading = in->calibration_reading_;
+ out->last_calibration_high_transition = in->last_calibration_high_transition_;
+ out->last_calibration_low_transition = in->last_calibration_low_transition_;
+ out->is_enabled = in->is_enabled_;
+ out->run_stop_hit = in->run_stop_hit_;
+ out->last_requested_current = in->last_requested_current_;
+ out->last_commanded_current = in->last_commanded_current_;
+ out->last_measured_current = in->last_measured_current_;
+ out->last_requested_effort = in->last_requested_effort_;
+ out->last_commanded_effort = in->last_commanded_effort_;
+ out->last_measured_effort = in->last_measured_effort_;
+ out->motor_voltage = in->motor_voltage_;
+ out->num_encoder_errors = in->num_encoder_errors_;
+ }
+ publisher_.msg_.time = mc_->hw_->current_time_;
+
+ publisher_.unlockAndPublish();
}
- mechanism_state_.time = mc_->hw_->current_time_;
- publisher_.publish(mechanism_state_);
-
// Frame transforms
- assert(mc_->model_.links_.size() == transform_array_msg_.get_quaternions_size());
- for (unsigned int i = 0; i < mc_->model_.links_.size(); ++i)
+ if (transform_publisher_.trylock())
{
- if (mc_->model_.links_[i]->parent_name_ == std::string("world"))
- continue;
+ assert(mc_->model_.links_.size() == transform_publisher_.msg_.get_quaternions_size());
+ for (unsigned int i = 0; i < mc_->model_.links_.size(); ++i)
+ {
+ if (mc_->model_.links_[i]->parent_name_ == std::string("world"))
+ continue;
- libTF::Position pos;
- libTF::Quaternion quat;
- mc_->state_->link_states_[i].rel_frame_.getPosition(pos);
- mc_->state_->link_states_[i].rel_frame_.getQuaternion(quat);
- rosTF::TransformQuaternion &out = transform_array_msg_.quaternions[i];
+ libTF::Position pos;
+ libTF::Quaternion quat;
+ mc_->state_->link_states_[i].rel_frame_.getPosition(pos);
+ mc_->state_->link_states_[i].rel_frame_.getQuaternion(quat);
+ rosTF::TransformQuaternion &out = transform_publisher_.msg_.quaternions[i];
- out.header.stamp.from_double(mc_->hw_->current_time_);
- out.header.frame_id = mc_->model_.links_[i]->name_;
- out.parent = mc_->model_.links_[i]->parent_name_;
- out.xt = pos.x;
- out.yt = pos.y;
- out.zt = pos.z;
- out.w = quat.w;
- out.xr = quat.x;
- out.yr = quat.y;
- out.zr = quat.z;
+ out.header.stamp.from_double(mc_->hw_->current_time_);
+ out.header.frame_id = mc_->model_.links_[i]->name_;
+ out.parent = mc_->model_.links_[i]->parent_name_;
+ out.xt = pos.x;
+ out.yt = pos.y;
+ out.zt = pos.z;
+ out.w = quat.w;
+ out.xr = quat.x;
+ out.yr = quat.y;
+ out.zr = quat.z;
+ }
+
+ transform_publisher_.unlockAndPublish();
}
- transform_array_publisher_.publish(transform_array_msg_);
- // rostime
- time_msg_.rostime.from_double(mc_->hw_->current_time_);
- time_publisher_.publish(time_msg_);
-
last_publish_time = mc_->hw_->current_time_;
}
}
Modified: pkg/trunk/util/misc_utils/include/misc_utils/realtime_publisher.h
===================================================================
--- pkg/trunk/util/misc_utils/include/misc_utils/realtime_publisher.h 2008-09-29 20:03:36 UTC (rev 4778)
+++ pkg/trunk/util/misc_utils/include/misc_utils/realtime_publisher.h 2008-09-29 20:17:19 UTC (rev 4779)
@@ -50,7 +50,7 @@
{
public:
RealtimePublisher(const std::string &topic, int queue_size)
- : topic_(topic), node_(NULL), is_running_(false), keep_running_(false), thread_(NULL)
+ : topic_(topic), node_(NULL), is_running_(false), keep_running_(false), thread_(NULL), turn_(REALTIME)
{
if ((node_ = ros::node::instance()) == NULL)
{
@@ -70,6 +70,7 @@
~RealtimePublisher()
{
+ node_->unadvertise(topic_);
}
void stop()
@@ -77,31 +78,57 @@
keep_running_ = false;
}
- void publish(const Msg &msg)
+ Msg msg_;
+
+ void lock()
{
+ pthread_mutex_lock(&msg_lock_);
+ }
+
+ bool trylock()
+ {
if (0 == pthread_mutex_trylock(&msg_lock_))
{
- msg_ = msg;
- msg_updated_ = true;
- pthread_mutex_unlock(&msg_lock_);
- pthread_cond_signal(&updated_cond_);
+ if (turn_ == REALTIME)
+ {
+ return true;
+ }
+ else
+ {
+ pthread_mutex_unlock(&msg_lock_);
+ return false;
+ }
}
+ else
+ return false;
}
+ void unlock()
+ {
+ pthread_mutex_unlock(&msg_lock_);
+ }
+
+ void unlockAndPublish()
+ {
+ turn_ = NON_REALTIME;
+ pthread_mutex_unlock(&msg_lock_);
+ pthread_cond_signal(&updated_cond_);
+ }
+
bool is_running() const { return is_running_; }
void publishingLoop()
{
is_running_ = true;
- msg_updated_ = false;
+ turn_ = REALTIME;
while (keep_running_)
{
// Locks msg_ and copies it
pthread_mutex_lock(&msg_lock_);
- while (!msg_updated_)
+ while (turn_ != NON_REALTIME)
pthread_cond_wait(&updated_cond_, &msg_lock_);
Msg outgoing(msg_);
- msg_updated_ = false;
+ turn_ = REALTIME;
pthread_mutex_unlock(&msg_lock_);
// Sends the outgoing message
@@ -111,6 +138,7 @@
}
private:
+
std::string topic_;
ros::node *node_;
bool is_running_;
@@ -118,10 +146,11 @@
pthread_t *thread_;
- Msg msg_;
- bool msg_updated_;
pthread_mutex_t msg_lock_; // Protects msg_
pthread_cond_t updated_cond_;
+
+ enum {REALTIME, NON_REALTIME};
+ int turn_; // Who's turn is it to use msg_?
};
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2008-09-29 23:11:58
|
Revision: 4792
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4792&view=rev
Author: stuglaser
Date: 2008-09-29 23:11:37 +0000 (Mon, 29 Sep 2008)
Log Message:
-----------
The calibration controller now works consistently.
Modified Paths:
--------------
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_calibration_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp
pkg/trunk/manip/teleop_robot/scripts/calibrate.py
pkg/trunk/mechanism/hardware_interface/include/hardware_interface/hardware_interface.h
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_calibration_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_calibration_controller.h 2008-09-29 23:11:18 UTC (rev 4791)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_calibration_controller.h 2008-09-29 23:11:37 UTC (rev 4792)
@@ -43,6 +43,17 @@
This class moves the joint and reads the value of the clibration_reading_ field to find the zero position of the joint. Once these are determined, these values
* are passed to the joint and enable the joint for the other controllers.
+ Example XML:
+ <controller type="JointCalibrationController">
+ <calibrate joint="upperarm_roll_right_joint"
+ actuator="upperarm_roll_right_act"
+ transmission="upperarm_roll_right_trans"
+ velocity="0.3" />
+ <pid p="15" i="0" d="0" iClamp="0" />
+ </controller>
+
+
+
*/
/***************************************************/
@@ -56,7 +67,7 @@
namespace controller
{
-class JointCalibrationController : public JointManualCalibrationController
+class JointCalibrationController : public controller::Controller
{
public:
/*!
@@ -82,15 +93,26 @@
*/
virtual void update();
+ bool calibrated() { return state_ == STOPPED; }
+ void beginCalibration()
+ {
+ if (state_ == INITIALIZED)
+ state_ = BEGINNING;
+ }
+
protected:
- double previous_reading_; /**Previous calibration reading*/
+ enum { INITIALIZED, BEGINNING, MOVING, STOPPED };
+ int state_;
double search_velocity_;
+ bool original_switch_state_;
- double velocity_cmd_;
+ Actuator *actuator_;
+ mechanism::JointState *joint_;
+ mechanism::Transmission *transmission_;
- controller::JointVelocityController vcontroller_; /** The joint velocity controller used to sweep the joint.*/
+ controller::JointVelocityController vc_;
};
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp 2008-09-29 23:11:18 UTC (rev 4791)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp 2008-09-29 23:11:37 UTC (rev 4792)
@@ -41,7 +41,7 @@
ROS_REGISTER_CONTROLLER(JointCalibrationController)
JointCalibrationController::JointCalibrationController()
- : JointManualCalibrationController()
+: state_(INITIALIZED)
{
}
@@ -53,78 +53,110 @@
{
assert(robot);
assert(config);
- robot_ = robot->model_;
- bool base=JointManualCalibrationController::initXml(robot, config);
- if(!base)
- return false;
- TiXmlElement *j = config->FirstChildElement("param");
- if (!j)
+
+ TiXmlElement *cal = config->FirstChildElement("calibrate");
+ if (!cal)
{
- std::cerr<<"JointCalibrationController was not given parameters"<<std::endl;
+ std::cerr<<"JointCalibrationController was not given calibration parameters"<<std::endl;
return false;
}
- if(j->QueryDoubleAttribute("velocity", &search_velocity_ ) != TIXML_SUCCESS)
+ if(cal->QueryDoubleAttribute("velocity", &search_velocity_) != TIXML_SUCCESS)
{
std::cerr<<"Velocity value was not specified\n";
return false;
}
- if(search_velocity_ == 0)
+ const char *joint_name = cal->Attribute("joint");
+ joint_ = joint_name ? robot->getJointState(joint_name) : NULL;
+ if (!joint_)
{
- std::cerr<<"You gave zero velocity\n";
+ fprintf(stderr, "Error: JointCalibrationController could not find joint \"%s\"\n",
+ joint_name);
return false;
}
- TiXmlElement *v = config->FirstChildElement("controller");
- if(!v)
+ const char *actuator_name = cal->Attribute("actuator");
+ actuator_ = actuator_name ? robot->model_->getActuator(actuator_name) : NULL;
+ if (!actuator_)
{
- std::cerr<<"JointCalibrationController was not given a controller to move the joint."<<std::endl;
+ fprintf(stderr, "Error: JointCalibrationController could not find actuator \"%s\"\n",
+ actuator_name);
return false;
}
- return vcontroller_.initXml(robot, v);
+ const char *transmission_name = cal->Attribute("transmission");
+ transmission_ = transmission_name ? robot->model_->getTransmission(transmission_name) : NULL;
+ if (!transmission_)
+ {
+ fprintf(stderr, "Error: JointCalibrationController could not find transmission \"%s\"\n",
+ transmission_name);
+ return false;
+ }
+
+ control_toolbox::Pid pid;
+ TiXmlElement *pid_el = config->FirstChildElement("pid");
+ if (!pid_el)
+ {
+ fprintf(stderr, "Error: JointCalibrationController was not given a pid element.\n");
+ return false;
+ }
+ if (!pid.initXml(pid_el))
+ return false;
+
+ if (!vc_.init(robot, joint_name, pid))
+ return false;
+
+ return true;
}
void JointCalibrationController::update()
{
- assert(joint_state_);
+ assert(joint_);
assert(actuator_);
- const double cur_reading = actuator_->state_.calibration_reading_;
- if(state_ == Begin)
+ switch(state_)
{
- joint_state_->calibrated_ = false;
- previous_reading_ = cur_reading;
- velocity_cmd_ = cur_reading > 0.5 ? search_velocity_ : -search_velocity_;
- state_ = Search;
- }
+ case INITIALIZED:
+ vc_.setCommand(0.0);
+ break;
+ case BEGINNING:
+ original_switch_state_ = actuator_->state_.calibration_reading_ > 0.5;
+ vc_.setCommand(original_switch_state_ ? search_velocity_ : -search_velocity_);
+ state_ = MOVING;
+ break;
+ case MOVING: {
+ bool switch_state_ = actuator_->state_.calibration_reading_ > 0.5;
+ if (switch_state_ != original_switch_state_)
+ {
+ std::vector<Actuator*> fake_a;
+ std::vector<mechanism::JointState*> fake_j;
+ fake_a.push_back(new Actuator);
+ fake_j.push_back(new mechanism::JointState);
- if(std::abs(cur_reading-previous_reading_)>0.5 && state_==Search)
- {
- const double offset_ = offset(actuator_->state_.position_, joint_state_->joint_->reference_position_);
- actuator_->state_.zero_offset_ = offset_;
- state_ = Initialized;
- }
+ // Where was the joint when the optical switch triggered?
+ if (original_switch_state_ = true)
+ fake_a[0]->state_.position_ = actuator_->state_.last_calibration_high_transition_;
+ else
+ fake_a[0]->state_.position_ = actuator_->state_.last_calibration_low_transition_;
+ transmission_->propagatePosition(fake_a, fake_j);
- if(state_ == Initialized)
- {
- joint_state_->calibrated_ = true;
- velocity_cmd_ = 0;
- state_ = Stop;
- state_mutex_.unlock();
- }
+ // What is the actuator position at the joint's zero?
+ fake_j[0]->position_ = fake_j[0]->position_ - joint_->joint_->reference_position_;
+ transmission_->propagatePositionBackwards(fake_j, fake_a);
- if(state_ == Stop)
- {
- velocity_cmd_ = 0;
+ actuator_->state_.zero_offset_ = fake_a[0]->state_.position_;
+
+ state_ = STOPPED;
+ }
+ break;
}
+ case STOPPED:
+ vc_.setCommand(0.0);
+ break;
+ }
- if(state_!=Stop)
- {
- vcontroller_.setCommand(velocity_cmd_);
- vcontroller_.update();
- }
+ vc_.update();
}
@@ -152,7 +184,7 @@
ros::Duration d=ros::Duration(0,1000000);
while(!c_->calibrated())
d.sleep();
- c_->getOffset(resp.offset);
+ resp.offset = 0;
return true;
}
Modified: pkg/trunk/manip/teleop_robot/scripts/calibrate.py
===================================================================
--- pkg/trunk/manip/teleop_robot/scripts/calibrate.py 2008-09-29 23:11:18 UTC (rev 4791)
+++ pkg/trunk/manip/teleop_robot/scripts/calibrate.py 2008-09-29 23:11:37 UTC (rev 4792)
@@ -92,15 +92,33 @@
kill_controller(name)
print "Calibrated"
-calibrate_optically(slurp('pr2_arm/calibration_shoulder_pan.xml'))
-calibrate_optically(slurp('pr2_arm/calibration_shoulder_lift.xml'))
+calibrate_optically('''
+<controller name="cal_shoulder_pan" topic="cal_shoulder_pan" type="JointCalibrationControllerNode">
+ <calibrate joint="shoulder_pan_right_joint"
+ actuator="shoulder_pan_right_act"
+ transmission="shoulder_pan_right_trans"
+ velocity="0.6" />
+ <pid p="7" i="0.5" d="0" iClamp="1.0" />
+</controller>
+''')
+
+calibrate_optically('''
+<controller name="cal_shoulder_pitch" topic="cal_shoulder_pitch" type="JointCalibrationControllerNode">
+ <calibrate joint="shoulder_pitch_right_joint"
+ actuator="shoulder_lift_right_act"
+ transmission="shoulder_lift_right_trans"
+ velocity="0.6" />
+ <pid p="7" i="0.5" d="0" iClamp="1.0" />
+</controller>
+''')
+
calibrate_blindly('''
<controller name="upperarm_calibration" topic="upperarm_calibration" type="JointBlindCalibrationControllerNode">
<calibrate joint="upperarm_roll_right_joint"
actuator="upperarm_roll_right_act"
transmission="upperarm_roll_right_trans"
- velocity="0.6" />
+ velocity="0.9" />
<pid p="5" i="0.5" d="0" iClamp="1.0" />
</controller>
@@ -111,7 +129,7 @@
<calibrate joint="elbow_right_joint"
actuator="elbow_right_act"
transmission="elbow_right_trans"
- velocity="0.6" />
+ velocity="0.8" />
<pid p="5" i="0.5" d="0" iClamp="1.0" />
</controller>
Modified: pkg/trunk/mechanism/hardware_interface/include/hardware_interface/hardware_interface.h
===================================================================
--- pkg/trunk/mechanism/hardware_interface/include/hardware_interface/hardware_interface.h 2008-09-29 23:11:18 UTC (rev 4791)
+++ pkg/trunk/mechanism/hardware_interface/include/hardware_interface/hardware_interface.h 2008-09-29 23:11:37 UTC (rev 4792)
@@ -63,7 +63,7 @@
double velocity_;
bool calibration_reading_;
- int32_t last_calibration_high_transition_;
+ int32_t last_calibration_high_transition_; // Last transition from high to low.
int32_t last_calibration_low_transition_;
bool is_enabled_;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2008-09-29 23:23:20
|
Revision: 4793
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4793&view=rev
Author: stuglaser
Date: 2008-09-29 23:23:08 +0000 (Mon, 29 Sep 2008)
Log Message:
-----------
The calibration controller comes down cleanly.
Modified Paths:
--------------
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_calibration_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp
pkg/trunk/mechanism/mechanism_model/src/robot.cpp
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_calibration_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_calibration_controller.h 2008-09-29 23:11:37 UTC (rev 4792)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_calibration_controller.h 2008-09-29 23:23:08 UTC (rev 4793)
@@ -150,7 +150,9 @@
private:
JointCalibrationController *c_;
+ AdvertisedServiceGuard guard_calibrate_;
};
+
}
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp 2008-09-29 23:11:37 UTC (rev 4792)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp 2008-09-29 23:23:08 UTC (rev 4793)
@@ -202,5 +202,7 @@
if (!c_->initXml(robot, config))
return false;
node->advertise_service(topic + "/calibrate", &JointCalibrationControllerNode::calibrateCommand, this);
+ guard_calibrate_.set(topic + "/calibrate");
+
return true;
}
Modified: pkg/trunk/mechanism/mechanism_model/src/robot.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_model/src/robot.cpp 2008-09-29 23:11:37 UTC (rev 4792)
+++ pkg/trunk/mechanism/mechanism_model/src/robot.cpp 2008-09-29 23:23:08 UTC (rev 4793)
@@ -256,7 +256,7 @@
break;
}
- rotation.setFromEuler(0,0,0, l.origin_rpy_[0], l.origin_rpy_[1], l.origin_rpy_[2]);
+ rotation.setFromEuler(0,0,0, l.origin_rpy_[2], l.origin_rpy_[1], l.origin_rpy_[0]);
link_states_[i].rel_frame_.setIdentity();
link_states_[i].rel_frame_.multiplyPose(offset);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-09-30 00:17:29
|
Revision: 4795
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4795&view=rev
Author: hsujohnhsu
Date: 2008-09-30 00:17:02 +0000 (Tue, 30 Sep 2008)
Log Message:
-----------
stripped ifaces from ros plugins for gazebo.
udpate 2dnav gazebo to non-respawn dead pr2_guis.
Modified Paths:
--------------
pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-simple-headless-fake_localization.xml
pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-simple-headless.xml
pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg-fake_localization.xml
pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg-headless-fake_localization.xml
pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg-headless.xml
pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg.xml
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Block_Laser.hh
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Camera.hh
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Laser.hh
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_PTZ.hh
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Time.hh
pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc
pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Block_Laser.cc
pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Camera.cc
pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Laser.cc
pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_PTZ.cc
Modified: pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-simple-headless-fake_localization.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-simple-headless-fake_localization.xml 2008-09-29 23:27:11 UTC (rev 4794)
+++ pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-simple-headless-fake_localization.xml 2008-09-30 00:17:02 UTC (rev 4795)
@@ -8,7 +8,7 @@
<node pkg="nav_view" type="nav_view" respawn="false" output="screen" />
<!-- for visualization -->
- <node pkg="pr2_gui" type="pr2_gui" respawn="true" output="screen" />
+ <node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" />
<!-- for manual control -->
<!--node pkg="teleop_base_keyboard" type="teleop_base_keyboard" respawn="false" output="screen" /-->
Modified: pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-simple-headless.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-simple-headless.xml 2008-09-29 23:27:11 UTC (rev 4794)
+++ pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-simple-headless.xml 2008-09-30 00:17:02 UTC (rev 4795)
@@ -8,7 +8,7 @@
<node pkg="nav_view" type="nav_view" respawn="false" output="screen" />
<!-- for visualization -->
- <node pkg="pr2_gui" type="pr2_gui" respawn="true" output="screen" />
+ <node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" />
<!-- for manual control -->
<!--node pkg="teleop_base_keyboard" type="teleop_base_keyboard" respawn="false" output="screen" /-->
Modified: pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg-fake_localization.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg-fake_localization.xml 2008-09-29 23:27:11 UTC (rev 4794)
+++ pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg-fake_localization.xml 2008-09-30 00:17:02 UTC (rev 4795)
@@ -8,7 +8,7 @@
<node pkg="nav_view" type="nav_view" respawn="false" output="screen" />
<!-- for visualization -->
- <node pkg="pr2_gui" type="pr2_gui" respawn="true" output="screen" />
+ <node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" />
<!-- for manual control -->
<!--node pkg="teleop_base_keyboard" type="teleop_base_keyboard" respawn="false" output="screen" /-->
Modified: pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg-headless-fake_localization.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg-headless-fake_localization.xml 2008-09-29 23:27:11 UTC (rev 4794)
+++ pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg-headless-fake_localization.xml 2008-09-30 00:17:02 UTC (rev 4795)
@@ -8,7 +8,7 @@
<node pkg="nav_view" type="nav_view" respawn="false" output="screen" />
<!-- for visualization -->
- <node pkg="pr2_gui" type="pr2_gui" respawn="true" output="screen" />
+ <node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" />
<!-- for manual control -->
<!--node pkg="teleop_base_keyboard" type="teleop_base_keyboard" respawn="false" output="screen" /-->
Modified: pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg-headless.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg-headless.xml 2008-09-29 23:27:11 UTC (rev 4794)
+++ pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg-headless.xml 2008-09-30 00:17:02 UTC (rev 4795)
@@ -8,7 +8,7 @@
<node pkg="nav_view" type="nav_view" respawn="false" output="screen" />
<!-- for visualization -->
- <node pkg="pr2_gui" type="pr2_gui" respawn="true" output="screen" />
+ <node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" />
<!-- for manual control -->
<!--node pkg="teleop_base_keyboard" type="teleop_base_keyboard" respawn="false" output="screen" /-->
Modified: pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg.xml 2008-09-29 23:27:11 UTC (rev 4794)
+++ pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-wg.xml 2008-09-30 00:17:02 UTC (rev 4795)
@@ -5,7 +5,7 @@
<node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/willowMap.png 0.1" respawn="false" />
<node pkg="amcl_player" type="amcl_player" args="scan:=base_scan" respawn="false" />
<node pkg="wavefront_player" type="wavefront_player" args="scan:=base_scan" respawn="false" />
- <node pkg="nav_view" type="nav_view" respawn="true" />
+ <node pkg="nav_view" type="nav_view" respawn="false" />
<!--node pkg="teleop_base_keyboard" type="teleop_base_keyboard" respawn="false" /-->
</group>
</launch>
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh 2008-09-29 23:27:11 UTC (rev 4794)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh 2008-09-30 00:17:02 UTC (rev 4795)
@@ -66,7 +66,7 @@
/// \brief P3D controller
/// \li Starts a ROS node if none exists.
- /// \li This controller simulates a 6 dof position and rate sensor.
+ /// \li This controller simulates a 6 dof position and rate sensor, publishes std_msgs::TransformWithRateStamped.msg ROS topic.
class P3D : public Controller
{
/// \brief Constructor
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Block_Laser.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Block_Laser.hh 2008-09-29 23:27:11 UTC (rev 4794)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Block_Laser.hh 2008-09-30 00:17:02 UTC (rev 4795)
@@ -36,8 +36,6 @@
namespace gazebo
{
- class LaserIface;
- class FiducialIface;
class RaySensor;
/// @addtogroup gazebo_dynamic_plugins Gazebo ROS Dynamic Plugins
@@ -46,7 +44,7 @@
\brief ROS Block Laser Scanner Controller Plugin
- This is a controller that gathers range data from a ray sensor, and returns results via publishing ROS topic for point clouds and Iface.
+ This is a controller that gathers range data from a ray sensor, and returns results via publishing ROS topic for point clouds.
\verbatim
<model:physical name="ray_model">
@@ -90,6 +88,7 @@
/// \brief ROS laser block simulation.
/// \li Starts a ROS node if none exists.
/// \li This controller simulates a block of laser range detections.
+/// Resulting point cloud (std_msgs::PointCloudFloat32.msg are published as a ROS topic.
class Ros_Block_Laser : public Controller
{
/// \brief Constructor
@@ -112,17 +111,9 @@
/// \brief Finalize the controller
protected: virtual void FiniChild();
- /// \brief Put laser data to the iface
+ /// \brief Put laser data to the ROS topic
private: void PutLaserData();
- /// \brief Put fiducial data to the iface
- private: void PutFiducialData();
-
- /// \brief The laser interface
- private: LaserIface *laserIface;
-
- private: FiducialIface *fiducialIface;
-
/// \brief The parent sensor
private: RaySensor *myParent;
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Camera.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Camera.hh 2008-09-29 23:27:11 UTC (rev 4794)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Camera.hh 2008-09-30 00:17:02 UTC (rev 4795)
@@ -33,7 +33,6 @@
namespace gazebo
{
- class CameraIface;
class MonoCameraSensor;
/// @addtogroup gazebo_dynamic_plugins Gazebo ROS Dynamic Plugins
@@ -53,7 +52,6 @@
<updateRate>15.0</updateRate>
<topicName>camera_name/image</topicName>
<frameName>camera_body_name</frameName>
- <interface:camera name="ros_camera_iface" />
</controller:ros_camera>
</sensor:camera>
</body:empty>
@@ -88,12 +86,9 @@
/// \brief Finalize the controller, unadvertise topics
protected: virtual void FiniChild();
- /// \brief Put camera data to the iface
+ /// \brief Put camera data to the ROS topic
private: void PutCameraData();
- /// \brief The camera Iface
- private: CameraIface *cameraIface;
-
/// \brief A pointer to the parent camera sensor
private: MonoCameraSensor *myParent;
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Laser.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Laser.hh 2008-09-29 23:27:11 UTC (rev 4794)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Laser.hh 2008-09-30 00:17:02 UTC (rev 4795)
@@ -35,8 +35,6 @@
namespace gazebo
{
- class LaserIface;
- class FiducialIface;
class RaySensor;
/// @addtogroup gazebo_dynamic_plugins Gazebo ROS Dynamic Plugins
@@ -46,7 +44,7 @@
\brief ROS Laser Scanner Controller Plugin
This controller gathers range data from a simulated ray sensor, publishes range data through
- std_msgs::LaserScan ROS topic and Gazebo Iface.
+ std_msgs::LaserScan ROS topic.
\verbatim
<model:physical name="ray_model">
@@ -80,7 +78,7 @@
/// \brief ROS laser scan controller.
/// \li Starts a ROS node if none exists.
-/// \li Simulates a laser range sensor.
+/// \li Simulates a laser range sensor and publish results over ROS on std_msgs::LaserScan.msg
class Ros_Laser : public Controller
{
/// \brief Constructor
@@ -103,17 +101,9 @@
/// \brief Finalize the controller
protected: virtual void FiniChild();
- /// \brief Put laser data to the iface
+ /// \brief Put laser data to the ROS topic
private: void PutLaserData();
- /// \brief Put fiducial data to the iface
- private: void PutFiducialData();
-
- /// \brief The laser interface
- private: LaserIface *laserIface;
-
- private: FiducialIface *fiducialIface;
-
/// \brief The parent sensor
private: RaySensor *myParent;
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_PTZ.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_PTZ.hh 2008-09-29 23:27:11 UTC (rev 4794)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_PTZ.hh 2008-09-30 00:17:02 UTC (rev 4795)
@@ -41,7 +41,6 @@
namespace gazebo
{
class HingeJoint;
- class PTZIface;
/// @addtogroup gazebo_dynamic_plugins Gazebo ROS Dynamic Plugins
/// @{
@@ -68,9 +67,12 @@
\{
*/
- /// \brief Ros ptz controller.
+ /// \brief ROS Pan/Tilt/Zoom Camera Controller
+ /// \li Starts a ROS node if none exists.
+ /// \li Simulates PTZ camera actuators.
+ /// - publish state information (PT angles) to ROS topic: \e camera_name/ptz_state
+ /// - subscribe to ROS topic: \e camera_name/ptz_cmd
///
- /// This is a controller for a ros PTZ
class Ros_PTZ : public Controller
{
/// \brief Constructor
@@ -100,12 +102,9 @@
/// \brief Reset the controller
protected: virtual void ResetChild();
- /// \brief Put camera data to the iface
+ /// \brief Put camera data to the ROS topic
private: void PutPTZData();
- /// \brief The camera interface
- private: PTZIface *ptzIface;
-
/// \brief The parent sensor
private: Model *myParent;
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Time.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Time.hh 2008-09-29 23:27:11 UTC (rev 4794)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Time.hh 2008-09-30 00:17:02 UTC (rev 4795)
@@ -69,8 +69,10 @@
\{
*/
-/// \brief Starts a ROS node if none exists and broadcast simulator time
-/// over rostools::Time.
+/// \brief ROS Time Controller
+/// \li Starts a ROS node if none exists
+/// \li broadcast simulator time over rostools::Time.
+/// .
class Ros_Time : public Controller
{
/// \brief Constructor
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc 2008-09-29 23:27:11 UTC (rev 4794)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc 2008-09-30 00:17:02 UTC (rev 4795)
@@ -77,7 +77,7 @@
this->myIface = dynamic_cast<PositionIface*>(this->ifaces[0]);
if (!this->myIface)
- gzthrow("P3D controller requires a Actarray Iface");
+ gzthrow("P3D controller requires a Actarray Iface, though not used.");
std::string bodyName = node->GetString("bodyName", "", 1);
this->myBody = dynamic_cast<Body*>(this->myParent->GetBody(bodyName));
@@ -116,17 +116,8 @@
rpyTotal.z = this->rpyOffsets.z + rot.GetYaw();
rot.SetFromEuler(rpyTotal);
- this->myIface->Lock(1);
- this->myIface->data->head.time = Simulator::Instance()->GetSimTime();
+ double cur_time = Simulator::Instance()->GetSimTime();
- this->myIface->data->pose.pos.x = pos.x;
- this->myIface->data->pose.pos.y = pos.y;
- this->myIface->data->pose.pos.z = pos.z;
-
- this->myIface->data->pose.roll = rot.GetRoll();
- this->myIface->data->pose.pitch = rot.GetPitch();
- this->myIface->data->pose.yaw = rot.GetYaw();
-
// get Rates
Vector3 vpos = this->myBody->GetPositionRate(); // get velocity in gazebo frame
Quatern vrot = this->myBody->GetRotationRate(); // get velocity in gazebo frame
@@ -135,8 +126,8 @@
this->lock.lock();
// copy data into pose message
this->transformMsg.header.frame_id = this->frameName;
- this->transformMsg.header.stamp.sec = (unsigned long)floor(this->myIface->data->head.time);
- this->transformMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->myIface->data->head.time - this->transformMsg.header.stamp.sec) );
+ this->transformMsg.header.stamp.sec = (unsigned long)floor(cur_time);
+ this->transformMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( cur_time - this->transformMsg.header.stamp.sec) );
this->transformMsg.transform.translation.x = pos.x;
this->transformMsg.transform.translation.y = pos.y;
@@ -167,7 +158,6 @@
rosnode->publish(this->topicName,this->transformMsg);
this->lock.unlock();
- this->myIface->Unlock();
}
////////////////////////////////////////////////////////////////////////////////
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Block_Laser.cc
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Block_Laser.cc 2008-09-29 23:27:11 UTC (rev 4794)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Block_Laser.cc 2008-09-30 00:17:02 UTC (rev 4795)
@@ -57,9 +57,6 @@
// set parent sensor to active automatically
this->myParent->SetActive(true);
- this->laserIface = NULL;
- this->fiducialIface = NULL;
-
rosnode = ros::g_node; // comes from where? common.h exports as global variable
int argc = 0;
char** argv = NULL;
@@ -82,18 +79,6 @@
// Load the controller
void Ros_Block_Laser::LoadChild(XMLConfigNode *node)
{
- std::vector<Iface*>::iterator iter;
-
- for (iter = this->ifaces.begin(); iter != this->ifaces.end(); iter++)
- {
- if ((*iter)->GetType() == "laser")
- this->laserIface = dynamic_cast<LaserIface*>(*iter);
- else if ((*iter)->GetType() == "fiducial")
- this->fiducialIface = dynamic_cast<FiducialIface*>(*iter);
- }
-
- if (!this->laserIface) gzthrow("Ros_Block_Laser controller requires a LaserIface");
-
this->topicName = node->GetString("topicName","default_ros_laser",0); //read from xml file
std::cout << "================= " << this->topicName << std::endl;
rosnode->advertise<std_msgs::PointCloudFloat32>(this->topicName,10);
@@ -128,40 +113,7 @@
// Update the controller
void Ros_Block_Laser::UpdateChild()
{
-#if 0
- bool laserOpened = false;
- bool fidOpened = false;
-
- if (this->laserIface->Lock(1))
- {
- laserOpened = this->laserIface->GetOpenCount() > 0;
- this->laserIface->Unlock();
- }
-
- if (this->fiducialIface && this->fiducialIface->Lock(1))
- {
- fidOpened = this->fiducialIface->GetOpenCount() > 0;
- this->fiducialIface->Unlock();
- }
-
- if (laserOpened)
- {
- this->myParent->SetActive(true);
this->PutLaserData();
- }
-
- if (fidOpened)
- {
- this->myParent->SetActive(true);
- this->PutFiducialData();
- }
-
- if (!laserOpened && !fidOpened)
- {
- this->myParent->SetActive(false);
- }
-#endif
- this->PutLaserData();
}
////////////////////////////////////////////////////////////////////////////////
@@ -206,228 +158,103 @@
for (int i=0; i< r_size; i++)
this->cloudMsg.chan[i].set_vals_size(1);
+ /***************************************************************/
+ /* */
+ /* point scan from laser */
+ /* */
+ /***************************************************************/
+ this->lock.lock();
+ // Add Frame Name
+ this->cloudMsg.header.frame_id = this->frameName;
+ this->cloudMsg.header.stamp.sec = (unsigned long)floor(Simulator::Instance()->GetSimTime());
+ this->cloudMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( Simulator::Instance()->GetSimTime() - this->cloudMsg.header.stamp.sec) );
- if (this->laserIface->Lock(1))
+ for (j = 0; j<verticalRangeCount; j++)
{
- // Add Frame Name
+ // interpolating in vertical direction
+ vb = (verticalRangeCount == 1) ? 0 : (double) j * (verticalRayCount - 1) / (verticalRangeCount - 1);
+ vja = (int) floor(vb);
+ vjb = std::min(vja + 1, verticalRayCount - 1);
+ vb = vb - floor(vb); // fraction from min
+ assert(vja >= 0 && vja < verticalRayCount);
+ assert(vjb >= 0 && vjb < verticalRayCount);
- // Data timestamp
- this->laserIface->data->head.time = Simulator::Instance()->GetSimTime();
-
- // Read out the laser range data
- this->laserIface->data->min_angle = minAngle.GetAsRadian();
- this->laserIface->data->max_angle = maxAngle.GetAsRadian();
- this->laserIface->data->res_angle = (maxAngle.GetAsRadian() - minAngle.GetAsRadian()) / (rangeCount - 1);
- this->laserIface->data->res_range = 0.1;
- this->laserIface->data->max_range = maxRange;
- this->laserIface->data->range_count = rangeCount;
-
- assert(this->laserIface->data->range_count < GZ_LASER_MAX_RANGES );
-
- /***************************************************************/
- /* */
- /* point scan from laser */
- /* */
- /***************************************************************/
- this->lock.lock();
- this->cloudMsg.header.frame_id = this->frameName;
- this->cloudMsg.header.stamp.sec = (unsigned long)floor(this->laserIface->data->head.time);
- this->cloudMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->laserIface->data->head.time - this->cloudMsg.header.stamp.sec) );
-
- for (j = 0; j<verticalRangeCount; j++)
+ for (i = 0; i<rangeCount; i++)
{
- // interpolating in vertical direction
- vb = (verticalRangeCount == 1) ? 0 : (double) j * (verticalRayCount - 1) / (verticalRangeCount - 1);
- vja = (int) floor(vb);
- vjb = std::min(vja + 1, verticalRayCount - 1);
- vb = vb - floor(vb); // fraction from min
+ // Interpolate the range readings from the rays in horizontal direction
+ hb = (rangeCount == 1)? 0 : (double) i * (rayCount - 1) / (rangeCount - 1);
+ hja = (int) floor(hb);
+ hjb = std::min(hja + 1, rayCount - 1);
+ hb = hb - floor(hb); // fraction from min
- assert(vja >= 0 && vja < verticalRayCount);
- assert(vjb >= 0 && vjb < verticalRayCount);
+ assert(hja >= 0 && hja < rayCount);
+ assert(hjb >= 0 && hjb < rayCount);
- for (i = 0; i<rangeCount; i++)
- {
- // Interpolate the range readings from the rays in horizontal direction
- hb = (rangeCount == 1)? 0 : (double) i * (rayCount - 1) / (rangeCount - 1);
- hja = (int) floor(hb);
- hjb = std::min(hja + 1, rayCount - 1);
- hb = hb - floor(hb); // fraction from min
+ // indices of 4 corners
+ j1 = hja + vja * rayCount;
+ j2 = hjb + vja * rayCount;
+ j3 = hja + vjb * rayCount;
+ j4 = hjb + vjb * rayCount;
+ // range readings of 4 corners
+ r1 = std::min(this->myParent->GetRange(j1) , maxRange);
+ r2 = std::min(this->myParent->GetRange(j2) , maxRange);
+ r3 = std::min(this->myParent->GetRange(j3) , maxRange);
+ r4 = std::min(this->myParent->GetRange(j4) , maxRange);
- assert(hja >= 0 && hja < rayCount);
- assert(hjb >= 0 && hjb < rayCount);
+ // Range is linear interpolation if values are close,
+ // and min if they are very different
+ r = (1-vb)*((1 - hb) * r1 + hb * r2)
+ + vb *((1 - hb) * r3 + hb * r4);
- // indices of 4 corners
- j1 = hja + vja * rayCount;
- j2 = hjb + vja * rayCount;
- j3 = hja + vjb * rayCount;
- j4 = hjb + vjb * rayCount;
- // range readings of 4 corners
- r1 = std::min(this->myParent->GetRange(j1) , maxRange);
- r2 = std::min(this->myParent->GetRange(j2) , maxRange);
- r3 = std::min(this->myParent->GetRange(j3) , maxRange);
- r4 = std::min(this->myParent->GetRange(j4) , maxRange);
+ // Intensity is either-or
+ v = (int) this->myParent->GetRetro(j1) || (int) this->myParent->GetRetro(j2) ||
+ (int) this->myParent->GetRetro(j3) || (int) this->myParent->GetRetro(j4);
- // Range is linear interpolation if values are close,
- // and min if they are very different
- r = (1-vb)*((1 - hb) * r1 + hb * r2)
- + vb *((1 - hb) * r3 + hb * r4);
+ // std::cout << " block debug "
+ // << " ij("<<i<<","<<j<<")"
+ // << " j1234("<<j1<<","<<j2<<","<<j3<<","<<j4<<")"
+ // << " r1234("<<r1<<","<<r2<<","<<r3<<","<<r4<<")"
+ // << std::endl;
- // Intensity is either-or
- v = (int) this->myParent->GetRetro(j1) || (int) this->myParent->GetRetro(j2) ||
- (int) this->myParent->GetRetro(j3) || (int) this->myParent->GetRetro(j4);
+ // get angles of ray to get xyz for point
+ double yAngle = 0.5*(hja+hjb) * yDiff / (rayCount -1) + minAngle.GetAsRadian();
+ double pAngle = 0.5*(vja+vjb) * pDiff / (verticalRayCount -1) + verticalMinAngle.GetAsRadian();
- // std::cout << " block debug "
- // << " ij("<<i<<","<<j<<")"
- // << " j1234("<<j1<<","<<j2<<","<<j3<<","<<j4<<")"
- // << " r1234("<<r1<<","<<r2<<","<<r3<<","<<r4<<")"
- // << std::endl;
-
- this->laserIface->data->ranges[i] = r + minRange;
- this->laserIface->data->intensity[i] = v;
-
- // get angles of ray to get xyz for point
- double yAngle = 0.5*(hja+hjb) * yDiff / (rayCount -1) + minAngle.GetAsRadian();
- double pAngle = 0.5*(vja+vjb) * pDiff / (verticalRayCount -1) + verticalMinAngle.GetAsRadian();
-
- int n = i + j*rayCount;
- /***************************************************************/
- /* */
- /* point scan from laser */
- /* */
- /***************************************************************/
- if (r == maxRange)
- {
- // no noise if at max range
- this->cloudMsg.pts[n].x = (r+minRange) * cos(pAngle)*cos(yAngle);
- this->cloudMsg.pts[n].y = (r+minRange) * sin(yAngle);
- this->cloudMsg.pts[n].z = (r+minRange) * sin(pAngle)*cos(yAngle);
- }
- else
- {
- this->cloudMsg.pts[n].x = (r+minRange) * cos(pAngle)*cos(yAngle) + this->GaussianKernel(0,this->gaussianNoise) ;
- this->cloudMsg.pts[n].y = (r+minRange) * sin(yAngle) + this->GaussianKernel(0,this->gaussianNoise) ;
- this->cloudMsg.pts[n].z = (r+minRange) * sin(pAngle)*cos(yAngle) + this->GaussianKernel(0,this->gaussianNoise) ;
- }
- this->cloudMsg.chan[n].vals[0] = v + this->GaussianKernel(0,this->gaussianNoise) ;
- }
- }
-
- // iface writing can be skipped if iface is not used.
- // send data out via ros message
- rosnode->publish(this->topicName,this->cloudMsg);
- this->lock.unlock();
-
- this->laserIface->Unlock();
-
- // New data is available
- this->laserIface->Post();
- }
-
-
-}
-
-//////////////////////////////////////////////////////////////////////////////
-// Update the data in the interface
-void Ros_Block_Laser::PutFiducialData()
-{
- int i, j, count;
- FiducialFid *fid;
- double r, b;
- double ax, ay, bx, by, cx, cy;
-
- Angle maxAngle = this->myParent->GetMaxAngle();
- Angle minAngle = this->myParent->GetMinAngle();
-
- double maxRange = this->myParent->GetMaxRange();
- double minRange = this->myParent->GetMinRange();
- int rayCount = this->myParent->GetRayCount();
- int rangeCount = this->myParent->GetRangeCount();
-
- if (this->fiducialIface->Lock(1))
- {
- // Data timestamp
- this->fiducialIface->data->head.time = Simulator::Instance()->GetSimTime();
- this->fiducialIface->data->count = 0;
-
- // TODO: clean this up
- count = 0;
- for (i = 0; i < rayCount; i++)
- {
- if (this->myParent->GetFiducial(i) < 0)
- continue;
-
- // Find the end of the fiducial
- for (j = i + 1; j < rayCount; j++)
+ int n = i + j*rayCount;
+ /***************************************************************/
+ /* */
+ /* point scan from laser */
+ /* */
+ /***************************************************************/
+ if (r == maxRange)
{
- if (this->myParent->GetFiducial(j) != this->myParent->GetFiducial(i))
- break;
+ // no noise if at max range
+ this->cloudMsg.pts[n].x = (r+minRange) * cos(pAngle)*cos(yAngle);
+ this->cloudMsg.pts[n].y = (r+minRange) * sin(yAngle);
+ this->cloudMsg.pts[n].z = (r+minRange) * sin(pAngle)*cos(yAngle);
}
- j--;
-
- // Need at least three points to get orientation
- if (j - i + 1 >= 3)
- {
- r = minRange + this->myParent->GetRange(i);
- b = minAngle.GetAsRadian() + i * ((maxAngle.GetAsRadian()-minAngle.GetAsRadian()) / (rayCount - 1));
- ax = r * cos(b);
- ay = r * sin(b);
-
- r = minRange + this->myParent->GetRange(j);
- b = minAngle.GetAsRadian() + j * ((maxAngle.GetAsRadian()-minAngle.GetAsRadian()) / (rayCount - 1));
- bx = r * cos(b);
- by = r * sin(b);
-
- cx = (ax + bx) / 2;
- cy = (ay + by) / 2;
-
- assert(count < GZ_FIDUCIAL_MAX_FIDS);
- fid = this->fiducialIface->data->fids + count++;
-
- fid->id = this->myParent->GetFiducial(j);
- fid->pose.pos.x = cx;
- fid->pose.pos.y = cy;
- fid->pose.yaw = atan2(by - ay, bx - ax) + M_PI / 2;
- }
-
- // Fewer points get no orientation
else
{
- r = minRange + this->myParent->GetRange(i);
- b = minAngle.GetAsRadian() + i * ((maxAngle.GetAsRadian()-minAngle.GetAsRadian()) / (rayCount - 1));
- ax = r * cos(b);
- ay = r * sin(b);
-
- r = minRange + this->myParent->GetRange(j);
- b = minAngle.GetAsRadian() + j * ((maxAngle.GetAsRadian()-minAngle.GetAsRadian()) / (rayCount - 1));
- bx = r * cos(b);
- by = r * sin(b);
-
- cx = (ax + bx) / 2;
- cy = (ay + by) / 2;
-
- assert(count < GZ_FIDUCIAL_MAX_FIDS);
- fid = this->fiducialIface->data->fids + count++;
-
- fid->id = this->myParent->GetFiducial(j);
- ...
[truncated message content] |
|
From: <stu...@us...> - 2008-09-30 18:28:06
|
Revision: 4814
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4814&view=rev
Author: stuglaser
Date: 2008-09-30 18:27:27 +0000 (Tue, 30 Sep 2008)
Log Message:
-----------
CartesianPositionController operates in vector space now.
Modified Paths:
--------------
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_effort_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_position_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml
pkg/trunk/controllers/robot_mechanism_controllers/scripts/control.py
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_effort_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_position_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/robot_mechanism_controllers/controllers.py
pkg/trunk/mechanism/mechanism_control/scripts/mech.py
Added Paths:
-----------
pkg/trunk/controllers/robot_mechanism_controllers/srv/GetVector.srv
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_effort_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_effort_controller.h 2008-09-30 18:13:14 UTC (rev 4813)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_effort_controller.h 2008-09-30 18:27:27 UTC (rev 4814)
@@ -31,7 +31,7 @@
* Example config:
<controller type="CartesianEffortController" name="controller_name">
- <chain root="root_link" tip="tip_link" />
+ <chain root="root_link" tip="tip_link" offset="0.3 0.1 0.2" />
</controller>
* The root is fixed, and all commands are specified in its coordinate
@@ -62,6 +62,7 @@
btVector3 command_;
+ btVector3 offset_;
std::vector<mechanism::LinkState*> links_; // root to tip
std::vector<mechanism::JointState*> joints_; // root to tip, 1 element smaller than links_
};
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_position_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_position_controller.h 2008-09-30 18:13:14 UTC (rev 4813)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_position_controller.h 2008-09-30 18:27:27 UTC (rev 4814)
@@ -47,6 +47,7 @@
#include <vector>
#include "ros/node.h"
#include "robot_mechanism_controllers/SetVectorCommand.h"
+#include "robot_mechanism_controllers/GetVector.h"
#include "robot_mechanism_controllers/cartesian_effort_controller.h"
#include "control_toolbox/pid.h"
#include "mechanism_model/controller.h"
@@ -65,12 +66,13 @@
void update();
btVector3 command_;
+ void getTipPosition(btVector3 *p);
private:
mechanism::RobotState *robot_;
mechanism::LinkState *tip_;
CartesianEffortController effort_;
- control_toolbox::Pid pid_;
+ control_toolbox::Pid pid_x_, pid_y_, pid_z_;
double last_time_;
bool reset_;
@@ -87,10 +89,12 @@
bool setCommand(robot_mechanism_controllers::SetVectorCommand::request &req,
robot_mechanism_controllers::SetVectorCommand::response &resp);
+ bool getActual(robot_mechanism_controllers::GetVector::request &req,
+ robot_mechanism_controllers::GetVector::response &resp);
private:
CartesianPositionController c_;
- AdvertisedServiceGuard guard_set_actual_;
+ AdvertisedServiceGuard guard_set_command_, guard_get_actual_;
};
}
Modified: pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml 2008-09-30 18:13:14 UTC (rev 4813)
+++ pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml 2008-09-30 18:27:27 UTC (rev 4814)
@@ -1,7 +1,7 @@
<package>
<description brief="Generic Mechanism Controller Library">
</description>
- <author>John Hsu, David Li, Melonee Wise</author>
+ <author>John Hsu, David Li, Melonee Wise, Stuart Glaser</author>
<license>BSD</license>
<depend package="mechanism_model" />
<depend package="control_toolbox" />
@@ -10,6 +10,8 @@
<depend package="misc_utils" />
<depend package="roscpp" />
<depend package="bullet" />
+ <depend package="std_msgs" />
+ <depend package="tf" />
<url>http://pr.willowgarage.com</url>
<repository>http://pr.willowgarage.com/repos</repository>
<export>
Modified: pkg/trunk/controllers/robot_mechanism_controllers/scripts/control.py
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/scripts/control.py 2008-09-30 18:13:14 UTC (rev 4813)
+++ pkg/trunk/controllers/robot_mechanism_controllers/scripts/control.py 2008-09-30 18:27:27 UTC (rev 4814)
@@ -16,6 +16,7 @@
set <controller> <command> - Set the controller's commanded value
setv <controller> <x> <y> <z> - Set the controller's command as a vector
get <controller> - Get the controller's commanded value
+ getv <controller> - Get the controller's vector value
profile <controller> <upper turnaround offset> <lower turnaround offset> <upper decel buffer> <lower decel bufer>
- Define how far away from joint limit to turn around. Buffers indicate how far from that point to start decelerating. Set to 0 to disable'''
sys.exit(exit_code)
@@ -37,6 +38,10 @@
if len(sys.argv) < 3:
print_usage()
controllers.get_controller(sys.argv[2])
+ elif sys.argv[1] == 'getv':
+ if len(sys.argv) < 3:
+ print_usage()
+ controllers.get_controller_vector(sys.argv[2])
elif sys.argv[1] == 'profile':
controllers.set_profile(sys.argv[2],float(sys.argv[3]),float(sys.argv[4]), float(sys.argv[5]), float(sys.argv[6]))
elif sys.argv[1] == 'setPosition':
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_effort_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_effort_controller.cpp 2008-09-30 18:13:14 UTC (rev 4813)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_effort_controller.cpp 2008-09-30 18:27:27 UTC (rev 4814)
@@ -32,6 +32,7 @@
*/
#include "robot_mechanism_controllers/cartesian_effort_controller.h"
+#include "urdf/parser.h"
#include <algorithm>
namespace controller {
@@ -40,6 +41,7 @@
CartesianEffortController::CartesianEffortController()
: command_(0,0,0),
+ offset_(0,0,0),
links_(0,(mechanism::LinkState*)NULL),
joints_(0,(mechanism::JointState*)NULL)
{
@@ -111,6 +113,17 @@
assert(joints_.size() == links_.size() - 1);
+ if (chain->Attribute("offset"))
+ {
+ std::vector<double> offset_pieces;
+ urdf::queryVectorAttribute(chain, "offset", &offset_pieces);
+ assert(offset_pieces.size() == 3); // TODO
+
+ offset_[0] = offset_pieces[0];
+ offset_[1] = offset_pieces[1];
+ offset_[2] = offset_pieces[2];
+ }
+
return true;
}
@@ -129,8 +142,7 @@
F.setValue(tempF.x, tempF.y, tempF.z);
// At this point, F is the desired force in the current link's frame
- // TODO: actual tip offset, not a made-up offset
- btVector3 r(0.1,0,0); // position of the force in the current frame
+ btVector3 r(offset_); // position of the force in the current frame
for (int i = links_.size() - 2; i >= 0; --i)
{
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_position_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_position_controller.cpp 2008-09-30 18:13:14 UTC (rev 4813)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_position_controller.cpp 2008-09-30 18:27:27 UTC (rev 4814)
@@ -33,6 +33,7 @@
#include "robot_mechanism_controllers/cartesian_position_controller.h"
#include <algorithm>
+#include "tf/transform_datatypes.h"
namespace controller {
@@ -63,8 +64,10 @@
fprintf(stderr, "Error: CartesianPositionController requires a pid element\n");
return false;
}
- if (!pid_.initXml(pid_el))
+ if (!pid_x_.initXml(pid_el))
return false;
+ pid_y_ = pid_x_;
+ pid_z_ = pid_x_;
last_time_ = robot_->hw_->current_time_;
@@ -75,24 +78,29 @@
{
if (reset_) {
reset_ = false;
- command_ = tip_->abs_position_;
+ command_ = tip_->abs_position_ + effort_.offset_;
}
assert(tip_);
double time = robot_->hw_->current_time_;
+ btVector3 error = command_ - (tip_->abs_position_ + effort_.offset_);
+ effort_.command_[0] = -pid_x_.updatePid(error.x(), time - last_time_);
+ effort_.command_[1] = -pid_y_.updatePid(error.y(), time - last_time_);
+ effort_.command_[2] = -pid_z_.updatePid(error.z(), time - last_time_);
- btVector3 v = command_ - tip_->abs_position_;
- double error = v.length();
- if (error > 0.0)
- v.normalize();
-
- effort_.command_ = v * -pid_.updatePid(error, time - last_time_);
+ printf(" %.4lf %.4lf %.4lf\n", effort_.command_[0],effort_.command_[1],effort_.command_[2]);
effort_.update();
last_time_ = time;
}
+void CartesianPositionController::getTipPosition(btVector3 *p)
+{
+ *p = tip_->abs_position_ + effort_.offset_;
+}
+
+
ROS_REGISTER_CONTROLLER(CartesianPositionControllerNode)
CartesianPositionControllerNode::CartesianPositionControllerNode()
@@ -119,7 +127,10 @@
node->advertise_service(topic + "/set_command",
&CartesianPositionControllerNode::setCommand, this);
- guard_set_actual_.set(topic + "/set_command");
+ guard_set_command_.set(topic + "/set_command");
+ node->advertise_service(topic + "/get_actual",
+ &CartesianPositionControllerNode::getActual, this);
+ guard_get_actual_.set(topic + "/get_actual");
return true;
}
@@ -136,5 +147,14 @@
return true;
}
+bool CartesianPositionControllerNode::getActual(
+ robot_mechanism_controllers::GetVector::request &req,
+ robot_mechanism_controllers::GetVector::response &resp)
+{
+ btVector3 v;
+ c_.getTipPosition(&v);
+ tf::Vector3TFToMsg(v, resp.v);
+ return true;
+}
}
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/robot_mechanism_controllers/controllers.py
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/robot_mechanism_controllers/controllers.py 2008-09-30 18:13:14 UTC (rev 4813)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/robot_mechanism_controllers/controllers.py 2008-09-30 18:27:27 UTC (rev 4814)
@@ -33,6 +33,12 @@
resp = s.call(GetActualRequest())
print str(resp.time) + ": " + str(resp.command)
+def get_controller_vector(controller):
+ rospy.wait_for_service(controller + '/get_actual')
+ s = rospy.ServiceProxy(controller + '/get_actual', GetVector)
+ resp = s()
+ print "(%f, %f, %f)" % (resp.v.x, resp.v.y, resp.v.z)
+
def set_position(controller, command):
rospy.wait_for_service(controller + '/set_position')
s = rospy.ServiceProxy(controller + '/set_position', SetPosition)
Added: pkg/trunk/controllers/robot_mechanism_controllers/srv/GetVector.srv
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/srv/GetVector.srv (rev 0)
+++ pkg/trunk/controllers/robot_mechanism_controllers/srv/GetVector.srv 2008-09-30 18:27:27 UTC (rev 4814)
@@ -0,0 +1,2 @@
+---
+std_msgs/Vector3 v
\ No newline at end of file
Modified: pkg/trunk/mechanism/mechanism_control/scripts/mech.py
===================================================================
--- pkg/trunk/mechanism/mechanism_control/scripts/mech.py 2008-09-30 18:13:14 UTC (rev 4813)
+++ pkg/trunk/mechanism/mechanism_control/scripts/mech.py 2008-09-30 18:27:27 UTC (rev 4814)
@@ -37,3 +37,5 @@
mechanism.kill_controller(sys.argv[2])
elif sys.argv[1] == 'shutdown':
mechanism.shutdown()
+ else:
+ print_usage(1)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2008-09-30 18:38:05
|
Revision: 4816
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4816&view=rev
Author: gerkey
Date: 2008-09-30 18:37:48 +0000 (Tue, 30 Sep 2008)
Log Message:
-----------
moved overhead_grasp_behavior to deprecated
Added Paths:
-----------
pkg/trunk/deprecated/overhead_grasp_behavior/
pkg/trunk/deprecated/overhead_grasp_behavior/rospack_nosubdirs
Removed Paths:
-------------
pkg/trunk/manip/overhead_grasp_behavior/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2008-09-30 18:41:08
|
Revision: 4818
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4818&view=rev
Author: gerkey
Date: 2008-09-30 18:40:51 +0000 (Tue, 30 Sep 2008)
Log Message:
-----------
moved pr2_kinematic_controllers to deprecated
Added Paths:
-----------
pkg/trunk/deprecated/pr2_kinematic_controllers/
pkg/trunk/deprecated/pr2_kinematic_controllers/rospack_nosubdirs
Removed Paths:
-------------
pkg/trunk/manip/pr2_kinematic_controllers/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-09-30 19:18:34
|
Revision: 4823
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4823&view=rev
Author: hsujohnhsu
Date: 2008-09-30 19:18:07 +0000 (Tue, 30 Sep 2008)
Log Message:
-----------
* added more comments to pr2_gazebo_actuators.xml
* docum. for gazebo_plugin
* update MoveCartesian.srv according to change in pr2_msgs
Modified Paths:
--------------
pkg/trunk/drivers/simulator/gazebo_plugin/doc.dox
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_PTZ.hh
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Stereo_Camera.hh
pkg/trunk/drivers/simulator/gazebo_plugin/srv/MoveCartesian.srv
pkg/trunk/robot_descriptions/gazebo_robot_description/pr2_gazebo_actuators.xml
Added Paths:
-----------
pkg/trunk/drivers/simulator/gazebo_plugin/gazebo_mcn.jpg
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/doc.dox
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/doc.dox 2008-09-30 18:54:44 UTC (rev 4822)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/doc.dox 2008-09-30 19:18:07 UTC (rev 4823)
@@ -1,5 +1,6 @@
/**
@mainpage
+
@htmlinclude manifest.html
Here are the gazebo plugins which are currently implemented:
@@ -11,6 +12,9 @@
\li \ref gazebo::Ros_PTZ "ROS PTZ Camera Plugin"
\li \ref gazebo::P3D "ROS Ground Truth Broadcaster"
+
+ @image html gazebo_mcn.jpg "Gazebo Mechanism Control Model"
+
@section usage Quick Start Guide
\li If you have successfully \e rosmake \e 2dnav-gazebo, you have EVERYTHING needed to run all simulator+PR2 demos. Otherwise, to compile a minimum set of packages for running Gazebo:
@verbatim
@@ -22,27 +26,35 @@
\li Here is a simple launch script that starts Gazebo:
@verbatim
<launch>
+ <!-- automatically start a new master if none active -->
<master auto="start">
+ <!-- if needed, group tag allows pushing components into namespace via attribute ns="namespace" -->
<group name="wg">
+ <!-- assign environment variables for gazebo and startup gazebo with argument containing the world file. -->
<node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/world/robot.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
<env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
<env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
<env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
</node>
- </group>
+ </group>
</launch>
@endverbatim
see <a href="http://pr.willowgarage.com/wiki/roslaunch">roslaunch</a> for more details.
- To start a mechanism::MechanismControlNode and spawn some controllers, you have to first send \b pr2.xml to the parameter server, then invoke \b mech.py with the controllers xml configuration file as the argument to spawn controllers:
@verbatim
+ <!-- send pr2.xml to parameter server as a string, allow retrieval by various components whe needs it
+ (Mechanism Control, BaseControllerNode, etc...) -->
<param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/pr2/pr2.xml"" />
+ <!-- use mech.py to spawn all controllers listed in controllers.xml -->
<node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/pr2/controllers.xml" respawn="false" output="screen" />
@endverbatim
- The following \b control.py node sends a \e set_profile command of 46 to the \b tilt_laser_controller:
@verbatim
+ <!-- start tilting Hokuyo laser by sending it a preset code of 46, this means sawtooth profile sweep.
+ for details of the profile, rates, see controller::LaserScannerControllerNode. -->
<node pkg="robot_mechanism_controllers" type="control.py" args="set tilt_laser_controller 46" respawn="false" output="screen" />
@endverbatim
Property changes on: pkg/trunk/drivers/simulator/gazebo_plugin/gazebo_mcn.jpg
___________________________________________________________________
Added: svn:mime-type
+ application/octet-stream
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_PTZ.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_PTZ.hh 2008-09-30 18:54:44 UTC (rev 4822)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_PTZ.hh 2008-09-30 19:18:07 UTC (rev 4823)
@@ -87,7 +87,7 @@
protected: virtual void LoadChild(XMLConfigNode *node);
/// \brief Save the controller.
- /// \stream Output stream
+ /// stream Output stream
protected: void SaveChild(std::string &prefix, std::ostream &stream);
/// \brief Init the controller
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Stereo_Camera.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Stereo_Camera.hh 2008-09-30 18:54:44 UTC (rev 4822)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Stereo_Camera.hh 2008-09-30 19:18:07 UTC (rev 4823)
@@ -109,7 +109,7 @@
protected: virtual void LoadChild(XMLConfigNode *node);
/// \brief Save the controller.
- /// \stream Output stream
+ /// stream Output stream
protected: void SaveChild(std::string &prefix, std::ostream &stream);
/// \brief Init the controller
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/srv/MoveCartesian.srv
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/srv/MoveCartesian.srv 2008-09-30 18:54:44 UTC (rev 4822)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/srv/MoveCartesian.srv 2008-09-30 19:18:07 UTC (rev 4823)
@@ -1,3 +1,3 @@
-pr2_msgs/EndEffectorState e
+pr2_msgs/MoveEndEffectorState e
---
int32 reachable
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/pr2_gazebo_actuators.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/pr2_gazebo_actuators.xml 2008-09-30 18:54:44 UTC (rev 4822)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/pr2_gazebo_actuators.xml 2008-09-30 19:18:07 UTC (rev 4823)
@@ -1,13 +1,22 @@
<launch>
+ <!-- start a new master if none active -->
+ <master auto="start">
+ <!-- if needed, group tag allows pushing components into namespace via ns="namespace" -->
<group name="wg">
+ <!-- send pr2.xml to parameter server as a string, allow retrieval by various components whe needs it
+ (Mechanism Control, BaseControllerNode, etc...) -->
<param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/pr2/pr2.xml"" />
+ <!-- assign environment variables for gazebo and startup gazebo with argument containing the world file. -->
<node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/world/robot.world" respawn="false" output="screen">
<env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
<env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
<env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
<env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
</node>
+ <!-- use mech.py to spawn all controllers listed in controllers.xml -->
<node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/pr2/controllers.xml" respawn="false" output="screen" />
+ <!-- start tilting Hokuyo laser by sending it a preset code of 46, this means sawtooth profile sweep.
+ for details of the profile, rates, see controller::LaserScannerControllerNode. -->
<node pkg="robot_mechanism_controllers" type="control.py" args="set tilt_laser_controller 46" respawn="false" output="screen" />
</group>
</launch>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-09-30 21:57:49
|
Revision: 4845
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4845&view=rev
Author: hsujohnhsu
Date: 2008-09-30 21:57:26 +0000 (Tue, 30 Sep 2008)
Log Message:
-----------
* updates to single link robot, simpler and cleaner.
* updates to documentation.
Modified Paths:
--------------
pkg/trunk/drivers/simulator/gazebo_plugin/doc.dox
pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/actuators_single_link.xml
pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/controllers_single_link.xml
pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/gazebo/controllers_gazebo_single_link.xml
pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/gazebo/gazebo_joints_single_link.xml
pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/groups_single_link.xml
pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/pr2_single_link.xml
pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/transmissions_single_link.xml
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/doc.dox
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/doc.dox 2008-09-30 21:47:24 UTC (rev 4844)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/doc.dox 2008-09-30 21:57:26 UTC (rev 4845)
@@ -73,18 +73,204 @@
$ gazebo `rospack find gazebo_robot_description`/world/robot.world
@endverbatim
-@section parameters PR2 Robot Descriptions
-Robot descriptions are explained in the <a href="http://pr.willowgarage.com/wiki/RobotDescriptionFormat">Robot Description Format</a> page.
-You can find the PR2 XML files in \b ros-pkg/robot_descriptions/wg_robot_description/pr2
+@section parameters Robot Descriptions
+\li PR2 Robot Description
+ Robot descriptions are explained in the <a href="http://pr.willowgarage.com/wiki/RobotDescriptionFormat">Robot Description Format</a> page.
+ You can find the PR2 XML files in \b ros-pkg/robot_descriptions/wg_robot_description/pr2
-To convert \b pr2.xml into Gazebo recognizable format (\b ros-pkg/robot_descriptions/gazebo_robot_description) run the following commands:
-@verbatim
-$ roscd gazebo_robot_description
-$ rosmake
-@endverbatim
-Above generates \b ros-pkg/robot_descriptions/gazebo_robot_description/world/pr2_xml.model, which is included by the world file: \b ros-pkg/robot_descriptions/gazebo_robot_description/world/robot.world.
+ To convert \b pr2.xml into Gazebo recognizable format (\b ros-pkg/robot_descriptions/gazebo_robot_description) run the following commands:
+ @verbatim
+ $ roscd gazebo_robot_description
+ $ rosmake
+ @endverbatim
+ Above generates \b ros-pkg/robot_descriptions/gazebo_robot_description/world/pr2_xml.model, which is included by the world file: \b ros-pkg/robot_descriptions/gazebo_robot_description/world/robot.world.
+\li Another Example: Simple Robot (Not really a robot, single link only)
+ Here is an example demonstrating a simple mechanism control / controller stack for a single DOF.
+ - \b pr2_single_link.xml
+ @verbatim
+ <?xml version="1.0"?>
+ <robot name="pr2"><!-- name of the robot-->
+
+ <!-- joint blocks -->
+ <joint name="single_link_joint" type="revolute" >
+ <axis xyz="1 0 0" />
+ <anchor xyz="0 0 0" />
+ <limit min="-M_PI" max="M_PI" effort="100" velocity="5" />
+ <calibration values="1.5 -1 " />
+ </joint>
+
+ <joint name="base_block_joint" type="planar">
+ </joint>
+
+ <!-- link blocks -->
+ <link name="base_block">
+ <parent name="world" />
+ <origin xyz="0 0 0.002 " rpy="0 0 0" />
+ <joint name="base_block_joint" />
+ <inertial>
+ <mass value="1000" />
+ <com xyz="0 0 0" />
+ <inertia ixx="1000" ixy="0" ixz="0" iyy="1000" iyz="0" izz="1000" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <map name="gazebo_material" flag="gazebo">
+ <elem key="material">Gazebo/GrassFloor</elem>
+ </map>
+ <geometry name="base_block_visual_geom">
+ <mesh scale="20 20 0.1" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0.0 0.0 0.0" />
+ <geometry name="base_block_collision_geom">
+ <box size="20 20 0.1" />
+ </geometry>
+ </collision>
+ </link>
+
+ <link name="single_link">
+ <parent name="base_block" />
+ <origin xyz="0 0 1" rpy="0 0 0" />
+ <joint name="single_link_joint" />
+ <inertial >
+ <mass value="10" />
+ <com xyz="0 0 0" />
+ <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1" />
+ </inertial>
+ <visual >
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <map name="gazebo_material" flag="gazebo">
+ <elem key="material">Gazebo/Red</elem>
+ </map>
+ <geometry name="sholder_roll_mesh_file">
+ <mesh scale="1.0 0.1 0.1" />
+ </geometry>
+ </visual>
+ <collision >
+ <origin xyz="0.5 0 0" rpy="0.0 0.0 0.0 " />
+ <geometry name="single_link_collision_geom">
+ <box size="1.0 0.1 0.1" />
+ </geometry>
+ </collision>
+ <map name="single_link_gravity" flag="gazebo">
+ <elem key="turnGravityOff">true</elem>
+ </map>
+ </link>
+ <!-- Define groups of links; a link may be part of multiple groups -->
+ <include>groups_single_link.xml</include>
+ <!-- mechanism controls -->
+ <include>actuators_single_link.xml</include>
+ <include>transmissions_single_link.xml</include>
+
+ <!-- setup for GazeboActuators plugin -->
+ <map name="transmissions_gazebo_actuators" flag="gazebo"> <!-- we can set a name too, but the convertor only cares about the flag -->
+ <verbatim key="transmissions_gazebo_actuators" includes="true"> <!-- The key attribute is needed noly if multiple <xml> tags are in the same <map> tag -->
+ <!-- PR2_ACTARRAY -->
+ <controller:gazebo_actuators name="gazebo_actuators" plugin="libgazebo_actuators.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>100.0</updateRate>
+ <robot filename="pr2_single_link.xml" />
+ <gazebo_physics filename="gazebo_joints_single_link.xml" /> <!-- for simulator/physics specific settigs -->
+ <interface:audio name="gazebo_actuators_dummy_iface" />
+ </controller:gazebo_actuators>
+ </verbatim>
+ </map>
+ <!-- setup for a ground truth widget, returns pose/rate information over ROS via P3D plugin -->
+ <map name="controllers" flag="gazebo"> <!-- we can set a name too, but the convertor only cares about the flag -->
+ <verbatim key="controllers"> <!-- The key attribute is needed noly if multiple <xml> tags are in the same <map> tag -->
+
+ <!-- P3D for position groundtruth -->
+ <controller:P3D name="p3d_single_link_controller" plugin="libP3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>100.0</updateRate>
+ <bodyName>single_link</bodyName>
+ <topicName>single_link_pose</topicName>
+ <frameName>single_link_frame</frameName>
+ <interface:position name="p3d_single_link_position"/>
+ </controller:P3D>
+
+ </verbatim>
+ </map>
+ </robot>
+ @endverbatim
+ - where \b gazebo_joints_single_link.xml specifies damping of the joint
+ @verbatim
+ <robot name="pr2">
+ <joint name="single_link_joint" >
+ <explicitDampingCoefficient>1</explicitDampingCoefficient>
+ </joint>
+ </robot>
+ @endverbatim
+ - \b groups_single_link.xml is used by kinematics library to construct a kinematic chain, though meaningless in this example.
+ @verbatim
+ <?xml version="1.0"?>
+ <robot name="pr2"><!-- name of the robot-->
+ <group name="my_group" flags="planning kinematic">
+ roll_link
+ </group>
+ </robot>
+ @endverbatim
+ - \b actuators_single_link.xml
+ @verbatim
+ <?xml version="1.0"?>
+ <robot name="pr2"><!-- name of the robot-->
+ <actuator name="single_link_motor">
+ <motorboardID>12345</motorboardID>
+ <maxCurrent>5</maxCurrent>
+ <motor>MAXON</motor>
+ <ip> 10.12.0.103 </ip>
+ <port> 0 </port>
+ <reduction>10</reduction>
+ <polymap>-1 -0.2 -0.5 </polymap>
+ </actuator>
+ </robot>
+ @endverbatim
+ - \b transmissions_single_link.xml
+ @verbatim
+ <?xml version="1.0"?>
+ <robot name="pr2"><!-- name of the robot-->
+ <transmission type="SimpleTransmission" name="single_link_trans">
+ <actuator name="single_link_motor" />
+ <joint name="single_link_joint" />
+ <mechanicalReduction>1</mechanicalReduction>
+ <motorTorqueConstant>1</motorTorqueConstant>
+ <pulsesPerRevolution>90000</pulsesPerRevolution>
+ </transmission>
+ </robot>
+ @endverbatim
+ - here's a simple controller xml to control the single link \bcontroller_single_link.xml:
+ @verbatim
+ <?xml version="1.0"?>
+ <controllers>
+ <controller name="test_controller" topic="single_link_controller" type="JointPositionControllerNode">
+ <joint name="single_link_joint" >
+ <pid p="2000" d="200" i="0.1" iClamp="1" />
+ </joint>
+ </controller>
+ </controllers>
+ @endverbatim
+ - and finally, to run it all, a roslaunch script can be found in \b ros-pkg/robot_descriptions/gazebo_robot_description/single_link.xml:
+ @verbatim
+ <launch>
+ <group name="wg">
+ <param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/single_link_test/pr2_single_link.xml"" />
+ <node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/world/robot_single_link.world" respawn="false" output="screen">
+ <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
+ <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
+ <env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ </node>
+ </group>
+ <!-- startup a single controller -->
+ <node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/single_link_test/controllers_single_link.xml" respawn="false" output="screen" />
+ <!-- set controller to .5 radians -->
+ <node pkg="robot_mechanism_controllers" type="control.py" args="set test_controller 0.5" respawn="false" output="screen" />
+ </launch>
+ @endverbatim
+
@section topic PR2 ROS topics
Various dynamically loaded plugins have been outfitted for PR2.
Below are the ROS messages actively used by PR2 simulation as described by launch script \b roslaunch \b ros-pkg/robot_descriptions/gazebo_robot_description/pr2_gazebo_actuators.xml.
@@ -325,7 +511,7 @@
@endverbatim
-\li The corresponding XML snippit required to setup the controller:
+\li The corresponding XML snippet required to setup the controller:
@verbatim
<model:physical name="my_model">
<controller:my_plugin name="my_plugin_name" plugin="libmy_plugin.so">
Modified: pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/actuators_single_link.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/actuators_single_link.xml 2008-09-30 21:47:24 UTC (rev 4844)
+++ pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/actuators_single_link.xml 2008-09-30 21:57:26 UTC (rev 4845)
@@ -1,11 +1,6 @@
<?xml version="1.0"?>
-
<robot name="pr2"><!-- name of the robot-->
-
-<!-- Actuators -->
-<!-- ========================================= -->
-<!-- Left Arm -->
- <actuator name="upperarm_roll_left_motor">
+ <actuator name="single_link_motor">
<motorboardID>12345</motorboardID>
<maxCurrent>5</maxCurrent>
<motor>MAXON</motor>
@@ -14,8 +9,6 @@
<reduction>10</reduction>
<polymap>-1 -0.2 -0.5 </polymap>
</actuator>
-<!-- ========================================= -->
-
</robot>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/controllers_single_link.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/controllers_single_link.xml 2008-09-30 21:47:24 UTC (rev 4844)
+++ pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/controllers_single_link.xml 2008-09-30 21:57:26 UTC (rev 4845)
@@ -1,8 +1,7 @@
<?xml version="1.0"?>
-
<controllers>
- <controller name="test_controller" topic="upperarm_roll_left_controller" type="JointPositionControllerNode">
- <joint name="upperarm_roll_left_joint" >
+ <controller name="test_controller" topic="single_link_controller" type="JointPositionControllerNode">
+ <joint name="single_link_joint" >
<pid p="2000" d="200" i="0.1" iClamp="1" />
</joint>
</controller>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/gazebo/controllers_gazebo_single_link.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/gazebo/controllers_gazebo_single_link.xml 2008-09-30 21:47:24 UTC (rev 4844)
+++ pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/gazebo/controllers_gazebo_single_link.xml 2008-09-30 21:57:26 UTC (rev 4845)
@@ -28,13 +28,13 @@
<verbatim key="controllers"> <!-- The key attribute is needed noly if multiple <xml> tags are in the same <map> tag -->
<!-- P3D for position groundtruth -->
- <controller:P3D name="p3d_upperarm_roll_controller" plugin="libP3D.so">
+ <controller:P3D name="p3d_single_link_controller" plugin="libP3D.so">
<alwaysOn>true</alwaysOn>
<updateRate>100.0</updateRate>
- <bodyName>upperarm_roll_left</bodyName>
- <topicName>upperarm_roll_left_pose</topicName>
- <frameName>upperarm_roll_left_frame</frameName>
- <interface:position name="p3d_upperarm_roll_position"/>
+ <bodyName>single_link</bodyName>
+ <topicName>single_link_pose</topicName>
+ <frameName>single_link_frame</frameName>
+ <interface:position name="p3d_single_link_position"/>
</controller:P3D>
</verbatim>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/gazebo/gazebo_joints_single_link.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/gazebo/gazebo_joints_single_link.xml 2008-09-30 21:47:24 UTC (rev 4844)
+++ pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/gazebo/gazebo_joints_single_link.xml 2008-09-30 21:57:26 UTC (rev 4845)
@@ -1,8 +1,5 @@
<robot name="pr2">
- <!-- ========================================= -->
- <!-- left arm array -->
- <joint name="upperarm_roll_left_joint" >
- <explicitDampingCoefficient>1</explicitDampingCoefficient>
- </joint>
-
+ <joint name="single_link_joint" >
+ <explicitDampingCoefficient>1</explicitDampingCoefficient>
+ </joint>
</robot>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/groups_single_link.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/groups_single_link.xml 2008-09-30 21:47:24 UTC (rev 4844)
+++ pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/groups_single_link.xml 2008-09-30 21:57:26 UTC (rev 4845)
@@ -1,10 +1,6 @@
<?xml version="1.0"?>
-
<robot name="pr2"><!-- name of the robot-->
-
- <group name="left_arm" flags="planning kinematic">
- upperarm_roll_left
+ <group name="my_group" flags="planning kinematic">
+ roll_link
</group>
-
-
</robot>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/pr2_single_link.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/pr2_single_link.xml 2008-09-30 21:47:24 UTC (rev 4844)
+++ pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/pr2_single_link.xml 2008-09-30 21:57:26 UTC (rev 4845)
@@ -2,137 +2,75 @@
<robot name="pr2"><!-- name of the robot-->
- <!-- Begin template definitions for ease of reuse -->
-
-
- <const name="upperarm_roll_size_x" value="0.362" /> <!-- for defining collision geometry -->
- <const name="upperarm_roll_size_y" value="0.144" /> <!-- for defining collision geometry -->
- <const name="upperarm_roll_size_z" value="0.157" /> <!-- for defining collision geometry -->
- <const name="upperarm_roll_center_box_center_offset_x" value=".30" /> <!-- from origin of mesh to center of box-geom -->
-
-
- <const name="upperarm_roll_elbow_flex_offset_x" value="0.4" /> <!-- mp 20080801 -->
- <const name="upperarm_roll_min_limit" value="-2.3562" />
- <const name="upperarm_roll_max_limit" value="2.3562" />
-
- <!-- End constant dimensions -->
- <const_block name="pr2_upperarm_roll_joint">
- <axis xyz="1 0 0" /> <!-- direction of the joint in a global coordinate frame -->
- <anchor xyz="0 0 0" /> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
- <limit min="upperarm_roll_min_limit" max="upperarm_roll_max_limit" effort="100" velocity="5" />
- <calibration values="1.5 -1 " /> <!-- Calibration stop/flag indicating location and then direction -->
- </const_block>
-
- <const_block name="pr2_upperarm_roll_inertial">
- <mass value="4.9481" />
- <com xyz="0.21227 0.001205 -0.016293" /> <!-- position of the center of mass with respect to the link's own anchor in a local coordinate frame --> <!-- FIXME John switched x and z for now -->
- <inertia ixx="1.0" ixy="0.000547745916" ixz="0.000119476885" iyy="1.0" iyz="0.001544932307" izz="1.0" />
- </const_block>
-
- <const_block name="pr2_upperarm_roll_collision">
- <origin xyz="upperarm_roll_center_box_center_offset_x 0 0" rpy="0.0 0.0 0.0 " /> <!-- location defined with respect to the link origin in a local coordinate frame -->
- <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
- <geometry name="pr2_upperarm_roll_collision_geom">
- <box size=" upperarm_roll_size_x upperarm_roll_size_y upperarm_roll_size_z " />
- </geometry>
- </const_block>
-
- <const_block name="pr2_upperarm_roll_visual">
- <origin xyz="0 0 0" rpy="0 0 0 " /> <!-- location defined with respect to the link origin in a local coordinate frame -->
- <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
- <map name="gazebo_material" flag="gazebo">
- <elem key="material">Gazebo/Red</elem>
- </map>
- <geometry name="pr2_sholder_roll_mesh_file">
- <mesh filename="upperarm_roll" />
- </geometry>
- </const_block>
-
- <!-- End template definitions for ease of reuse -->
-
- <joint name="upperarm_roll_left_joint" type="revolute" >
- <insert_const_block name="pr2_upperarm_roll_joint"/>
+ <!-- joint blocks -->
+ <joint name="single_link_joint" type="revolute" >
+ <axis xyz="1 0 0" />
+ <anchor xyz="0 0 0" />
+ <limit min="-M_PI" max="M_PI" effort="100" velocity="5" />
+ <calibration values="1.5 -1 " />
</joint>
- <joint name="base_joint" type="planar">
+ <joint name="base_block_joint" type="planar">
</joint>
<!-- link blocks -->
-
- <!-- Begin arm definitions -->
-
- <link name="base"><!-- link specifying the base of the robot -->
-
+ <link name="base_block">
<parent name="world" />
-
- <!-- rotation of a local coordinate frame attached to the link with respect to a global coordinate frame -->
- <origin xyz="0 0 0.002 " rpy="0 0 0" /> <!-- position of a local coordinate frame attached to the link with respect to the parent link's coordinate frame -->
-
- <joint name="base_joint" />
-
+ <origin xyz="0 0 0.002 " rpy="0 0 0" />
+ <joint name="base_block_joint" />
<inertial>
<mass value="1000" />
- <com xyz="0 0 0" /> <!-- position of the center of mass with respect to the link's own anchor in a local coordinate frame -->
+ <com xyz="0 0 0" />
<inertia ixx="1000" ixy="0" ixz="0" iyy="1000" iyz="0" izz="1000" />
</inertial>
-
<visual>
- <origin xyz="0 0 0" rpy="0 0 0" /> <!-- location defined with respect to the link origin in a local coordinate frame -->
- <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
+ <origin xyz="0 0 0" rpy="0 0 0" />
<map name="gazebo_material" flag="gazebo">
<elem key="material">Gazebo/GrassFloor</elem>
</map>
- <geometry name="pr2_base_visual_geom">
+ <geometry name="base_block_visual_geom">
<mesh scale="20 20 0.1" />
</geometry>
</visual>
-
<collision>
- <origin xyz="0 0 0" rpy="0.0 0.0 0.0 " /> <!-- default box is centered at the origin -->
- <!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
- <geometry name="base_collision_geom"> <!-- think about putting mesh here as well -->
+ <origin xyz="0 0 0" rpy="0.0 0.0 0.0" />
+ <geometry name="base_block_collision_geom">
<box size="20 20 0.1" />
</geometry>
</collision>
-
</link>
- <link name="upperarm_roll_left"><!-- link specifying the shoulder of the robot -->
- <parent name="base" />
- <!-- rotation of a local coordinate frame attached to the link with respect to the parent's frame -->
- <origin xyz="0 0 1" rpy="0 0 0" /> <!-- position of a local coordinate frame attached to the link with respect to parent's frame-->
-
- <joint name="upperarm_roll_left_joint" />
+ <link name="single_link">
+ <parent name="base_block" />
+ <origin xyz="0 0 1" rpy="0 0 0" />
+ <joint name="single_link_joint" />
<inertial >
- <insert_const_block name="pr2_upperarm_roll_inertial"/>
+ <mass value="10" />
+ <com xyz="0 0 0" />
+ <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1" />
</inertial>
<visual >
- <insert_const_block name="pr2_upperarm_roll_visual"/>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <map name="gazebo_material" flag="gazebo">
+ <elem key="material">Gazebo/Red</elem>
+ </map>
+ <geometry name="sholder_roll_mesh_file">
+ <mesh scale="1.0 0.1 0.1" />
+ </geometry>
</visual>
<collision >
- <insert_const_block name="pr2_upperarm_roll_collision"/>
+ <origin xyz="0.5 0 0" rpy="0.0 0.0 0.0 " />
+ <geometry name="single_link_collision_geom">
+ <box size="1.0 0.1 0.1" />
+ </geometry>
</collision>
- <map name="upperarm_roll_gravity" flag="gazebo">
+ <map name="single_link_gravity" flag="gazebo">
<elem key="turnGravityOff">true</elem>
</map>
</link>
-
- <!-- End sensor definitions -->
-
- <!--
- <frame name="test">
- <parent name="finger_l_left" />
- <origin xyz="0 0 0 " rpy="0.0 0.0 0.0 " /> // location defined with respect to the link origin in the sensor's local coordinate frame
- // All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis
- </frame>
- -->
-
-
<!-- Define groups of links; a link may be part of multiple groups -->
-
<include>groups_single_link.xml</include>
-
- <!-- controllers for gazebo -->
+ <!-- mechanism controls -->
<include>actuators_single_link.xml</include>
<include>transmissions_single_link.xml</include>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/transmissions_single_link.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/transmissions_single_link.xml 2008-09-30 21:47:24 UTC (rev 4844)
+++ pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/transmissions_single_link.xml 2008-09-30 21:57:26 UTC (rev 4845)
@@ -1,17 +1,12 @@
<?xml version="1.0"?>
-
<robot name="pr2"><!-- name of the robot-->
-
- <!-- Transmissions -->
- <!-- ========================================= -->
- <transmission type="SimpleTransmission" name="upperarm_roll_left_trans">
- <actuator name="upperarm_roll_left_motor" />
- <joint name="upperarm_roll_left_joint" />
+ <transmission type="SimpleTransmission" name="single_link_trans">
+ <actuator name="single_link_motor" />
+ <joint name="single_link_joint" />
<mechanicalReduction>1</mechanicalReduction>
<motorTorqueConstant>1</motorTorqueConstant>
<pulsesPerRevolution>90000</pulsesPerRevolution>
</transmission>
-<!-- ========================================= -->
</robot>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-09-30 22:48:04
|
Revision: 4853
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4853&view=rev
Author: hsujohnhsu
Date: 2008-09-30 22:47:46 +0000 (Tue, 30 Sep 2008)
Log Message:
-----------
more doc updates.
Modified Paths:
--------------
pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-simple.xml
pkg/trunk/demos/2dnav-gazebo/manifest.xml
pkg/trunk/drivers/simulator/gazebo_plugin/doc.dox
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Block_Laser.hh
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Camera.hh
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Laser.hh
Added Paths:
-----------
pkg/trunk/demos/2dnav-gazebo/doc.dox
Modified: pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-simple.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-simple.xml 2008-09-30 22:32:32 UTC (rev 4852)
+++ pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-simple.xml 2008-09-30 22:47:46 UTC (rev 4853)
@@ -1,6 +1,7 @@
<launch>
<master auto="start"/>
<group name="wg">
+ <!-- this is another script that starts up a simple world with PR2 in it -->
<include file="$(find gazebo_robot_description)/pr2_gazebo_actuators.xml"/>
<node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/map3.png 0.1" respawn="false" output="screen" />
<node pkg="amcl_player" type="amcl_player" args="scan:=base_scan" respawn="false" output="screen" />
Added: pkg/trunk/demos/2dnav-gazebo/doc.dox
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/doc.dox (rev 0)
+++ pkg/trunk/demos/2dnav-gazebo/doc.dox 2008-09-30 22:47:46 UTC (rev 4853)
@@ -0,0 +1,42 @@
+/**
+@mainpage
+
+@htmlinclude manifest.html
+
+Here are the gazebo plugins which are currently implemented:
+ \li Simple world, two desks and some simple geometric objects
+ -# \2dnav-gazebo-simple.xml
+ -# \2dnav-gazebo-simple-fake_localization.xml
+ -# \2dnav-gazebo-simple-headless-.xml
+ -# \2dnav-gazebo-simple-headless--fake_localization.xml
+ \li Complex world, Willow Garage Office Map
+ -# \2dnav-gazebo-wg.xml
+ -# \2dnav-gazebo-wg-fake_localization.xml
+ -# \2dnav-gazebo-wg-headless-.xml
+ -# \2dnav-gazebo-wg-headless--fake_localization.xml
+
+ \li Example launch script \b 2dnav-gazebo.xml:
+ -# @verbatim
+ <launch>
+ <master auto="start"/>
+ <group name="wg">
+ <!-- this is another script that starts up a simple world with PR2 in it -->
+ <include file="$(find gazebo_robot_description)/pr2_gazebo_actuators.xml"/>
+ <node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/map3.png 0.1" respawn="false" output="screen" />
+ <node pkg="amcl_player" type="amcl_player" args="scan:=base_scan" respawn="false" output="screen" />
+ <node pkg="wavefront_player" type="wavefront_player" args="scan:=base_scan" respawn="false" output="screen" />
+ <node pkg="nav_view" type="nav_view" respawn="false" output="screen" />
+ <!--node pkg="teleop_base_keyboard" type="teleop_base_keyboard" respawn="false" output="screen" /-->
+ </group>
+
+ <!-- Test for publication of 'tilt_scan' topic. -->
+ <test test-name="hztest_test_amcl1" pkg="rostest" type="hztest" name="amcl_test">
+ <param name="topic" value="localizedpose" />
+ <param name="hz" value="5.0" />
+ <param name="hzerror" value="3.0" />
+ <param name="test_duration" value="5.0" />
+ <param name="check_intervals" value="false" />
+ </test>
+ </launch>
+ @endverbatim
+**/
Modified: pkg/trunk/demos/2dnav-gazebo/manifest.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/manifest.xml 2008-09-30 22:32:32 UTC (rev 4852)
+++ pkg/trunk/demos/2dnav-gazebo/manifest.xml 2008-09-30 22:47:46 UTC (rev 4853)
@@ -1,8 +1,8 @@
<package>
- <description>A demo of 2-D navigation using gazebo, simply replace rosstage with rosgazebo.</description>
+ <description>A clone of 2-D navigation stack \b rosstage using a 3D simulation environment.</description>
<author>John Hsu</author>
<license>BSD</license>
- <url>http://pr.willowgarage.com/wiki/FIXME</url>
+ <url>http://pr.willowgarage.com/wiki/Simulator</url>
<depend package="nav_view"/>
<depend package="roslaunch"/>
<depend package="gazebo_plugin"/>
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/doc.dox
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/doc.dox 2008-09-30 22:32:32 UTC (rev 4852)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/doc.dox 2008-09-30 22:47:46 UTC (rev 4853)
@@ -410,7 +410,7 @@
/// \brief A mutex to lock access to fields that are used in message callbacks
private: ros::thread::mutex lock_;
- /// \brief pointer to ros node
+ /// \brief pointer to ROS node
ros::node *rosnode_;
rostools::Time my_message_;
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Block_Laser.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Block_Laser.hh 2008-09-30 22:32:32 UTC (rev 4852)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Block_Laser.hh 2008-09-30 22:47:46 UTC (rev 4853)
@@ -88,7 +88,7 @@
/// \brief ROS laser block simulation.
/// \li Starts a ROS node if none exists.
/// \li This controller simulates a block of laser range detections.
-/// Resulting point cloud (std_msgs::PointCloudFloat32.msg are published as a ROS topic.
+/// Resulting point cloud (std_msgs::PointCloudFloat32.msg) is published as a ROS topic.
class Ros_Block_Laser : public Controller
{
/// \brief Constructor
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Camera.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Camera.hh 2008-09-30 22:32:32 UTC (rev 4852)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Camera.hh 2008-09-30 22:47:46 UTC (rev 4853)
@@ -62,7 +62,7 @@
*/
/// \brief Ros_Camera Controller.
-/// \li Starts a ros node if none exists. \n
+/// \li Starts a ROS node if none exists. \n
/// \li Simulates a generic camera and broadcast std_msgs::Image topic over ROS. \n
class Ros_Camera : public Controller
{
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Laser.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Laser.hh 2008-09-30 22:32:32 UTC (rev 4852)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Laser.hh 2008-09-30 22:47:46 UTC (rev 4853)
@@ -78,7 +78,7 @@
/// \brief ROS laser scan controller.
/// \li Starts a ROS node if none exists.
-/// \li Simulates a laser range sensor and publish results over ROS on std_msgs::LaserScan.msg
+/// \li Simulates a laser range sensor and publish std_msgs::LaserScan.msg over ROS.
class Ros_Laser : public Controller
{
/// \brief Constructor
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-10-01 00:49:51
|
Revision: 4870
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4870&view=rev
Author: hsujohnhsu
Date: 2008-10-01 00:49:48 +0000 (Wed, 01 Oct 2008)
Log Message:
-----------
doc updates.
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/doc.dox
pkg/trunk/drivers/simulator/gazebo_plugin/doc.dox
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/doc.dox
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/doc.dox 2008-10-01 00:42:13 UTC (rev 4869)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/doc.dox 2008-10-01 00:49:48 UTC (rev 4870)
@@ -7,33 +7,39 @@
@section topic PR2 Controllers ROS Topics
- ROS topics published by controllers
- - BaseControllerNode:
- - \b odom
+ <table border="1">
+ <tr><th> Topic Name </th> <th> Message Type </th> <th> Plugin Name </th> <th> Description </th> </tr>
+ <tr><td> \b odom </td> <td> std_msgs::RobotBase2DOdom </td> <td> base_controller.h </td> <td> odometry information from base caster and wheels. </td> </tr>
+ </table><br>
+
- ROS topics subscribed by controllers
- - BaseControllerNode:
- - \e cmd_vel
+ <table border="1">
+ <tr><th> Topic Name </th> <th> Message Type </th> <th> Plugin Name </th> <th> Description </th> </tr>
+ <tr><td> \e cmd_vel </td> <td> std_msgs::BaseVel </td> <td> base_controller.h </td> <td> velocity commands for the base (vx,vy,vw). </td> </tr>
+ </table><br>
- arm position controllers:
- - \e left_arm_commands
- - \e right_arm_commands
+ <table border="1">
+ <tr><th> Topic Name </th> <th> Message Type </th> <th> Plugin Name </th> <th> Description </th> </tr>
+ <tr><td> \e left_arm_commands </td> <td> pr2_mechanism_controllers::JointPosCmd </td><td>arm_position_controller.h</td> <td> Arm configuration command for each joint. </td> </tr>
+ <tr><td> \e right_arm_commands </td> <td> pr2_mechanism_controllers::JointPosCmd </td><td>arm_position_controller.h</td> <td> Arm configuration command for each joint. </td> </tr>
+ </table><br>
@section services PR2 Controllers ROS Services
- ROS service published by controllers
- - BaseControllerNode:
- - \b base_controller/set_command
- - \b base_controller/get_actual
- - LaserScannerControllerNode:
- - \b tilt_laser_controller/set_command
- - \b tilt_laser_controller/get_actual
- - \b tilt_laser_controller/set_profile
- - ArmPositionControllerNode:
- - \b left_arm_controller/set_command
- - \b left_arm_controller/set_command_array
- - \b left_arm_controller/get_command
- - \b left_arm_controller/set_target
- - \b right_arm_controller/set_command
- - \b right_arm_controller/set_command_array
- - \b right_arm_controller/get_command
- - \b right_arm_controller/set_target
+ <table border="1">
+ <tr><th> Topic Name </th> <th> Message Type </th> <th> Plugin Name </th> <th> Description </th> </tr>
+ <tr><td> \b base_controller/set_command </td> <td>pr2_mechanism_controllers::SetBaseCommand </td> <td> base_controller.h </td> <td> set base command. </td> </tr>
+ <tr><td> \b base_controller/get_actual </td> <td>pr2_mechanism_controllers::GetBaseCommand </td> <td> base_controller.h </td> <td> get base command. </td> </tr>
+ <tr><td> \b tilt_laser_controller/set_command </td> <td>robot_mechanism_controllers::SetCommand </td> <td>laser_scanner_controller.h</td> <td> set controller command. </td> </tr>
+ <tr><td> \b tilt_laser_controller/get_actual </td> <td>robot_mechanism_controllers::GetCommand </td> <td>laser_scanner_controller.h</td> <td> get controller command. </td> </tr>
+ <tr><td> \b tilt_laser_controller/set_profile </td> <td>pr2_mechanism_controllers::SetProfile </td> <td>laser_scanner_controller.h</td> <td> set sweep profile. </td> </tr>
+ <tr><td> \b left_arm_controller/set_command </td> <td>pr2_mechanism_controllers::SetJointTarget </td> <td> arm_position_controller.h</td> <td> set arm joint targets. </td> </tr>
+ <tr><td> \b left_arm_controller/set_command_array </td> <td>pr2_mechanism_controllers::SetJointPosCmd </td> <td> arm_position_controller.h</td> <td> set arm joint positions. </td> </tr>
+ <tr><td> \b left_arm_controller/get_command </td> <td>pr2_mechanism_controllers::GetJointPosCmd </td> <td> arm_position_controller.h</td> <td> get arm joint positions. </td> </tr>
+ <tr><td> \b left_arm_controller/set_target </td> <td>pr2_mechanism_controllers::SetJointTarget </td> <td> arm_position_controller.h</td> <td> set arm joint targets. </td> </tr>
+ </table><br>
+ @todo rename base_controller/get_actual to base_controller/get_command.
+ update documentation for arm position controller services.
**/
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/doc.dox
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/doc.dox 2008-10-01 00:42:13 UTC (rev 4869)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/doc.dox 2008-10-01 00:49:48 UTC (rev 4870)
@@ -279,7 +279,7 @@
- ROS topics published directly by the simulator plugins
<table border="1">
<tr><th> Topic Name </th> <th> Message Type </th> <th> Plugin Name </th> <th> Description </th> </tr>
- <tr><td> \b wrist_left/image </td> <td> std_msgs::Image.msg </td> <td> Ros_Camera.hh </td> <td> Images from left wrist camera. </th> </tr>
+ <tr><td> \b wrist_left/image </td> <td> std_msgs::Image.msg </td> <td> Ros_Camera.hh </td> <td> Images from left wrist camera. </td> </tr>
<tr><td> \b wrist_right/image </td> <td> std_msgs::Image.msg </td> <td> Ros_Camera.hh </td> <td> Images from right wrist camera. </td> </tr>
<tr><td> \b forearm_left/image </td> <td> std_msgs::Image.msg </td> <td> Ros_Camera.hh </td> <td> Images from left forearm camera. </td> </tr>
<tr><td> \b forearm_right/image </td> <td> std_msgs::Image.msg </td> <td> Ros_Camera.hh </td> <td> Images from right forearm camera. </td> </tr>
@@ -304,27 +304,27 @@
<tr><td> \b finger_tip_R_right_ground_truth </td> <td> std_msgs::TransformWithRateStamped </td> <td> P3D.hh </td> <td> Ground truth from center of right finger tip on right arm. </td> </tr>
<tr><td> \b axis_right/ptz_state </td> <td> axis_cam::PTZActuatorState </td> <td> Ros_PTZ.hh </td> <td> PTZ cameras actuator positions. </td> </tr>
<tr><td> \b axis_left/ptz_state </td> <td> axis_cam::PTZActuatorState </td> <td> Ros_PTZ.hh </td> <td> PTZ cameras actuator positions. </td> </tr>
- <tr><td> \b time </td> <td> rostools::Time </td> <td> Ros_Time.hh </td> <td> Simulation time. </th> </tr>
+ <tr><td> \b time </td> <td> rostools::Time </td> <td> Ros_Time.hh </td> <td> Simulation time. </td> </tr>
</table><br>
- ROS topics subscribed by the simulator
<table border="1">
<tr><th> Topic Name </th> <th> Message Type </th> <th> Plugin Name </th> <th> Description </th> </tr>
- <tr><td> \e axis_left_ptz_cmd </td> <td> axis_cam::PTZActuatorCmd </td> <td> Ros_PTZ.hh </td> <td> Actuator commands for PTZ cameras. </th> </tr>
- <tr><td> \e axis_right_ptz_cmd </td> <td> axis_cam::PTZActuatorCmd </td> <td> Ros_PTZ.hh </td> <td> Actuator commands for PTZ cameras. </th> </tr>
+ <tr><td> \e axis_left_ptz_cmd </td> <td> axis_cam::PTZActuatorCmd </td> <td> Ros_PTZ.hh </td> <td> Actuator commands for PTZ cameras. </td> </tr>
+ <tr><td> \e axis_right_ptz_cmd </td> <td> axis_cam::PTZActuatorCmd </td> <td> Ros_PTZ.hh </td> <td> Actuator commands for PTZ cameras. </td> </tr>
</table><br>
@section param PR2 ROS Parameters
- ROS parameters published by the simulator
<table border="1">
<tr><th> Parameter Name </th> <th> Message Type </th> <th> Plugin Name </th> <th> Description </th> </tr>
- <tr><td> \b full_charge_energy </td> <td> \e Float32 </td> <td> gazebo_battery.h </td> <td> Battery state when fully charged in Joules. </th> </tr>
+ <tr><td> \b full_charge_energy </td> <td> \e Float32 </td> <td> gazebo_battery.h </td> <td> Battery state when fully charged in Joules. </td> </tr>
</table><br>
- ROS parameters subscribed by the simulator
<table border="1">
<tr><th> Parameter Name </th> <th> Message Type </th> <th> Plugin Name </th> <th> Description </th> </tr>
- <tr><td> \e timeout </td> <td> \e Float32 </td> <td> gazebo_battery.h </td> <td> Timeout in seconds. (default=10.0sec) </th> </tr>
- <tr><td> \e diagnostic_rate </td> <td> \e Float32 </td> <td> gazebo_battery.h </td> <td> Broadcast rate in Hz. (default=1.0 Hz) </th> </tr>
- <tr><td> \e battery_state_rate </td> <td> \e Float32 </td> <td> gazebo_battery.h </td> <td> Broadcast rate in Hz. (default=1.0 Hz) </th> </tr>
+ <tr><td> \e timeout </td> <td> \e Float32 </td> <td> gazebo_battery.h </td> <td> Timeout in seconds. (default=10.0sec) </td> </tr>
+ <tr><td> \e diagnostic_rate </td> <td> \e Float32 </td> <td> gazebo_battery.h </td> <td> Broadcast rate in Hz. (default=1.0 Hz) </td> </tr>
+ <tr><td> \e battery_state_rate </td> <td> \e Float32 </td> <td> gazebo_battery.h </td> <td> Broadcast rate in Hz. (default=1.0 Hz) </td> </tr>
</table><br>
@section templates How To Construct Your Own ROS Gazebo Plugin
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-10-01 17:31:25
|
Revision: 4881
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4881&view=rev
Author: hsujohnhsu
Date: 2008-10-01 17:31:05 +0000 (Wed, 01 Oct 2008)
Log Message:
-----------
* changed battery full capacity for debug.
* update simulated battery plugin.
listens to ROS topic plugged_in, and if the message (gazebo_plugin::PlugCommand.status) is "the robot is very much plugged into the wall",
then the robot begins charging. any other message content will stop charging.
Modified Paths:
--------------
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_battery.h
pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_battery.cpp
pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/controllers_gazebo.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/controllers_gazebo_headless.xml
Added Paths:
-----------
pkg/trunk/drivers/simulator/gazebo_plugin/msg/PlugCommand.msg
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_battery.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_battery.h 2008-10-01 15:01:31 UTC (rev 4880)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_battery.h 2008-10-01 17:31:05 UTC (rev 4881)
@@ -38,6 +38,7 @@
#include <robot_msgs/BatteryState.h>
#include <robot_msgs/DiagnosticMessage.h>
#include <robot_msgs/DiagnosticStatus.h>
+#include <gazebo_plugin/PlugCommand.h>
#include <ros/node.h>
namespace gazebo
@@ -115,6 +116,38 @@
/// \brief stores current simulator time
private: double current_time_;
+
+ /// \brief stores last simulator time
+ private: double last_time_;
+
+ /// \brief rate to broadcast diagnostic message
+ private: double diagnostic_rate_;
+
+ /// \brief rate to broadcast battery states message
+ private: double battery_state_rate_;
+
+ /// \brief some internal variables for keeping track of simulated battery
+ /// @todo make consumption rate vary with joint commands, motion, etc
+
+ /// \brief full capacity of battery
+ private: double full_capacity_;
+
+ /// \brief charge state;
+ private: double charge_;
+
+ /// \brief default charge rate when plugged in
+ private: double default_charge_rate_;
+
+ /// \brief power drain, if this is negative, we are charging the battery.
+ private: double consumption_rate_;
+
+ /// \brief listen to ROS to see if we are charging
+ private: void SetPlug();
+ private: gazebo_plugin::PlugCommand plug_msg_;
+
+/// @todo make DISCHAGE_RATE something else
+#define DISCHARGE_RATE 1.0
+
};
/** \} */
Added: pkg/trunk/drivers/simulator/gazebo_plugin/msg/PlugCommand.msg
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/msg/PlugCommand.msg (rev 0)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/msg/PlugCommand.msg 2008-10-01 17:31:05 UTC (rev 4881)
@@ -0,0 +1 @@
+string status
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_battery.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_battery.cpp 2008-10-01 15:01:31 UTC (rev 4880)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_battery.cpp 2008-10-01 17:31:05 UTC (rev 4881)
@@ -78,12 +78,37 @@
rosnode_->advertise<robot_msgs::BatteryState>(this->stateTopicName_,10);
this->diagnosticMessageTopicName_ = node->GetString("diagnosticMessageTopicName","diagnostic",0);
rosnode_->advertise<robot_msgs::DiagnosticMessage>(this->diagnosticMessageTopicName_,10);
+
+ /// faking the plug and unplug of robot
+ rosnode_->subscribe("plugged_in",this->plug_msg_,&GazeboBattery::SetPlug,this,10);
+
+ this->full_capacity_ = node->GetDouble("full_charge_energy",0.0,0);
+ this->default_charge_rate_ = node->GetDouble("default_charge_rate",-2.0,0);
+
+ /// @todo make below useful
+ this->diagnostic_rate_ = node->GetDouble("diagnostic_rate",1.0,0);
+ /// @todo make below useful
+ this->battery_state_rate_ = node->GetDouble("dbattery_state_rate_",1.0,0);
}
+ void GazeboBattery::SetPlug()
+ {
+ this->lock_.lock();
+ if (this->plug_msg_.status == "the robot is very much plugged into the wall")
+ this->consumption_rate_ = this->default_charge_rate_ + DISCHARGE_RATE;
+ else
+ this->consumption_rate_ = DISCHARGE_RATE;
+ this->lock_.unlock();
+ }
+
void GazeboBattery::InitChild()
{
this->current_time_ = Simulator::Instance()->GetSimTime();
+ this->last_time_ = this->current_time_;
+ /// initialize battery
+ this->charge_ = this->full_capacity_; /// our convention is joules
+ this->consumption_rate_ = DISCHARGE_RATE; /// time based decay rate in watts
}
void GazeboBattery::UpdateChild()
@@ -92,14 +117,24 @@
/**********************************************************/
/* */
+ /* update battery */
+ /* */
+ /**********************************************************/
+ this->charge_ = this->charge_ - (this->current_time_ - this->last_time_)*this->consumption_rate_;
+ if (this->charge_ < 0) this->charge_ = 0;
+ if (this->charge_ > this->full_capacity_) this->charge_ = this->full_capacity_;
+ //std::cout << " battery charge remaining: " << this->charge_ << " Joules " << std::endl;
+
+ /**********************************************************/
+ /* */
/* publish battery state */
/* */
/**********************************************************/
//this->battery_state_.header.frame_id = ; // no frame id for battery
this->battery_state_.header.stamp.sec = (unsigned long)floor(this->current_time_);
this->battery_state_.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->current_time_ - this->battery_state_.header.stamp.sec) );
- this->battery_state_.energy_remaining = 1000;
- this->battery_state_.power_consumption = 10;
+ this->battery_state_.energy_remaining = this->charge_;
+ this->battery_state_.power_consumption = this->consumption_rate_;
this->lock_.lock();
this->rosnode_->publish(this->stateTopicName_,this->battery_state_);
@@ -119,6 +154,7 @@
this->lock_.lock();
this->rosnode_->publish(this->diagnosticMessageTopicName_,diagnostic_message_);
this->lock_.unlock();
+ this->last_time_ = this->current_time_;
}
void GazeboBattery::FiniChild()
@@ -127,6 +163,7 @@
rosnode_->unadvertise(this->stateTopicName_);
rosnode_->unadvertise(this->diagnosticMessageTopicName_);
+ rosnode_->unsubscribe("plugged_in");
}
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/controllers_gazebo.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/controllers_gazebo.xml 2008-10-01 15:01:31 UTC (rev 4880)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/controllers_gazebo.xml 2008-10-01 17:31:05 UTC (rev 4881)
@@ -33,7 +33,7 @@
<timeout>5</timeout>
<diagnostic_rate>1.0</diagnostic_rate>
<battery_state_rate>1.0</battery_state_rate>
- <full_charge_energy>1.0</full_charge_energy>
+ <full_charge_energy>120.0</full_charge_energy>
<diagnosticTopicName>diagnostic</diagnosticTopicName>
<stateTopicName>battery_state</stateTopicName>
<selfTestServiceName>self_test</selfTestServiceName>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/controllers_gazebo_headless.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/controllers_gazebo_headless.xml 2008-10-01 15:01:31 UTC (rev 4880)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/controllers_gazebo_headless.xml 2008-10-01 17:31:05 UTC (rev 4881)
@@ -33,7 +33,7 @@
<timeout>5</timeout>
<diagnostic_rate>1.0</diagnostic_rate>
<battery_state_rate>1.0</battery_state_rate>
- <full_charge_energy>1.0</full_charge_energy>
+ <full_charge_energy>120.0</full_charge_energy>
<diagnosticTopicName>diagnostic</diagnosticTopicName>
<stateTopicName>battery_state</stateTopicName>
<selfTestServiceName>self_test</selfTestServiceName>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-10-01 22:38:44
|
Revision: 4895
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4895&view=rev
Author: hsujohnhsu
Date: 2008-10-01 22:38:35 +0000 (Wed, 01 Oct 2008)
Log Message:
-----------
* added dual link model
* temp fix for anchor offset problem.
* removed ifaces for gazebo_plugin
Modified Paths:
--------------
pkg/trunk/drivers/simulator/gazebo_plugin/doc.dox
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/F3D.hh
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Bumper.hh
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_JointForce.hh
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_actuators.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_battery.h
pkg/trunk/drivers/simulator/gazebo_plugin/src/F3D.cc
pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc
pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Bumper.cc
pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_JointForce.cc
pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp
pkg/trunk/robot_descriptions/gazebo_robot_description/CMakeLists.txt
pkg/trunk/robot_descriptions/gazebo_robot_description/src/urdf2gazebo.cpp
Added Paths:
-----------
pkg/trunk/robot_descriptions/gazebo_robot_description/dual_link.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/gazebo_joints_dual_link.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_dual_link.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_dual_link.world
pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/
pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/actuators_dual_link.xml
pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/controllers_dual_link.xml
pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/gazebo/
pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/gazebo/actuators_dual_link.xml
pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/gazebo/controllers_dual_link.xml
pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/gazebo/controllers_gazebo_dual_link.xml
pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/gazebo/gazebo_joints_dual_link.xml
pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/gazebo/groups_dual_link.xml
pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/gazebo/pr2_dual_link.xml
pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/gazebo/pr2_dual_link_gazebo_actuators.xml
pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/gazebo/transmissions_dual_link.xml
pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/groups_dual_link.xml
pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/pr2_dual_link.xml
pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/transmissions_dual_link.xml
Removed Paths:
-------------
pkg/trunk/drivers/simulator/gazebo_plugin/gazebo_mcn.jpg
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/doc.dox
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/doc.dox 2008-10-01 22:36:51 UTC (rev 4894)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/doc.dox 2008-10-01 22:38:35 UTC (rev 4895)
@@ -13,8 +13,6 @@
\li \ref gazebo::P3D "ROS Ground Truth Broadcaster"
- @image html gazebo_mcn.jpg "Gazebo Mechanism Control Model"
-
@section usage PR2 Simulation Quick Start Guide
\li If you have successfully \e rosmake \e 2dnav-gazebo, you have EVERYTHING needed to run all simulator+PR2 demos. Otherwise, to compile a minimum set of packages for running Gazebo:
@verbatim
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/F3D.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/F3D.hh 2008-10-01 22:36:51 UTC (rev 4894)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/F3D.hh 2008-10-01 22:38:35 UTC (rev 4895)
@@ -35,7 +35,6 @@
namespace gazebo
{
- class PositionIface;
/// @addtogroup gazebo_dynamic_plugins Gazebo ROS Dynamic Plugins
/// @{
@@ -85,9 +84,6 @@
/// \brief Finalize the controller
protected: virtual void FiniChild();
- /// \brief The Position Iface
- private: PositionIface *myIface;
-
/// \brief A link to the parent Model
private: Model *myParent;
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh 2008-10-01 22:36:51 UTC (rev 4894)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh 2008-10-01 22:38:35 UTC (rev 4895)
@@ -36,15 +36,13 @@
namespace gazebo
{
- class PositionIface;
-
/// @addtogroup gazebo_dynamic_plugins Gazebo ROS Dynamic Plugins
/// @{
/** \defgroup P3D Groud Truth Position Pose and Rates Interface
\brief P3D controller.
- This controller requires to a model as its parent. The plugin broadcasts a body's pose and rates through ROS std_msgs::TransformWithRateStamped message as well as filling out a Gazebo::PositionIface. In the example below, the plubin broadcasts pose and rate of a body named \b body_name over ROS topic name \b body_pose_groud_truth.
+ This controller requires to a model as its parent. The plugin broadcasts a body's pose and rates through ROS std_msgs::TransformWithRateStamped message. In the example below, the plubin broadcasts pose and rate of a body named \b body_name over ROS topic name \b body_pose_groud_truth.
\verbatim
<model:physical name="some_fancy_model">
@@ -88,9 +86,6 @@
/// \brief Finalize the controller
protected: virtual void FiniChild();
- /// \brief The actarray interface
- private: PositionIface *myIface;
-
/// \brief The parent Model
private: Model *myParent;
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Bumper.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Bumper.hh 2008-10-01 22:36:51 UTC (rev 4894)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Bumper.hh 2008-10-01 22:38:35 UTC (rev 4895)
@@ -87,8 +87,6 @@
/// The parent Model
private: ContactSensor *myParent;
- /// The Iface.
- private: BumperIface *myIface;
};
/** \} */
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_JointForce.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_JointForce.hh 2008-10-01 22:36:51 UTC (rev 4894)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_JointForce.hh 2008-10-01 22:38:35 UTC (rev 4895)
@@ -85,8 +85,6 @@
/// \brief The parent Model
private: Model *myParent;
- /// \brief The Iface. The dJointFeedback structs are rather arbitrary, so we use an Opaque Interface
- private: OpaqueIface *myIface;
/// \brief The joint feedbacks
private: dJointFeedback *jointfeedbacks[ROS_JOINTFORCE_CONTROLLER_MAX_FEEDBACKS];
/// \brief The number of joints we are watching
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_actuators.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_actuators.h 2008-10-01 22:36:51 UTC (rev 4894)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_actuators.h 2008-10-01 22:38:35 UTC (rev 4895)
@@ -44,7 +44,6 @@
namespace gazebo
{
class HingeJoint;
-class PositionIface;
class XMLConfigNode;
/// @addtogroup gazebo_dynamic_plugins Gazebo ROS Dynamic Plugins
@@ -102,6 +101,8 @@
* -# Controller reads \b joint-state and issues a \b joint-torque-command
* .
*
+ * @image html "http://pr.willowgarage.com/wiki/gazebo_plugin?action=AttachFile&do=get&target=gazebo_mcn.jpg" "Gazebo Mechanism Control Model"
+ *
**/
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_battery.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_battery.h 2008-10-01 22:36:51 UTC (rev 4894)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_battery.h 2008-10-01 22:38:35 UTC (rev 4895)
@@ -73,6 +73,7 @@
/**
* \brief Battery simulation
* \li starts a ROS node if none exists
+ * \li return battery state and diagnostic message over ROS topic.
* .
*
**/
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/F3D.cc
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/F3D.cc 2008-10-01 22:36:51 UTC (rev 4894)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/F3D.cc 2008-10-01 22:38:35 UTC (rev 4895)
@@ -77,11 +77,6 @@
// Load the controller
void F3D::LoadChild(XMLConfigNode *node)
{
- this->myIface = dynamic_cast<PositionIface*>(this->ifaces[0]);
-
- if (!this->myIface)
- gzthrow("F3D controller requires a Actarray Iface");
-
std::string bodyName = node->GetString("bodyName", "", 1);
this->myBody = dynamic_cast<Body*>(this->myParent->GetBody(bodyName));
// this->myBody = dynamic_cast<Body*>(this->myParent->GetBody(bodyName));
@@ -112,24 +107,13 @@
// get torque on body
torque = this->myBody->GetTorque();
- this->myIface->Lock(1);
- this->myIface->data->head.time = Simulator::Instance()->GetSimTime();
-
- this->myIface->data->pose.pos.x = force.x;
- this->myIface->data->pose.pos.y = force.y;
- this->myIface->data->pose.pos.z = force.z;
-
- this->myIface->data->pose.roll = torque.x;
- this->myIface->data->pose.pitch = torque.y;
- this->myIface->data->pose.yaw = torque.z;
-
//FIXME: toque not published, need new message type of new topic name for torque
//FIXME: use name space of body id (link name)?
this->lock.lock();
// copy data into pose message
this->vector3Msg.header.frame_id = this->frameName;
- this->vector3Msg.header.stamp.sec = (unsigned long)floor(this->myIface->data->head.time);
- this->vector3Msg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->myIface->data->head.time - this->vector3Msg.header.stamp.sec) );
+ this->vector3Msg.header.stamp.sec = (unsigned long)floor(Simulator::Instance()->GetSimTime());
+ this->vector3Msg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( Simulator::Instance()->GetSimTime() - this->vector3Msg.header.stamp.sec) );
this->vector3Msg.vector.x = force.x;
this->vector3Msg.vector.y = force.y;
@@ -142,7 +126,6 @@
rosnode->publish(this->topicName,this->vector3Msg);
this->lock.unlock();
- this->myIface->Unlock();
}
////////////////////////////////////////////////////////////////////////////////
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc 2008-10-01 22:36:51 UTC (rev 4894)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc 2008-10-01 22:38:35 UTC (rev 4895)
@@ -74,11 +74,6 @@
// Load the controller
void P3D::LoadChild(XMLConfigNode *node)
{
- this->myIface = dynamic_cast<PositionIface*>(this->ifaces[0]);
-
- if (!this->myIface)
- gzthrow("P3D controller requires a Actarray Iface, though not used.");
-
std::string bodyName = node->GetString("bodyName", "", 1);
this->myBody = dynamic_cast<Body*>(this->myParent->GetBody(bodyName));
// this->myBody = dynamic_cast<Body*>(this->myParent->GetBody(bodyName));
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Bumper.cc
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Bumper.cc 2008-10-01 22:36:51 UTC (rev 4894)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Bumper.cc 2008-10-01 22:38:35 UTC (rev 4895)
@@ -59,12 +59,6 @@
// Load the controller
void Ros_Bumper::LoadChild(XMLConfigNode *node)
{
- this->myIface = dynamic_cast<BumperIface*>(this->ifaces[0]);
-
- if (!this->myIface)
- {
- gzthrow("Ros_Bumper controller requires an BumperIface");
- }
}
////////////////////////////////////////////////////////////////////////////////
@@ -77,19 +71,6 @@
// Update the controller
void Ros_Bumper::UpdateChild()
{
- this->myIface->Lock(1);
-
- this->myIface->data->bumper_count = this->myParent->GetContactCount();
-
- for (unsigned int i=0; i < this->myParent->GetContactCount(); i++)
- {
- this->myIface->data->head.time = this->myParent->GetContactTime(i);
- this->myIface->data->bumpers[i] = this->myParent->GetContactState(i);
- }
-
- this->myParent->ResetContactStates();
-
- this->myIface->Unlock();
}
////////////////////////////////////////////////////////////////////////////////
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_JointForce.cc
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_JointForce.cc 2008-10-01 22:36:51 UTC (rev 4894)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_JointForce.cc 2008-10-01 22:38:35 UTC (rev 4895)
@@ -64,10 +64,7 @@
std::string jointName;
dJointFeedback *jFeedback = new dJointFeedback;
int i =0;
- this->myIface = dynamic_cast<OpaqueIface*>(this->ifaces[0]);
- if (!this->myIface) {
- gzthrow("Ros_JointForce controller requires an OpaqueIface");
- }
+
jNode = node->GetChild("joint");
while(jNode && i < ROS_JOINTFORCE_CONTROLLER_MAX_FEEDBACKS)
{
@@ -87,31 +84,31 @@
// Initialize the controller
void Ros_JointForce::InitChild()
{
- //this->myIface->data->data = new uint8_t[sizeof(dJointFeedback)*n_joints];
+
}
////////////////////////////////////////////////////////////////////////////////
// Update the controller
void Ros_JointForce::UpdateChild()
{
- this->myIface->Lock(1);
- this->myIface->data->head.time = Simulator::Instance()->GetSimTime();
- // Let me explain this number: each joint reports 4 vectors: Force and torque
- // on each jointed object, respectively. These vectors have 4 elements: x,y,z
- // and a fourth one. So we transmit 4 dReals per vector.
- this->myIface->data->data_count = n_joints*4*4*sizeof(dReal);
+ // this->myIface->Lock(1);
+ // this->myIface->data->head.time = Simulator::Instance()->GetSimTime();
+ // // Let me explain this number: each joint reports 4 vectors: Force and torque
+ // // on each jointed object, respectively. These vectors have 4 elements: x,y,z
+ // // and a fourth one. So we transmit 4 dReals per vector.
+ // this->myIface->data->data_count = n_joints*4*4*sizeof(dReal);
- for(int i=0; i< n_joints; i++) {
- // Copy vector for force on first object
- memcpy(this->myIface->data->data + (i*4 + 0)*4*sizeof(dReal), this->jointfeedbacks[i]->f1, 4*sizeof(dReal));
- // Copy vector for torque on first object
- memcpy(this->myIface->data->data + (i*4 + 1)*4*sizeof(dReal), this->jointfeedbacks[i]->t1, 4*sizeof(dReal));
- // Copy vector for force on second object
- memcpy(this->myIface->data->data + (i*4 + 2)*4*sizeof(dReal), this->jointfeedbacks[i]->f2, 4*sizeof(dReal));
- // Copy vector for torque on second object
- memcpy(this->myIface->data->data + (i*4 + 3)*4*sizeof(dReal), this->jointfeedbacks[i]->t2, 4*sizeof(dReal));
- }
- this->myIface->Unlock();
+ // for(int i=0; i< n_joints; i++) {
+ // // Copy vector for force on first object
+ // memcpy(this->myIface->data->data + (i*4 + 0)*4*sizeof(dReal), this->jointfeedbacks[i]->f1, 4*sizeof(dReal));
+ // // Copy vector for torque on first object
+ // memcpy(this->myIface->data->data + (i*4 + 1)*4*sizeof(dReal), this->jointfeedbacks[i]->t1, 4*sizeof(dReal));
+ // // Copy vector for force on second object
+ // memcpy(this->myIface->data->data + (i*4 + 2)*4*sizeof(dReal), this->jointfeedbacks[i]->f2, 4*sizeof(dReal));
+ // // Copy vector for torque on second object
+ // memcpy(this->myIface->data->data + (i*4 + 3)*4*sizeof(dReal), this->jointfeedbacks[i]->t2, 4*sizeof(dReal));
+ // }
+ // this->myIface->Unlock();
}
////////////////////////////////////////////////////////////////////////////////
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp 2008-10-01 22:36:51 UTC (rev 4894)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp 2008-10-01 22:38:35 UTC (rev 4895)
@@ -66,6 +66,7 @@
void GazeboActuators::LoadChild(XMLConfigNode *node)
{
// read pr2.xml (pr2_gazebo_actuators.xml)
+ // setup actuators, then setup mechanism control node
ReadPr2Xml(node);
// Initializes the fake state (for running the transmissions backwards).
@@ -254,6 +255,7 @@
hw_.actuators_.push_back(new Actuator(*it));
}
+ // Setup mechanism control node
mcn_.initXml(doc.RootElement());
}
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/CMakeLists.txt
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/CMakeLists.txt 2008-10-01 22:36:51 UTC (rev 4894)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/CMakeLists.txt 2008-10-01 22:38:35 UTC (rev 4895)
@@ -51,4 +51,10 @@
DEPENDS urdf2gazebo
COMMENT "Building Gazebo model for Single Link Test")
+# dual_link_test
+add_custom_target(pr2_gazebo_model_dual_link ALL
+ COMMAND ${urdf2gazebo_exe} ${wg_robot_description_PACKAGE_PATH}/dual_link_test/gazebo/pr2_dual_link_gazebo_actuators.xml ${gazebo_robot_description_PACKAGE_PATH}/world/pr2_xml_dual_link.model
+ DEPENDS urdf2gazebo
+ COMMENT "Building Gazebo model for Dual Link Test")
+
Added: pkg/trunk/robot_descriptions/gazebo_robot_description/dual_link.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/dual_link.xml (rev 0)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/dual_link.xml 2008-10-01 22:38:35 UTC (rev 4895)
@@ -0,0 +1,14 @@
+<launch>
+ <group name="wg">
+ <param name="robotdesc/pr2" command="$(find wg_robot_description_parser)/merge "$(find wg_robot_description)/dual_link_test/pr2_dual_link.xml"" />
+ <node pkg="gazebo" type="gazebo" args="$(find gazebo_robot_description)/world/robot_dual_link.world" respawn="false" output="screen">
+ <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$LD_LIBRARY_PATH" />
+ <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
+ <env name="MC_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
+ </node>
+ </group>
+ <!--node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/dual_link_test/controllers_dual_link.xml" respawn="false" output="screen" /--> <!-- load default arm controller -->
+ <!--node pkg="robot_mechanism_controllers" type="control.py" args="set test_controller 0.5" respawn="false" output="screen" /--> <!-- open gripper .5 radians -->
+</launch>
+
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/src/urdf2gazebo.cpp
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/src/urdf2gazebo.cpp 2008-10-01 22:36:51 UTC (rev 4894)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/src/urdf2gazebo.cpp 2008-10-01 22:38:35 UTC (rev 4895)
@@ -315,7 +315,7 @@
for (int j = 0 ; j < 3 ; ++j)
{
// undo Gazebo's shift of object anchor to geom xyz, stay in body cs
- tmpAnchor[j] = (link->joint->anchor)[j] - 0.0*(link->inertial->com)[j] - (link->collision->xyz)[j];
+ tmpAnchor[j] = (link->joint->anchor)[j] - 0.0*(link->inertial->com)[j] - 0.5*(link->collision->xyz)[j]; /// @todo compensate for gazebo's error. John is fixing this one
}
addKeyValue(joint, "anchorOffset", values2str(3, tmpAnchor));
Added: pkg/trunk/robot_descriptions/gazebo_robot_description/world/gazebo_joints_dual_link.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/gazebo_joints_dual_link.xml (rev 0)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/gazebo_joints_dual_link.xml 2008-10-01 22:38:35 UTC (rev 4895)
@@ -0,0 +1 @@
+link ../../wg_robot_description/dual_link_test/gazebo/gazebo_joints_dual_link.xml
\ No newline at end of file
Property changes on: pkg/trunk/robot_descriptions/gazebo_robot_description/world/gazebo_joints_dual_link.xml
___________________________________________________________________
Added: svn:special
+ *
Added: pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_dual_link.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_dual_link.xml (rev 0)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_dual_link.xml 2008-10-01 22:38:35 UTC (rev 4895)
@@ -0,0 +1,118 @@
+<?xml version="1.0"?>
+
+<robot name="pr2"><!-- name of the robot-->
+
+ <!-- joint blocks -->
+ <joint name="link1_joint" type="revolute" >
+ <axis xyz="0 0 1" />
+ <anchor xyz="0 0 0" />
+ <limit min="-3.14159" max="3.14159" effort="100" velocity="5" />
+ <calibration values="1.5 -1 " />
+ </joint>
+
+ <!-- joint blocks -->
+ <joint name="link2_joint" type="revolute" >
+ <axis xyz="0 1 0" />
+ <anchor xyz="0 0 0" />
+ <limit min="-3.14159" max="3.14159" effort="100" velocity="5" />
+ <calibration values="1.5 -1 " />
+ </joint>
+
+ <joint name="base_block_joint" type="planar">
+ </joint>
+
+ <!-- link blocks -->
+ <link name="base_block">
+ <parent name="world" />
+ <origin xyz="0 0 0.002 " rpy="0 0 0" />
+ <joint name="base_block_joint" />
+ <inertial>
+ <mass value="1000" />
+ <com xyz="0 0 0" />
+ <inertia ixx="1000" ixy="0" ixz="0" iyy="1000" iyz="0" izz="1000" />
+ </inertial>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <map name="gazebo_material" flag="gazebo">
+ <elem key="material">Gazebo/GrassFloor</elem>
+ </map>
+ <geometry name="base_block_visual_geom">
+ <mesh scale="0.1 0.1 0.01" />
+ </geometry>
+ </visual>
+ <collision>
+ <origin xyz="0 0 0" rpy="0.0 0.0 0.0" />
+ <geometry name="base_block_collision_geom">
+ <box size="20 20 0.01" />
+ </geometry>
+ </collision>
+ </link>
+
+ <link name="link1">
+ <parent name="base_block" />
+ <origin xyz="0 0 1" rpy="0 0 0" />
+ <joint name="link1_joint" />
+ <inertial >
+ <mass value="10" />
+ <com xyz="0 0 0" />
+ <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1" />
+ </inertial>
+ <visual >
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <map name="gazebo_material" flag="gazebo">
+ <elem key="material">Gazebo/Red</elem>
+ </map>
+ <geometry name="sholder_pan_mesh_file">
+ <!--mesh filename="shoulder_yaw" /-->
+ <mesh scale="0.1 0.1 1.0" />
+ </geometry>
+ </visual>
+ <collision >
+ <origin xyz="0 0 -0.5" rpy="0.0 0.0 0.0 " />
+ <geometry name="link1_collision_geom">
+ <box size="0.1 0.1 1.0" />
+ </geometry>
+ </collision>
+ <map name="link1_gravity" flag="gazebo">
+ <elem key="turnGravityOff">false</elem>
+ </map>
+ </link>
+
+
+ <link name="link2">
+ <parent name="link1" />
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <joint name="link2_joint" />
+ <inertial >
+ <mass value="10" />
+ <com xyz="0 0 0" />
+ <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1" />
+ </inertial>
+ <visual >
+ <origin xyz="0 0 0" rpy="0 0 0" />
+ <map name="gazebo_material" flag="gazebo">
+ <elem key="material">Gazebo/Blue</elem>
+ </map>
+ <geometry name="sholder_roll_mesh_file">
+ <mesh scale="1.0 0.1 0.1" />
+ </geometry>
+ </visual>
+ <collision >
+ <origin xyz="0.5 0 0" rpy="0.0 0.0 0.0 " />
+ <geometry name="link2_collision_geom">
+ <box size="1.0 0.1 0.1" />
+ </geometry>
+ </collision>
+ <map name="link2_gravity" flag="gazebo">
+ <elem key="turnGravityOff">false</elem>
+ </map>
+ </link>
+
+
+ <!-- Define groups of links; a link may be part of multiple groups -->
+ <include>groups_dual_link.xml</include>
+ <!-- mechanism controls -->
+ <include>actuators_dual_link.xml</include>
+ <include>transmissions_dual_link.xml</include>
+
+</robot>
Added: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_dual_link.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_dual_link.world (rev 0)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_dual_link.world 2008-10-01 22:38:35 UTC (rev 4895)
@@ -0,0 +1,201 @@
+<?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>
+
+ <!-- Physics Engine: ODE parameters -->
+ <!-- cfm is 1e-5 for single precision -->
+ <!-- erp is typically .1-.8 -->
+ <!-- here's the global contact cfm/erp -->
+ <physics:ode>
+ <stepTime>0.01</stepTime>
+ <gravity>0 0 -9.8</gravity>
+ <cfm>0.0000000001</cfm>
+ <erp>0.2</erp>
+ <quickStep>false</quickStep>
+ </physics:ode>
+
+ <!-- Rendering Engine -->
+ <rendering:gui>
+ <type>fltk</type>
+ <size>1024 800</size>
+ <pos>0 0</pos>
+ <frames>
+ <row height="50%">
+ <camera width="50%">
+ <xyz> .25 1.5 1.0</xyz>
+ <rpy> 0 0 -90</rpy>
+ </camera>
+ <camera width="50%">
+ <xyz>.25 0 2.5</xyz>
+ <rpy> 0 90 0</rpy>
+ </camera>
+ </row>
+ <row height="50%">
+ <camera width="50%">
+ <xyz>-1 0 1</xyz>
+ <rpy> 0 0 0</rpy>
+ </camera>
+ <camera width="50%">
+ <xyz>1.5 0 1</xyz>
+ <rpy>0 0 180</rpy>
+ </camera>
+ </row>
+ </frames>
+ </rendering:gui>
+
+ <!-- GUI Camera -->
+ <rendering:ogre>
+ <ambient>1.0 1.0 1.0 1.0</ambient>
+ <sky>
+ <material>Gazebo/CloudySky</material>
+ </sky>
+ <gazeboPath>media</gazeboPath>
+ <grid>false</grid>
+ <maxUpdateRate>100</maxUpdateRate>
+ </rendering:ogre>
+
+ <!-- White Directional light -->
+ <model:renderable name="directional_white">
+ <light>
+ <type>directional</type>
+ <direction>0 -0.5 -0.5</direction>
+ <diffuseColor>0.4 0.4 0.4</diffuseColor>
+ <specularColor>0.0 0.0 0.0</specularColor>
+ <attenuation>1 0.0 1.0 0.4</attenuation>
+ </light>
+ </model:renderable>
+
+
+ <!-- Ground plane -->
+ <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">
+ <kp>1000000.0</kp>
+ <kd>1.0</kd>
+ <normal>0 0 1</normal>
+ <size>51.3 51.3</size>
+ <material>Gazebo/GrassFloor</material>
+ </geom:plane>
+ </body:plane>
+ </model:physical>
+
+ <!-- Object1: The small cylinder "cup" -->
+ <model:physical name="cylinder1_model">
+ <xyz> 0.5 0.2 0.7</xyz>
+ <rpy> 0.0 0.0 0.0</rpy>
+ <static>true</static>
+ <body:cylinder name="cylinder1_body">
+ <geom:cylinder name="cylinder1_geom">
+ <kp>100000000.0</kp>
+ <kd>0.1</kd>
+ <mesh>default</mesh>
+ <size>0.025 0.075</size>
+ <mass> 0.05</mass>
+ <visual>
+ <size> 0.05 0.05 0.075</size>
+ <material>Gazebo/PioneerBody</material>
+ <mesh>unit_cylinder</mesh>
+ </visual>
+ </geom:cylinder>
+ <geom:box name="cylinder1_base_geom">
+ <kp>100000000.0</kp>
+ <kd>0.1</kd>
+ <mesh>default</mesh>
+ <xyz>0.0 0.0 -0.033</xyz>
+ <size>0.05 0.05 0.01</size>
+ <mass> 0.01</mass>
+ <visual>
+ <size> 0.05 0.05 0.01</size>
+ <material>Gazebo/Fish</material>
+ <mesh>unit_box</mesh>
+ </visual>
+ </geom:box>
+ </body:cylinder>
+ </model:physical>
+
+ <!-- Object2: The small box "cup" -->
+ <model:phys...
[truncated message content] |
|
From: <hsu...@us...> - 2008-10-02 02:26:38
|
Revision: 4918
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4918&view=rev
Author: hsujohnhsu
Date: 2008-10-02 02:26:26 +0000 (Thu, 02 Oct 2008)
Log Message:
-----------
* fix double anchor offset in gazebo, Inertia updates. validation pending, setting up test case.
* update gains in gripper and base. update effort limits in gripper.
Modified Paths:
--------------
pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_dual_link.world
pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/gazebo/gazebo_joints_dual_link.xml
pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/pr2_dual_link.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/gazebo_joints.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
Modified: pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
===================================================================
--- pkg/trunk/3rdparty/gazebo/gazebo_patch.diff 2008-10-02 00:43:55 UTC (rev 4917)
+++ pkg/trunk/3rdparty/gazebo/gazebo_patch.diff 2008-10-02 02:26:26 UTC (rev 4918)
@@ -302,18 +302,6 @@
};
/// \}
-Index: server/physics/HingeJoint.hh
-===================================================================
---- server/physics/HingeJoint.hh (revision 7049)
-+++ server/physics/HingeJoint.hh (working copy)
-@@ -126,6 +126,7 @@
- private: ParamT<Vector3> *axisP;
- private: ParamT<Angle> *loStopP;
- private: ParamT<Angle> *hiStopP;
-+ private: ParamT<Vector3> *anchorOffsetP;
- };
- /// \}
-
Index: server/physics/CylinderGeom.cc
===================================================================
--- server/physics/CylinderGeom.cc (revision 7049)
@@ -381,7 +369,7 @@
}
////////////////////////////////////////////////////////////////////////////////
-@@ -123,6 +144,30 @@
+@@ -123,6 +144,56 @@
this->massP->SetValue( 0.001 );
}
@@ -408,11 +396,42 @@
+ this->ixz = this->ixzP->GetValue();
+ this->iyz = this->iyzP->GetValue();
+
++ // setup this->mass as well
++ this->mass.c[0] = this->cx;
++ this->mass.c[1] = this->cy;
++ this->mass.c[2] = this->cz;
++ this->mass.I[0] = this->ixx;
++ this->mass.I[1] = this->ixy;
++ this->mass.I[2] = this->ixz;
++ this->mass.I[3] = this->ixy;
++ this->mass.I[4] = this->iyy;
++ this->mass.I[5] = this->iyz;
++ this->mass.I[6] = this->ixz;
++ this->mass.I[7] = this->iyz;
++ this->mass.I[8] = this->izz;
+
++ // std::cout << " c[0] " << this->mass.c[0] << std::endl;
++ // std::cout << " c[1] " << this->mass.c[1] << std::endl;
++ // std::cout << " c[2] " << this->mass.c[2] << std::endl;
++ // std::cout << " I[0] " << this->mass.I[0] << std::endl;
++ // std::cout << " I[1] " << this->mass.I[1] << std::endl;
++ // std::cout << " I[2] " << this->mass.I[2] << std::endl;
++ // std::cout << " I[3] " << this->mass.I[3] << std::endl;
++ // std::cout << " I[4] " << this->mass.I[4] << std::endl;
++ // std::cout << " I[5] " << this->mass.I[5] << std::endl;
++ // std::cout << " I[6] " << this->mass.I[6] << std::endl;
++ // std::cout << " I[7] " << this->mass.I[7] << std::endl;
++ // std::cout << " I[8] " << this->mass.I[8] << std::endl;
++
this->contact->Load(node);
this->LoadChild(node);
-@@ -401,18 +446,21 @@
+@@ -396,23 +467,25 @@
+ Pose3d pose;
+ dQuaternion q;
+ dMatrix3 r;
+- dMass bodyMass;
+
if (!this->placeable)
return NULL;
@@ -556,43 +575,43 @@
// Return the ID of this body
dBodyID Body::GetId() const
{
-@@ -505,6 +590,7 @@
+@@ -505,8 +590,15 @@
*/
void Body::UpdateCoM()
{
-+ return;
++ // std::cout << " mass x " << this->mass.c[0]
++ // << " y " << this->mass.c[1]
++ // << " z " << this->mass.c[2]
++ // << std::endl;
++ // comPose.pos.x = this->mass.c[0];
++ // comPose.pos.y = this->mass.c[1];
++ // comPose.pos.z = this->mass.c[2];
++
const dMass *lmass;
- Pose3d oldPose, newPose, pose;
+- Pose3d oldPose, newPose, pose;
std::map< std::string, Geom* >::iterator giter;
-Index: server/physics/HingeJoint.cc
-===================================================================
---- server/physics/HingeJoint.cc (revision 7049)
-+++ server/physics/HingeJoint.cc (working copy)
-@@ -44,6 +44,7 @@
- this->axisP = new ParamT<Vector3>("axis",Vector3(0,1,0), 1);
- this->loStopP = new ParamT<Angle>("lowStop",-M_PI,0);
- this->hiStopP = new ParamT<Angle>("highStop",M_PI,0);
-+ this->anchorOffsetP = new ParamT<Vector3>("anchorOffset",Vector3(0,0,0),0);
- Param::End();
- }
-@@ -64,6 +65,7 @@
- this->axisP->Load(node);
- this->loStopP->Load(node);
- this->hiStopP->Load(node);
-+ this->anchorOffsetP->Load(node);
+ if (!this->bodyId)
+@@ -523,8 +615,10 @@
+ dMassAdd( &this->mass, lmass );
+ }
+ }
++ return; // Stop pose update, we have full com xyz, I control
- // Perform this three step ordering to ensure the parameters are set
- // properly. This is taken from the ODE wiki.
-@@ -133,7 +135,7 @@
- // Set the anchor point
- void HingeJoint::SetAnchor( const Vector3 &anchor )
- {
-- dJointSetHingeAnchor( this->jointId, anchor.x, anchor.y, anchor.z );
-+ dJointSetHingeAnchor( this->jointId, anchor.x+anchorOffsetP->GetValue().x, anchor.y+anchorOffsetP->GetValue().y, anchor.z+anchorOffsetP->GetValue().z );
- }
+ // Old pose for the CoM
++ Pose3d oldPose, newPose, pose;
+ oldPose = this->comPose;
+ if (std::isnan(this->mass.c[0]))
+@@ -565,7 +659,7 @@
+ this->comPose = newPose;
+ // My Cheap Hack, to put the center of mass at the origin
+- this->mass.c[0] = this->mass.c[1] = this->mass.c[2] = 0;
++ //this->mass.c[0] = this->mass.c[1] = this->mass.c[2] = 0;
+
+ // Set the mass matrix
+ if (this->mass.mass > 0)
Index: server/physics/ode/ODEPhysics.hh
===================================================================
--- server/physics/ode/ODEPhysics.hh (revision 7049)
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_dual_link.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_dual_link.world 2008-10-02 00:43:55 UTC (rev 4917)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_dual_link.world 2008-10-02 02:26:26 UTC (rev 4918)
@@ -38,26 +38,12 @@
<size>1024 800</size>
<pos>0 0</pos>
<frames>
- <row height="50%">
- <camera width="50%">
+ <row height="100%">
+ <camera width="100%">
<xyz> .25 1.5 1.0</xyz>
<rpy> 0 0 -90</rpy>
</camera>
- <camera width="50%">
- <xyz>.25 0 2.5</xyz>
- <rpy> 0 90 0</rpy>
- </camera>
</row>
- <row height="50%">
- <camera width="50%">
- <xyz>-1 0 1</xyz>
- <rpy> 0 0 0</rpy>
- </camera>
- <camera width="50%">
- <xyz>1.5 0 1</xyz>
- <rpy>0 0 180</rpy>
- </camera>
- </row>
</frames>
</rendering:gui>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/gazebo/gazebo_joints_dual_link.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/gazebo/gazebo_joints_dual_link.xml 2008-10-02 00:43:55 UTC (rev 4917)
+++ pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/gazebo/gazebo_joints_dual_link.xml 2008-10-02 02:26:26 UTC (rev 4918)
@@ -3,6 +3,6 @@
<explicitDampingCoefficient>1</explicitDampingCoefficient>
</joint>
<joint name="link2_joint" >
- <explicitDampingCoefficient>1</explicitDampingCoefficient>
+ <explicitDampingCoefficient>0</explicitDampingCoefficient>
</joint>
</robot>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/pr2_dual_link.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/pr2_dual_link.xml 2008-10-02 00:43:55 UTC (rev 4917)
+++ pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/pr2_dual_link.xml 2008-10-02 02:26:26 UTC (rev 4918)
@@ -13,7 +13,7 @@
<!-- joint blocks -->
<joint name="link2_joint" type="revolute" >
<axis xyz="0 1 0" />
- <anchor xyz="0 0 0" />
+ <anchor xyz="-0.5 0 0" />
<limit min="-3.14159" max="3.14159" effort="100" velocity="5" />
<calibration values="1.5 -1 " />
</joint>
@@ -81,15 +81,15 @@
<link name="link2">
<parent name="link1" />
- <origin xyz="0 0 0" rpy="0 0 0" />
+ <origin xyz="0.5 0 0" rpy="0 0 0" />
<joint name="link2_joint" />
<inertial >
<mass value="10" />
- <com xyz="0.0 0 0" />
- <inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1" />
+ <com xyz="0 0 0" />
+ <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0" />
</inertial>
<visual >
- <origin xyz="0.5 0 0" rpy="0 0 0" />
+ <origin xyz="0.0 0 0" rpy="0 0 0" />
<map name="gazebo_material" flag="gazebo">
<elem key="material">Gazebo/Blue</elem>
</map>
@@ -98,7 +98,7 @@
</geometry>
</visual>
<collision >
- <origin xyz="0.5 0 0" rpy="0.0 0.0 0.0 " />
+ <origin xyz="0.0 0 0" rpy="0.0 0.0 0.0 " />
<geometry name="link2_collision_geom">
<box size="1.0 0.1 0.1" />
</geometry>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml 2008-10-02 00:43:55 UTC (rev 4917)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml 2008-10-02 02:26:26 UTC (rev 4918)
@@ -10,63 +10,63 @@
</map>
<controller name="wheel_front_left_l_controller" topic="wheel_front_left_l_controller" type="JointVelocityControllerNode">
<joint name="wheel_front_left_l_joint" >
- <pid p="2.0" d="0" i="0.2" iClamp="2" />
+ <pid p="0.5" d="0" i="0.2" iClamp="2" />
</joint>
</controller>
<controller name="wheel_front_left_r_controller" topic="wheel_front_left_r_controller" type="JointVelocityControllerNode">
<joint name="wheel_front_left_r_joint" >
- <pid p="2.0" d="0" i="0.2" iClamp="2" />
+ <pid p="0.5" d="0" i="0.2" iClamp="2" />
</joint>
</controller>
<controller name="wheel_front_right_l_controller" topic="wheel_front_right_l_controller" type="JointVelocityControllerNode">
<joint name="wheel_front_right_l_joint" >
- <pid p="2.0" d="0" i="0.2" iClamp="2" />
+ <pid p="0.5" d="0" i="0.2" iClamp="2" />
</joint>
</controller>
<controller name="wheel_front_right_r_controller" topic="wheel_front_right_r_controller" type="JointVelocityControllerNode">
<joint name="wheel_front_right_r_joint" >
- <pid p="2.0" d="0" i="0.2" iClamp="2" />
+ <pid p="0.5" d="0" i="0.2" iClamp="2" />
</joint>
</controller>
<controller name="wheel_rear_left_l_controller" topic="wheel_rear_left_l_controller" type="JointVelocityControllerNode">
<joint name="wheel_rear_left_l_joint" >
- <pid p="2.0" d="0" i="0.2" iClamp="2" />
+ <pid p="0.5" d="0" i="0.2" iClamp="2" />
</joint>
</controller>
<controller name="wheel_rear_left_r_controller" topic="wheel_rear_left_r_controller" type="JointVelocityControllerNode">
<joint name="wheel_rear_left_r_joint" >
- <pid p="2.0" d="0" i="0.2" iClamp="2" />
+ <pid p="0.5" d="0" i="0.2" iClamp="2" />
</joint>
</controller>
<controller name="wheel_rear_right_l_controller" topic="wheel_rear_right_l_controller" type="JointVelocityControllerNode">
<joint name="wheel_rear_right_l_joint" >
- <pid p="2.0" d="0" i="0.2" iClamp="2" />
+ <pid p="0.5" d="0" i="0.2" iClamp="2" />
</joint>
</controller>
<controller name="wheel_rear_right_r_controller" topic="wheel_rear_right_r_controller" type="JointVelocityControllerNode">
<joint name="wheel_rear_right_r_joint" >
- <pid p="2.0" d="0" i="0.2" iClamp="2" />
+ <pid p="0.5" d="0" i="0.2" iClamp="2" />
</joint>
</controller>
<controller name="caster_front_left_controller" topic="caster_front_left_controller" type="JointVelocityControllerNode">
<joint name="caster_front_left_joint" >
- <pid p="3" d="0" i="0.1" iClamp="4" />
+ <pid p="0.1" d="0.05" i="0.05" iClamp="0.5" />
</joint>
</controller>
<controller name="caster_front_right_controller" topic="caster_front_right_controller" type="JointVelocityControllerNode">
<joint name="caster_front_right_joint" >
- <pid p="3" d="0" i="0.1" iClamp="4" />
+ <pid p="0.1" d="0.05" i="0.05" iClamp="0.5" />
</joint>
</controller>
<controller name="caster_rear_left_controller" topic="caster_rear_left_controller" type="JointVelocityControllerNode">
<joint name="caster_rear_left_joint" >
- <pid p="3" d="0" i="0.1" iClamp="4" />
+ <pid p="0.1" d="0.05" i="0.05" iClamp="0.5" />
</joint>
</controller>
<controller name="caster_rear_right_controller" topic="caster_rear_right_controller" type="JointVelocityControllerNode">
<joint name="caster_rear_right_joint" >
- <pid p="3" d="0" i="0.1" iClamp="4" />
+ <pid p="0.1" d="0.05" i="0.05" iClamp="0.5" />
</joint>
</controller>
@@ -130,8 +130,8 @@
<controller name="gripper_left_controller" topic="gripper_left_controller" type="JointPositionControllerNode">
<listen_topic name="left_gripper_commands" />
<joint name="finger_l_left_joint">
- <pid p="10" d="0" i="0" iClamp="0" />
- <gripper_defaults effortLimit="100" velocityLimit="10" />
+ <pid p="100" d="10" i="1" iClamp="10" />
+ <gripper_defaults effortLimit="1000" velocityLimit="10" />
</joint>
</controller>
<!-- ========================================= -->
@@ -184,8 +184,8 @@
<controller name="gripper_right_controller" topic="gripper_right_controller" type="JointPositionControllerNode">
<listen_topic name="right_gripper_commands" />
<joint name="finger_l_right_joint">
- <pid p="10" d="0" i="0" iClamp="0" />
- <gripper_defaults effortLimit="100" velocityLimit="10" />
+ <pid p="100" d="10" i="1" iClamp="10" />
+ <gripper_defaults effortLimit="1000" velocityLimit="10" />
</joint>
</controller>
<!-- head and above array -->
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/gazebo_joints.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/gazebo_joints.xml 2008-10-02 00:43:55 UTC (rev 4917)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/gazebo_joints.xml 2008-10-02 02:26:26 UTC (rev 4918)
@@ -62,16 +62,16 @@
</joint>
<!-- Special gripper joint -->
<joint name="finger_l_left_joint" gripper_controller="gripper_left_controller" abstract="gripper" effortLimit="1000" velocityLimit="100">
- <explicitDampingCoefficient>0.1</explicitDampingCoefficient>
+ <explicitDampingCoefficient>10</explicitDampingCoefficient>
</joint>
<joint name="finger_r_left_joint" gripper_controller="gripper_left_controller" abstract="gripper" effortLimit="1000" velocityLimit="100">
- <explicitDampingCoefficient>0.1</explicitDampingCoefficient>
+ <explicitDampingCoefficient>10</explicitDampingCoefficient>
</joint>
<joint name="finger_tip_l_left_joint" gripper_controller="gripper_left_controller" abstract="gripper" effortLimit="1000" velocityLimit="100">
- <explicitDampingCoefficient>0.1</explicitDampingCoefficient>
+ <explicitDampingCoefficient>10</explicitDampingCoefficient>
</joint>
<joint name="finger_tip_r_left_joint" gripper_controller="gripper_left_controller" abstract="gripper" effortLimit="1000" velocityLimit="100">
- <explicitDampingCoefficient>0.1</explicitDampingCoefficient>
+ <explicitDampingCoefficient>10</explicitDampingCoefficient>
</joint>
<!-- ========================================= -->
<!-- right arm array -->
@@ -98,16 +98,16 @@
</joint>
<!-- Special gripper joint -->
<joint name="finger_l_right_joint" gripper_controller="gripper_right_controller" abstract="gripper" effortLimit="1000" velocityLimit="100">
- <explicitDampingCoefficient>0.1</explicitDampingCoefficient>
+ <explicitDampingCoefficient>10</explicitDampingCoefficient>
</joint>
<joint name="finger_r_right_joint" gripper_controller="gripper_right_controller" abstract="gripper" effortLimit="1000" velocityLimit="100">
- <explicitDampingCoefficient>0.1</explicitDampingCoefficient>
+ <explicitDampingCoefficient>10</explicitDampingCoefficient>
</joint>
<joint name="finger_tip_l_right_joint" gripper_controller="gripper_right_controller" abstract="gripper" effortLimit="1000" velocityLimit="100">
- <explicitDampingCoefficient>0.1</explicitDampingCoefficient>
+ <explicitDampingCoefficient>10</explicitDampingCoefficient>
</joint>
<joint name="finger_tip_r_right_joint" gripper_controller="gripper_right_controller" abstract="gripper" effortLimit="1000" velocityLimit="100">
- <explicitDampingCoefficient>0.1</explicitDampingCoefficient>
+ <explicitDampingCoefficient>10</explicitDampingCoefficient>
</joint>
<!-- head and above array -->
<joint name="head_pan_joint" >
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml 2008-10-02 00:43:55 UTC (rev 4917)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml 2008-10-02 02:26:26 UTC (rev 4918)
@@ -545,7 +545,7 @@
<const_block name="pr2_finger_l_joint">
<axis xyz=" 0 0 1 " /> <!-- direction of the joint in a global coordinate frame -->
<anchor xyz="0 0 0 " /> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
- <limit effort="10" velocity="5" />
+ <limit effort="1000" velocity="5" />
<calibration values="1.5 -1 " /> <!-- Calibration stop/flag indicating location and then direction -->
</const_block>
@@ -580,7 +580,7 @@
<const_block name="pr2_finger_tip_l_joint">
<axis xyz=" 0 0 1 " /> <!-- direction of the joint in a global coordinate frame -->
<anchor xyz="0 0 0 " /> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
- <limit effort="10" velocity="5" />
+ <limit effort="1000" velocity="5" />
<calibration values="1.5 -1 " /> <!-- Calibration stop/flag indicating location and then direction -->
</const_block>
@@ -615,7 +615,7 @@
<axis xyz=" 0 0 1 " /> <!-- direction of the joint in a global coordinate frame -->
<anchor xyz="0 0 0 " /> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
<calibration values="1.5 -1 " /> <!-- Calibration stop/flag indicating location and then direction -->
- <limit effort="10" velocity="10" min="-100" max="100" /><!-- FIXME -->
+ <limit effort="1000" velocity="10" min="-100" max="100" /><!-- FIXME -->
</const_block>
<const_block name="pr2_finger_r_inertial">
@@ -649,7 +649,7 @@
<axis xyz=" 0 0 1 " /> <!-- direction of the joint in a global coordinate frame -->
<anchor xyz="0 0 0 " /> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
<calibration values="1.5 -1 " /> <!-- Calibration stop/flag indicating location and then direction -->
- <limit effort="10" velocity="10" min="-100" max="100" /><!-- FIXME -->
+ <limit effort="1000" velocity="10" min="-100" max="100" /><!-- FIXME -->
</const_block>
<const_block name="pr2_finger_tip_r_inertial">
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rob...@us...> - 2008-10-02 06:40:24
|
Revision: 4921
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4921&view=rev
Author: rob_wheeler
Date: 2008-10-02 06:40:19 +0000 (Thu, 02 Oct 2008)
Log Message:
-----------
Allow unprogrammed boards to be used by pr2_etherCAT
Modified Paths:
--------------
pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h
pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp
pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp
Modified: pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h 2008-10-02 06:29:26 UTC (rev 4920)
+++ pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/ethercat_hardware.h 2008-10-02 06:40:19 UTC (rev 4921)
@@ -69,7 +69,7 @@
/*!
* \brief Initialize the EtherCAT Master Library.
*/
- void init(char *interface);
+ void init(char *interface, bool allow_unprogrammed);
/*!
* \brief Register actuators with mechanism control
Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp 2008-10-02 06:29:26 UTC (rev 4920)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/ethercat_hardware.cpp 2008-10-02 06:40:19 UTC (rev 4921)
@@ -70,7 +70,7 @@
return double(n.tv_nsec) / NSEC_PER_SEC + n.tv_sec;
}
-void EthercatHardware::init(char *interface)
+void EthercatHardware::init(char *interface, bool allow_unprogrammed)
{
ros::node *node = ros::node::instance();
@@ -137,7 +137,7 @@
for (unsigned int slave = 0, a = 0; slave < num_slaves_; ++slave)
{
- if (slaves_[slave]->initialize(hw_->actuators_[a]) < 0)
+ if (slaves_[slave]->initialize(hw_->actuators_[a], allow_unprogrammed) < 0)
{
node->log(ros::FATAL, "Unable to initialize slave #%d", slave);
}
Modified: pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp
===================================================================
--- pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp 2008-10-02 06:29:26 UTC (rev 4920)
+++ pkg/trunk/robot_control_loops/pr2_etherCAT/src/main.cpp 2008-10-02 06:40:19 UTC (rev 4921)
@@ -53,6 +53,7 @@
char *interface_;
char *xml_;
bool allow_override_;
+ bool allow_unprogrammed_;
bool quiet_;
} g_options;
@@ -136,7 +137,7 @@
// Initialize the hardware interface
EthercatHardware ec;
- ec.init(g_options.interface_);
+ ec.init(g_options.interface_, g_options.allow_unprogrammed_);
// Create mechanism control
MechanismControl mc(ec.hw_);
@@ -281,6 +282,7 @@
static struct option long_options[] = {
{"help", no_argument, 0, 'h'},
{"allow_override", no_argument, 0, 'a'},
+ {"allow_unprogrammed", no_argument, 0, 'u'},
{"quiet", no_argument, 0, 'q'},
{"interface", required_argument, 0, 'i'},
{"xml", required_argument, 0, 'x'},
@@ -296,6 +298,9 @@
case 'a':
g_options.allow_override_ = 1;
break;
+ case 'u':
+ g_options.allow_unprogrammed_ = 1;
+ break;
case 'q':
g_options.quiet_ = 1;
break;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-10-02 07:54:49
|
Revision: 4922
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4922&view=rev
Author: hsujohnhsu
Date: 2008-10-02 07:54:39 +0000 (Thu, 02 Oct 2008)
Log Message:
-----------
proper mass manipulation working, anchor offset was double counted due to recent updates in Gazebo.
returned damping and gains for caster and gripper to pre r4918.
dual link case working.
Modified Paths:
--------------
pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
pkg/trunk/robot_descriptions/gazebo_robot_description/src/urdf2gazebo.cpp
pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/pr2_dual_link.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/gazebo_joints.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
Modified: pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
===================================================================
--- pkg/trunk/3rdparty/gazebo/gazebo_patch.diff 2008-10-02 06:40:19 UTC (rev 4921)
+++ pkg/trunk/3rdparty/gazebo/gazebo_patch.diff 2008-10-02 07:54:39 UTC (rev 4922)
@@ -369,7 +369,7 @@
}
////////////////////////////////////////////////////////////////////////////////
-@@ -123,6 +144,56 @@
+@@ -123,6 +144,63 @@
this->massP->SetValue( 0.001 );
}
@@ -400,16 +400,23 @@
+ this->mass.c[0] = this->cx;
+ this->mass.c[1] = this->cy;
+ this->mass.c[2] = this->cz;
++
+ this->mass.I[0] = this->ixx;
+ this->mass.I[1] = this->ixy;
+ this->mass.I[2] = this->ixz;
++
+ this->mass.I[3] = this->ixy;
+ this->mass.I[4] = this->iyy;
+ this->mass.I[5] = this->iyz;
++
+ this->mass.I[6] = this->ixz;
+ this->mass.I[7] = this->iyz;
+ this->mass.I[8] = this->izz;
+
++ this->mass.I[9] = 1;
++ this->mass.I[10] = 1;
++ this->mass.I[11] = 1;
++
+ // std::cout << " c[0] " << this->mass.c[0] << std::endl;
+ // std::cout << " c[1] " << this->mass.c[1] << std::endl;
+ // std::cout << " c[2] " << this->mass.c[2] << std::endl;
@@ -426,7 +433,7 @@
this->contact->Load(node);
this->LoadChild(node);
-@@ -396,23 +467,25 @@
+@@ -396,23 +474,24 @@
Pose3d pose;
dQuaternion q;
dMatrix3 r;
@@ -446,7 +453,7 @@
- dQtoR(q,r);
+ dQtoR(q,r); // turn quaternion into rotation matrix
-
+-
+ // this->mass was init to zero at start,
+ // read user specified mass into this->dblMass and dMassAdd in this->mass
this->bodyMass = this->mass;
@@ -575,43 +582,59 @@
// Return the ID of this body
dBodyID Body::GetId() const
{
-@@ -505,8 +590,15 @@
- */
+@@ -506,7 +591,6 @@
void Body::UpdateCoM()
{
-+ // std::cout << " mass x " << this->mass.c[0]
-+ // << " y " << this->mass.c[1]
-+ // << " z " << this->mass.c[2]
-+ // << std::endl;
-+ // comPose.pos.x = this->mass.c[0];
-+ // comPose.pos.y = this->mass.c[1];
-+ // comPose.pos.z = this->mass.c[2];
-+
const dMass *lmass;
- Pose3d oldPose, newPose, pose;
std::map< std::string, Geom* >::iterator giter;
if (!this->bodyId)
-@@ -523,8 +615,10 @@
- dMassAdd( &this->mass, lmass );
+@@ -524,7 +608,13 @@
}
}
-+ return; // Stop pose update, we have full com xyz, I control
++ //return; // Stop pose update, we have full com xyz, I control
++
// Old pose for the CoM
-+ Pose3d oldPose, newPose, pose;
++ Pose3d oldPose, newPose, tmpPose;
++
++ // oldPose is the last comPose
++ // newPose is mass CoM
oldPose = this->comPose;
if (std::isnan(this->mass.c[0]))
-@@ -565,7 +659,7 @@
+@@ -546,19 +636,25 @@
+ {
+ if (giter->second->IsPlaceable())
+ {
++ // FOR GEOMS:
++ // get pose with comPose set to oldPose
+ this->comPose = oldPose;
+- pose = giter->second->GetPose();
++ tmpPose = giter->second->GetPose();
++
++ // get pose with comPose set to newPose
+ this->comPose = newPose;
+- giter->second->SetPose(pose, false);
++ giter->second->SetPose(tmpPose, false);
+ }
+ }
+
+
+- // Fixup the pose of the CoM (ODE body)
++ // FOR BODY: Fixup the pose of the CoM (ODE body)
++ // get pose with comPose set to oldPose
+ this->comPose = oldPose;
+- pose = this->GetPose();
++ tmpPose = this->GetPose();
++ // get pose with comPose set to newPose
this->comPose = newPose;
+- this->SetPose(pose);
++ this->SetPose(tmpPose);
- // My Cheap Hack, to put the center of mass at the origin
-- this->mass.c[0] = this->mass.c[1] = this->mass.c[2] = 0;
-+ //this->mass.c[0] = this->mass.c[1] = this->mass.c[2] = 0;
- // Set the mass matrix
- if (this->mass.mass > 0)
+ // Settle on the new CoM pose
Index: server/physics/ode/ODEPhysics.hh
===================================================================
--- server/physics/ode/ODEPhysics.hh (revision 7049)
@@ -1195,6 +1218,18 @@
this->RTTMode="PBuffer";
}
}
+Index: server/gui/StatusBar.cc
+===================================================================
+--- server/gui/StatusBar.cc (revision 7049)
++++ server/gui/StatusBar.cc (working copy)
+@@ -25,6 +25,7 @@
+ */
+
+ #include <stdio.h>
++#include <string.h>
+ #include <FL/Fl_Value_Output.H>
+ #include <FL/Fl_Output.H>
+ #include <FL/Fl_Button.H>
Index: server/Model.cc
===================================================================
--- server/Model.cc (revision 7049)
@@ -1266,18 +1301,6 @@
return this->UpdateChild();
}
-Index: server/gui/StatusBar.cc
-===================================================================
---- server/gui/StatusBar.cc (revision 7049)
-+++ server/gui/StatusBar.cc (working copy)
-@@ -25,6 +25,7 @@
- */
-
- #include <stdio.h>
-+#include <string.h>
- #include <FL/Fl_Value_Output.H>
- #include <FL/Fl_Output.H>
- #include <FL/Fl_Button.H>
Index: server/World.hh
===================================================================
--- server/World.hh (revision 7049)
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/src/urdf2gazebo.cpp
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/src/urdf2gazebo.cpp 2008-10-02 06:40:19 UTC (rev 4921)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/src/urdf2gazebo.cpp 2008-10-02 07:54:39 UTC (rev 4922)
@@ -315,7 +315,7 @@
for (int j = 0 ; j < 3 ; ++j)
{
// undo Gazebo's shift of object anchor to geom xyz, stay in body cs
- tmpAnchor[j] = (link->joint->anchor)[j] - 0.0*(link->inertial->com)[j] - 0.0*(link->collision->xyz)[j]; /// @todo compensate for gazebo's error. John is fixing this one
+ tmpAnchor[j] = (link->joint->anchor)[j] - 1.0*(link->inertial->com)[j] - 0.0*(link->collision->xyz)[j]; /// @todo compensate for gazebo's error. John is fixing this one
}
addKeyValue(joint, "anchorOffset", values2str(3, tmpAnchor));
Modified: pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/pr2_dual_link.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/pr2_dual_link.xml 2008-10-02 06:40:19 UTC (rev 4921)
+++ pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/pr2_dual_link.xml 2008-10-02 07:54:39 UTC (rev 4922)
@@ -13,7 +13,7 @@
<!-- joint blocks -->
<joint name="link2_joint" type="revolute" >
<axis xyz="0 1 0" />
- <anchor xyz="-0.5 0 0" />
+ <anchor xyz="0.0 0 0" />
<limit min="-3.14159" max="3.14159" effort="100" velocity="5" />
<calibration values="1.5 -1 " />
</joint>
@@ -81,15 +81,15 @@
<link name="link2">
<parent name="link1" />
- <origin xyz="0.5 0 0" rpy="0 0 0" />
+ <origin xyz="0.0 0 0" rpy="0 0 0" />
<joint name="link2_joint" />
<inertial >
<mass value="10" />
- <com xyz="0 0 0" />
- <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0" />
+ <com xyz="1.0 0 0" />
+ <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="100.0" iyz="0.0" izz="1.0" />
</inertial>
<visual >
- <origin xyz="0.0 0 0" rpy="0 0 0" />
+ <origin xyz="0.5 0 0" rpy="0 0 0" />
<map name="gazebo_material" flag="gazebo">
<elem key="material">Gazebo/Blue</elem>
</map>
@@ -98,7 +98,7 @@
</geometry>
</visual>
<collision >
- <origin xyz="0.0 0 0" rpy="0.0 0.0 0.0 " />
+ <origin xyz="0.5 0 0" rpy="0.0 0.0 0.0 " />
<geometry name="link2_collision_geom">
<box size="1.0 0.1 0.1" />
</geometry>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml 2008-10-02 06:40:19 UTC (rev 4921)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml 2008-10-02 07:54:39 UTC (rev 4922)
@@ -10,63 +10,63 @@
</map>
<controller name="wheel_front_left_l_controller" topic="wheel_front_left_l_controller" type="JointVelocityControllerNode">
<joint name="wheel_front_left_l_joint" >
- <pid p="0.5" d="0" i="0.2" iClamp="2" />
+ <pid p="2.0" d="0" i="0.2" iClamp="2" />
</joint>
</controller>
<controller name="wheel_front_left_r_controller" topic="wheel_front_left_r_controller" type="JointVelocityControllerNode">
<joint name="wheel_front_left_r_joint" >
- <pid p="0.5" d="0" i="0.2" iClamp="2" />
+ <pid p="2.0" d="0" i="0.2" iClamp="2" />
</joint>
</controller>
<controller name="wheel_front_right_l_controller" topic="wheel_front_right_l_controller" type="JointVelocityControllerNode">
<joint name="wheel_front_right_l_joint" >
- <pid p="0.5" d="0" i="0.2" iClamp="2" />
+ <pid p="2.0" d="0" i="0.2" iClamp="2" />
</joint>
</controller>
<controller name="wheel_front_right_r_controller" topic="wheel_front_right_r_controller" type="JointVelocityControllerNode">
<joint name="wheel_front_right_r_joint" >
- <pid p="0.5" d="0" i="0.2" iClamp="2" />
+ <pid p="2.0" d="0" i="0.2" iClamp="2" />
</joint>
</controller>
<controller name="wheel_rear_left_l_controller" topic="wheel_rear_left_l_controller" type="JointVelocityControllerNode">
<joint name="wheel_rear_left_l_joint" >
- <pid p="0.5" d="0" i="0.2" iClamp="2" />
+ <pid p="2.0" d="0" i="0.2" iClamp="2" />
</joint>
</controller>
<controller name="wheel_rear_left_r_controller" topic="wheel_rear_left_r_controller" type="JointVelocityControllerNode">
<joint name="wheel_rear_left_r_joint" >
- <pid p="0.5" d="0" i="0.2" iClamp="2" />
+ <pid p="2.0" d="0" i="0.2" iClamp="2" />
</joint>
</controller>
<controller name="wheel_rear_right_l_controller" topic="wheel_rear_right_l_controller" type="JointVelocityControllerNode">
<joint name="wheel_rear_right_l_joint" >
- <pid p="0.5" d="0" i="0.2" iClamp="2" />
+ <pid p="2.0" d="0" i="0.2" iClamp="2" />
</joint>
</controller>
<controller name="wheel_rear_right_r_controller" topic="wheel_rear_right_r_controller" type="JointVelocityControllerNode">
<joint name="wheel_rear_right_r_joint" >
- <pid p="0.5" d="0" i="0.2" iClamp="2" />
+ <pid p="2.0" d="0" i="0.2" iClamp="2" />
</joint>
</controller>
<controller name="caster_front_left_controller" topic="caster_front_left_controller" type="JointVelocityControllerNode">
<joint name="caster_front_left_joint" >
- <pid p="0.1" d="0.05" i="0.05" iClamp="0.5" />
+ <pid p="3" d="0" i="0.1" iClamp="4" />
</joint>
</controller>
<controller name="caster_front_right_controller" topic="caster_front_right_controller" type="JointVelocityControllerNode">
<joint name="caster_front_right_joint" >
- <pid p="0.1" d="0.05" i="0.05" iClamp="0.5" />
+ <pid p="3" d="0" i="0.1" iClamp="4" />
</joint>
</controller>
<controller name="caster_rear_left_controller" topic="caster_rear_left_controller" type="JointVelocityControllerNode">
<joint name="caster_rear_left_joint" >
- <pid p="0.1" d="0.05" i="0.05" iClamp="0.5" />
+ <pid p="3" d="0" i="0.1" iClamp="4" />
</joint>
</controller>
<controller name="caster_rear_right_controller" topic="caster_rear_right_controller" type="JointVelocityControllerNode">
<joint name="caster_rear_right_joint" >
- <pid p="0.1" d="0.05" i="0.05" iClamp="0.5" />
+ <pid p="3" d="0" i="0.1" iClamp="4" />
</joint>
</controller>
@@ -130,8 +130,8 @@
<controller name="gripper_left_controller" topic="gripper_left_controller" type="JointPositionControllerNode">
<listen_topic name="left_gripper_commands" />
<joint name="finger_l_left_joint">
- <pid p="100" d="10" i="1" iClamp="10" />
- <gripper_defaults effortLimit="1000" velocityLimit="10" />
+ <pid p="10" d="0" i="0" iClamp="0" />
+ <gripper_defaults effortLimit="100" velocityLimit="10" />
</joint>
</controller>
<!-- ========================================= -->
@@ -184,8 +184,8 @@
<controller name="gripper_right_controller" topic="gripper_right_controller" type="JointPositionControllerNode">
<listen_topic name="right_gripper_commands" />
<joint name="finger_l_right_joint">
- <pid p="100" d="10" i="1" iClamp="10" />
- <gripper_defaults effortLimit="1000" velocityLimit="10" />
+ <pid p="10" d="0" i="0" iClamp="0" />
+ <gripper_defaults effortLimit="100" velocityLimit="10" />
</joint>
</controller>
<!-- head and above array -->
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/gazebo_joints.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/gazebo_joints.xml 2008-10-02 06:40:19 UTC (rev 4921)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/gazebo_joints.xml 2008-10-02 07:54:39 UTC (rev 4922)
@@ -62,16 +62,16 @@
</joint>
<!-- Special gripper joint -->
<joint name="finger_l_left_joint" gripper_controller="gripper_left_controller" abstract="gripper" effortLimit="1000" velocityLimit="100">
- <explicitDampingCoefficient>10</explicitDampingCoefficient>
+ <explicitDampingCoefficient>0.1</explicitDampingCoefficient>
</joint>
<joint name="finger_r_left_joint" gripper_controller="gripper_left_controller" abstract="gripper" effortLimit="1000" velocityLimit="100">
- <explicitDampingCoefficient>10</explicitDampingCoefficient>
+ <explicitDampingCoefficient>0.1</explicitDampingCoefficient>
</joint>
<joint name="finger_tip_l_left_joint" gripper_controller="gripper_left_controller" abstract="gripper" effortLimit="1000" velocityLimit="100">
- <explicitDampingCoefficient>10</explicitDampingCoefficient>
+ <explicitDampingCoefficient>0.1</explicitDampingCoefficient>
</joint>
<joint name="finger_tip_r_left_joint" gripper_controller="gripper_left_controller" abstract="gripper" effortLimit="1000" velocityLimit="100">
- <explicitDampingCoefficient>10</explicitDampingCoefficient>
+ <explicitDampingCoefficient>0.1</explicitDampingCoefficient>
</joint>
<!-- ========================================= -->
<!-- right arm array -->
@@ -98,16 +98,16 @@
</joint>
<!-- Special gripper joint -->
<joint name="finger_l_right_joint" gripper_controller="gripper_right_controller" abstract="gripper" effortLimit="1000" velocityLimit="100">
- <explicitDampingCoefficient>10</explicitDampingCoefficient>
+ <explicitDampingCoefficient>0.1</explicitDampingCoefficient>
</joint>
<joint name="finger_r_right_joint" gripper_controller="gripper_right_controller" abstract="gripper" effortLimit="1000" velocityLimit="100">
- <explicitDampingCoefficient>10</explicitDampingCoefficient>
+ <explicitDampingCoefficient>0.1</explicitDampingCoefficient>
</joint>
<joint name="finger_tip_l_right_joint" gripper_controller="gripper_right_controller" abstract="gripper" effortLimit="1000" velocityLimit="100">
- <explicitDampingCoefficient>10</explicitDampingCoefficient>
+ <explicitDampingCoefficient>0.1</explicitDampingCoefficient>
</joint>
<joint name="finger_tip_r_right_joint" gripper_controller="gripper_right_controller" abstract="gripper" effortLimit="1000" velocityLimit="100">
- <explicitDampingCoefficient>10</explicitDampingCoefficient>
+ <explicitDampingCoefficient>0.1</explicitDampingCoefficient>
</joint>
<!-- head and above array -->
<joint name="head_pan_joint" >
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml 2008-10-02 06:40:19 UTC (rev 4921)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml 2008-10-02 07:54:39 UTC (rev 4922)
@@ -360,7 +360,7 @@
</const_block>
<const_block name="pr2_shoulder_pitch_collision">
- <origin xyz="shoulder_pitch_length/2 0 0 " rpy="M_PI/2 0.0 0.0 " /> <!-- location defined with respect to the link origin in a local coordinate frame -->
+ <origin xyz="0 0 0" rpy="M_PI/2 0.0 0.0 " /> <!-- location defined with respect to the link origin in a local coordinate frame -->
<!-- All angles always in radians, yaw about Z axis, pitch about the Y axis and roll about the X axis -->
<geometry name="pr2_shoulder_pitch_collision_geom">
<cylinder radius="shoulder_pitch_radius" length="shoulder_pitch_length" />
@@ -545,7 +545,7 @@
<const_block name="pr2_finger_l_joint">
<axis xyz=" 0 0 1 " /> <!-- direction of the joint in a global coordinate frame -->
<anchor xyz="0 0 0 " /> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
- <limit effort="1000" velocity="5" />
+ <limit effort="10" velocity="5" />
<calibration values="1.5 -1 " /> <!-- Calibration stop/flag indicating location and then direction -->
</const_block>
@@ -580,7 +580,7 @@
<const_block name="pr2_finger_tip_l_joint">
<axis xyz=" 0 0 1 " /> <!-- direction of the joint in a global coordinate frame -->
<anchor xyz="0 0 0 " /> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
- <limit effort="1000" velocity="5" />
+ <limit effort="10" velocity="5" />
<calibration values="1.5 -1 " /> <!-- Calibration stop/flag indicating location and then direction -->
</const_block>
@@ -615,7 +615,7 @@
<axis xyz=" 0 0 1 " /> <!-- direction of the joint in a global coordinate frame -->
<anchor xyz="0 0 0 " /> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
<calibration values="1.5 -1 " /> <!-- Calibration stop/flag indicating location and then direction -->
- <limit effort="1000" velocity="10" min="-100" max="100" /><!-- FIXME -->
+ <limit effort="10" velocity="10" min="-100" max="100" /><!-- FIXME -->
</const_block>
<const_block name="pr2_finger_r_inertial">
@@ -649,7 +649,7 @@
<axis xyz=" 0 0 1 " /> <!-- direction of the joint in a global coordinate frame -->
<anchor xyz="0 0 0 " /> <!-- point on the joint relative to the parent's anchor in a global coordinate frame -->
<calibration values="1.5 -1 " /> <!-- Calibration stop/flag indicating location and then direction -->
- <limit effort="1000" velocity="10" min="-100" max="100" /><!-- FIXME -->
+ <limit effort="10" velocity="10" min="-100" max="100" /><!-- FIXME -->
</const_block>
<const_block name="pr2_finger_tip_r_inertial">
@@ -1206,7 +1206,7 @@
<link name="shoulder_pitch_left">
<parent name=" shoulder_pan_left" />
<!-- rotation of a local coordinate frame attached to the link with respect to the parent's frame -->
- <origin xyz=" shoulder_pan_shoulder_pitch_offset_x 0 0 " rpy=" 0 0 0 " /> <!-- position of a local coordinate frame attached to the link with respect to parent's frame-->
+ <origin xyz="shoulder_pan_shoulder_pitch_offset_x 0 0 " rpy=" 0 0 0 " /> <!-- position of a local coordinate frame attached to the link with respect to parent's frame-->
<joint name="shoulder_pitch_left_joint" />
<inertial >
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2008-10-02 21:45:59
|
Revision: 4957
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4957&view=rev
Author: stuglaser
Date: 2008-10-02 21:45:49 +0000 (Thu, 02 Oct 2008)
Log Message:
-----------
Completed FK and ported mechanism over to the new tf package.
Modified Paths:
--------------
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_effort_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_effort_controller.cpp
pkg/trunk/mechanism/mechanism_control/manifest.xml
pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/joint.h
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/link.h
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/robot.h
pkg/trunk/mechanism/mechanism_model/manifest.xml
pkg/trunk/mechanism/mechanism_model/src/joint.cpp
pkg/trunk/mechanism/mechanism_model/src/link.cpp
pkg/trunk/mechanism/mechanism_model/src/robot.cpp
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_effort_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_effort_controller.h 2008-10-02 21:45:07 UTC (rev 4956)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_effort_controller.h 2008-10-02 21:45:49 UTC (rev 4957)
@@ -46,7 +46,7 @@
#include "ros/node.h"
#include "robot_mechanism_controllers/SetVectorCommand.h"
#include "mechanism_model/controller.h"
-#include "LinearMath/btVector3.h"
+#include "tf/transform_datatypes.h"
#include "misc_utils/advertised_service_guard.h"
namespace controller {
@@ -60,9 +60,9 @@
bool initXml(mechanism::RobotState *robot, TiXmlElement *config);
void update();
- btVector3 command_;
+ tf::Vector3 command_;
- btVector3 offset_;
+ tf::Vector3 offset_;
std::vector<mechanism::LinkState*> links_; // root to tip
std::vector<mechanism::JointState*> joints_; // root to tip, 1 element smaller than links_
};
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_effort_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_effort_controller.cpp 2008-10-02 21:45:07 UTC (rev 4956)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_effort_controller.cpp 2008-10-02 21:45:49 UTC (rev 4957)
@@ -130,19 +130,15 @@
void CartesianEffortController::update()
{
- btVector3 F = command_; // force vector, will be transformed to the current link's frame
+ tf::Vector3 F = command_; // force vector, will be transformed to the current link's frame
- libTF::Vector tempF(F.x(), F.y(), F.z());
for (unsigned int i = 1; i < links_.size(); ++i)
{
- libTF::Pose3D transform(links_[i]->rel_frame_);
- transform.invert();
- transform.applyToVector(tempF);
+ F = links_[i]->rel_frame_.getBasis().transpose() * F;
}
- F.setValue(tempF.x, tempF.y, tempF.z);
// At this point, F is the desired force in the current link's frame
- btVector3 r(offset_); // position of the force in the current frame
+ tf::Vector3 r(offset_); // position of the force in the current frame
for (int i = links_.size() - 2; i >= 0; --i)
{
@@ -152,11 +148,8 @@
{
case mechanism::JOINT_ROTARY:
case mechanism::JOINT_CONTINUOUS: {
- btVector3 torque = cross(r, F);
- btVector3 axis(joints_[i]->joint_->axis_[0],
- joints_[i]->joint_->axis_[1],
- joints_[i]->joint_->axis_[2]);
- joints_[i]->commanded_effort_ = torque.dot(axis);
+ tf::Vector3 torque = cross(r, F);
+ joints_[i]->commanded_effort_ = torque.dot(joints_[i]->joint_->axis_);
// Propagate back to link i
break;
@@ -168,18 +161,9 @@
abort();
}
- // Transforms the force to the previous link's coordinate frame
- libTF::Pose3D transform(links_[i+1]->rel_frame_);
-
- // Temporarily wraps r and F (until the new tf package is ready).
- libTF::Position tempR(r.x(), r.y(), r.z());
- libTF::Vector tempF(F.x(), F.y(), F.z());
-
- transform.applyToPosition(tempR);
- transform.applyToVector(tempF);
-
- r.setValue(tempR.x, tempR.y, tempR.z);
- F.setValue(tempF.x, tempF.y, tempF.z);
+ // Transforms the effort to the parent link's coordinate frame
+ r = links_[i+1]->rel_frame_(r);
+ F = links_[i+1]->rel_frame_.getBasis() * F;
}
}
Modified: pkg/trunk/mechanism/mechanism_control/manifest.xml
===================================================================
--- pkg/trunk/mechanism/mechanism_control/manifest.xml 2008-10-02 21:45:07 UTC (rev 4956)
+++ pkg/trunk/mechanism/mechanism_control/manifest.xml 2008-10-02 21:45:49 UTC (rev 4957)
@@ -11,7 +11,7 @@
<depend package="mechanism_model"/>
<depend package="misc_utils" />
<depend package="rospy" />
-<depend package="rosTF" />
+<depend package="tf" />
<export>
<cpp cflags='-I${prefix}/include -I${prefix}/msg/cpp -I${prefix}/srv/cpp'
lflags='-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lmechanism_control'/>
Modified: pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-10-02 21:45:07 UTC (rev 4956)
+++ pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-10-02 21:45:49 UTC (rev 4957)
@@ -30,7 +30,6 @@
#include "mechanism_control/mechanism_control.h"
#include "rosthread/member_thread.h"
-#include "rosTF/rosTF.h"
using namespace mechanism;
@@ -295,22 +294,20 @@
if (mc_->model_.links_[i]->parent_name_ == std::string("world"))
continue;
- libTF::Position pos;
- libTF::Quaternion quat;
- mc_->state_->link_states_[i].rel_frame_.getPosition(pos);
- mc_->state_->link_states_[i].rel_frame_.getQuaternion(quat);
+ tf::Vector3 pos = mc_->state_->link_states_[i].rel_frame_.getOrigin();
+ tf::Quaternion quat = mc_->state_->link_states_[i].rel_frame_.getRotation();
rosTF::TransformQuaternion &out = transform_publisher_.msg_.quaternions[i];
out.header.stamp.from_double(mc_->hw_->current_time_);
out.header.frame_id = mc_->model_.links_[i]->name_;
out.parent = mc_->model_.links_[i]->parent_name_;
- out.xt = pos.x;
- out.yt = pos.y;
- out.zt = pos.z;
- out.w = quat.w;
- out.xr = quat.x;
- out.yr = quat.y;
- out.zr = quat.z;
+ out.xt = pos.x();
+ out.yt = pos.y();
+ out.zt = pos.z();
+ out.w = quat.w();
+ out.xr = quat.x();
+ out.yr = quat.y();
+ out.zr = quat.z();
}
transform_publisher_.unlockAndPublish();
Modified: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/joint.h
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/joint.h 2008-10-02 21:45:07 UTC (rev 4956)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/joint.h 2008-10-02 21:45:49 UTC (rev 4957)
@@ -38,6 +38,7 @@
#define JOINT_H
#include <tinyxml/tinyxml.h>
+#include "tf/tf.h"
namespace mechanism {
@@ -78,7 +79,7 @@
// Axis of motion (x,y,z). Axis of rotation for revolute/continuous
// joints, axis of translation for prismatic joints.
- double axis_[3];
+ tf::Vector3 axis_;
};
@@ -97,6 +98,14 @@
bool calibrated_;
+ tf::Vector3 getTranslation();
+ tf::Quaternion getRotation();
+ tf::Vector3 getTransVelocity();
+ tf::Vector3 getRotVelocity();
+
+ tf::Transform getTransform();
+
+
JointState() : joint_(NULL), commanded_effort_(0), calibrated_(false) {}
JointState(const JointState &s)
: joint_(s.joint_), position_(s.position_), velocity_(s.velocity_),
Modified: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/link.h
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/link.h 2008-10-02 21:45:07 UTC (rev 4956)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/link.h 2008-10-02 21:45:49 UTC (rev 4957)
@@ -37,7 +37,7 @@
#include "mechanism_model/joint.h"
#include <vector>
#include "libTF/Pose3D.h"
-#include "LinearMath/btVector3.h"
+#include "tf/transform_datatypes.h"
namespace mechanism {
@@ -56,8 +56,8 @@
std::string parent_name_;
std::string joint_name_;
- double origin_xyz_[3];
- double origin_rpy_[3];
+ tf::Vector3 origin_xyz_;
+ tf::Vector3 origin_rpy_;
};
class LinkState
@@ -65,10 +65,14 @@
public:
Link *link_;
- libTF::Pose3D rel_frame_; // Transformation relative to the parent's frame.
- btVector3 abs_position_; // Absolute position (in the robot frame)
- btVector3 abs_orientation_; // Absolute orientation (in the robot frame)
+ tf::Transform rel_frame_; // Relative transform to the parent link's frame.
+ tf::Vector3 abs_position_; // Absolute position (in the robot frame)
+ tf::Quaternion abs_orientation_; // Absolute orientation (in the robot frame)
+ tf::Vector3 abs_velocity_;
+ tf::Vector3 abs_rot_velocity_;
+ void propagateFK(LinkState *parent, JointState *joint);
+
LinkState() : link_(NULL) {}
LinkState(const LinkState &s) : link_(s.link_), rel_frame_(s.rel_frame_) {}
};
Modified: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/robot.h
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/robot.h 2008-10-02 21:45:07 UTC (rev 4956)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/robot.h 2008-10-02 21:45:49 UTC (rev 4957)
@@ -134,8 +134,7 @@
void propagateEffortBackwards();
private:
- void propagateAbsolutePose(int index, const libTF::Pose3D&);
-
+ void propagateAbsolutePose(int index);
};
}
Modified: pkg/trunk/mechanism/mechanism_model/manifest.xml
===================================================================
--- pkg/trunk/mechanism/mechanism_model/manifest.xml 2008-10-02 21:45:07 UTC (rev 4956)
+++ pkg/trunk/mechanism/mechanism_model/manifest.xml 2008-10-02 21:45:49 UTC (rev 4957)
@@ -10,7 +10,7 @@
<depend package="tinyxml" />
<depend package="wg_robot_description_parser" />
<depend package="kdl" />
-<depend package="libTF" />
+<depend package="tf" />
<depend package="control_toolbox" />
<depend package="bullet" />
<url>http://pr.willowgarage.com</url>
Modified: pkg/trunk/mechanism/mechanism_model/src/joint.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_model/src/joint.cpp 2008-10-02 21:45:07 UTC (rev 4956)
+++ pkg/trunk/mechanism/mechanism_model/src/joint.cpp 2008-10-02 21:45:49 UTC (rev 4957)
@@ -209,7 +209,69 @@
axis_[0] = axis_pieces[0];
axis_[1] = axis_pieces[1];
axis_[2] = axis_pieces[2];
+ axis_.normalize();
}
return true;
}
+tf::Vector3 JointState::getTranslation()
+{
+ switch (joint_->type_)
+ {
+ case JOINT_PRISMATIC:
+ return position_ * joint_->axis_;
+ default:
+ return tf::Vector3(0, 0, 0);
+ }
+}
+
+tf::Quaternion JointState::getRotation()
+{
+ switch (joint_->type_)
+ {
+ case JOINT_CONTINUOUS:
+ case JOINT_ROTARY:
+ return tf::Quaternion(joint_->axis_, position_);
+ default:
+ return tf::Quaternion(0, 0, 0, 1);
+ }
+}
+
+tf::Vector3 JointState::getTransVelocity()
+{
+ switch (joint_->type_)
+ {
+ case JOINT_PRISMATIC:
+ return velocity_ * joint_->axis_;
+ default:
+ return tf::Vector3(0, 0, 0);
+ }
+}
+
+tf::Vector3 JointState::getRotVelocity()
+{
+ switch (joint_->type_)
+ {
+ case JOINT_CONTINUOUS:
+ case JOINT_ROTARY:
+ return velocity_ * joint_->axis_;
+ default:
+ return tf::Vector3(0, 0, 0);
+ }
+}
+
+tf::Transform JointState::getTransform()
+{
+ switch (joint_->type_)
+ {
+ case JOINT_CONTINUOUS:
+ case JOINT_ROTARY:
+ return tf::Transform(getRotation());
+ case JOINT_PRISMATIC:
+ return tf::Transform(tf::Quaternion(0,0,0,1), getTranslation());
+ default:
+ tf::Transform t;
+ t.setIdentity();
+ return t;
+ }
+}
Modified: pkg/trunk/mechanism/mechanism_model/src/link.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_model/src/link.cpp 2008-10-02 21:45:07 UTC (rev 4956)
+++ pkg/trunk/mechanism/mechanism_model/src/link.cpp 2008-10-02 21:45:49 UTC (rev 4957)
@@ -116,4 +116,47 @@
return true;
}
+void LinkState::propagateFK(LinkState *p, JointState *j)
+{
+ if (p == NULL && j == NULL)
+ {
+ rel_frame_.setIdentity();
+ abs_position_.setValue(0, 0, 0);
+ abs_orientation_.setValue(0, 0, 0, 1);
+ abs_velocity_.setValue(0, 0, 0);
+ abs_rot_velocity_.setValue(0, 0, 0);
+ }
+ else
+ {
+ assert(p); assert(j);
+
+ abs_position_ =
+ p->abs_position_
+ + quatRotate(p->abs_orientation_, link_->origin_xyz_)
+ + j->getTranslation();
+
+ tf::Quaternion rel_or(link_->origin_rpy_[2], link_->origin_rpy_[1], link_->origin_rpy_[0]);
+ abs_orientation_ = p->abs_orientation_ * j->getRotation() * rel_or;
+ abs_orientation_.normalize();
+
+ abs_velocity_ =
+ p->abs_velocity_
+ + cross(p->abs_rot_velocity_, link_->origin_xyz_)
+ + j->getTransVelocity();
+
+ abs_rot_velocity_ = p->abs_rot_velocity_ + quatRotate(abs_orientation_, j->getRotVelocity());
+
+
+ // Computes the relative frame transform
+ rel_frame_.setIdentity();
+ rel_frame_ *= tf::Transform(tf::Quaternion(0,0,0), link_->origin_xyz_);
+ rel_frame_ *= j->getTransform();
+ rel_frame_ *= tf::Transform(tf::Quaternion(link_->origin_rpy_[2],
+ link_->origin_rpy_[1],
+ link_->origin_rpy_[0]));
+
+ tf::Vector3 jo = j->getTransform().getOrigin();
+ }
+}
+
} // namespace mechanism
Modified: pkg/trunk/mechanism/mechanism_model/src/robot.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_model/src/robot.cpp 2008-10-02 21:45:07 UTC (rev 4956)
+++ pkg/trunk/mechanism/mechanism_model/src/robot.cpp 2008-10-02 21:45:49 UTC (rev 4957)
@@ -197,6 +197,11 @@
links_parent_[i] = parent_index;
links_children_[parent_index].push_back(i);
}
+ else
+ {
+ links_joint_[i] = -1;
+ links_parent_[i] = -1;
+ }
}
}
@@ -221,58 +226,12 @@
transmissions_out_[i]);
}
- // Computes the frame transform for each link relative to its parent.
- for (unsigned int i = 0; i < link_states_.size(); ++i)
- {
- if (!links_joint_[i]) // Root link, attached to the world
- {
- link_states_[i].rel_frame_.setIdentity();
- }
- else
- {
- JointState &j = joint_states_[links_joint_[i]];
- Link &l = *link_states_[i].link_;
-
- libTF::Pose3D offset;
- libTF::Pose3D joint_transform;
- libTF::Pose3D rotation;
-
- offset.setPosition(l.origin_xyz_[0], l.origin_xyz_[1], l.origin_xyz_[2]);
-
- switch (j.joint_->type_)
- {
- case JOINT_ROTARY:
- case JOINT_CONTINUOUS:
- joint_transform.setAxisAngle(j.joint_->axis_, j.position_);
- break;
- case JOINT_PRISMATIC:
- joint_transform.setPosition(j.position_ * j.joint_->axis_[0],
- j.position_ * j.joint_->axis_[1],
- j.position_ * j.joint_->axis_[2]);
- break;
- case JOINT_FIXED:
- case JOINT_PLANAR:
- joint_transform.setIdentity();
- break;
- }
-
- rotation.setFromEuler(0,0,0, l.origin_rpy_[2], l.origin_rpy_[1], l.origin_rpy_[0]);
-
- link_states_[i].rel_frame_.setIdentity();
- link_states_[i].rel_frame_.multiplyPose(offset);
- link_states_[i].rel_frame_.multiplyPose(joint_transform);
- link_states_[i].rel_frame_.multiplyPose(rotation);
- }
- }
-
// Computes the absolute pose of the links using the relative transforms
for (unsigned int i = 0; i < link_states_.size(); ++i)
{
- if (!links_joint_[i]) // Root link, attached to the world
+ if (links_joint_[i] < 0) // Root link, attached to the world
{
- link_states_[i].abs_position_.setValue(0, 0, 0);
- link_states_[i].abs_orientation_.setValue(0, 0, 0);
- propagateAbsolutePose(i, libTF::Pose3D());
+ propagateAbsolutePose(i);
}
}
}
@@ -318,21 +277,14 @@
}
}
-void RobotState::propagateAbsolutePose(int index, const libTF::Pose3D &parent_transform)
+void RobotState::propagateAbsolutePose(int index)
{
- libTF::Pose3D my_transform(parent_transform);
- my_transform.multiplyPose(link_states_[index].rel_frame_);
+ LinkState *p = links_parent_[index] >= 0 ? &link_states_[links_parent_[index]] : NULL;
+ JointState *j = links_joint_[index] >= 0 ? &joint_states_[links_joint_[index]] : NULL;
+ link_states_[index].propagateFK(p, j);
- libTF::Position pos;
- libTF::Euler orient;
- my_transform.getPosition(pos);
- my_transform.getEuler(orient);
-
- link_states_[index].abs_position_.setValue(pos.x, pos.y, pos.z);
- link_states_[index].abs_orientation_.setValue(orient.roll, orient.pitch, orient.yaw);
-
for (unsigned int i = 0; i < links_children_[index].size(); ++i)
- propagateAbsolutePose(links_children_[index][i], my_transform);
+ propagateAbsolutePose(links_children_[index][i]);
}
} // namespace mechanism
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2008-10-03 05:32:57
|
Revision: 4988
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4988&view=rev
Author: gerkey
Date: 2008-10-03 05:32:51 +0000 (Fri, 03 Oct 2008)
Log Message:
-----------
moved teleop_base_overhead_cam to deprecated, because it's old and doesn't build
Added Paths:
-----------
pkg/trunk/deprecated/teleop_base_overhead_cam/
Removed Paths:
-------------
pkg/trunk/nav/teleop_base_overhead_cam/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-10-03 23:30:43
|
Revision: 5038
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5038&view=rev
Author: hsujohnhsu
Date: 2008-10-03 23:30:33 +0000 (Fri, 03 Oct 2008)
Log Message:
-----------
* renamed gazebo_actuators to gazebo_mechanism_control (GazeboActuators to GazeboMechanismControl)
* updated xml files according to above change in wg_robot_description
* updated gazebo_mechanism_control to read directly from pr2.xml on param server. @todo remove <robot filename="pr2.xml" /> tag.
* added rostests for camera, laser and collision into gazebo_plugin CMakeLists.txt
* update doxygen pages.
* removed wrist cameras from pr2.xml.
* disabled publish diagnostic information from gazebo_battery plugin.
Modified Paths:
--------------
pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt
pkg/trunk/drivers/simulator/gazebo_plugin/doc.dox
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Block_Laser.hh
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Camera.hh
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Laser.hh
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_PTZ.hh
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Time.hh
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_battery.h
pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/gazebo/controllers_gazebo_dual_link.xml
pkg/trunk/robot_descriptions/wg_robot_description/multi_link_test/gazebo/controllers_gazebo_multi_link.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/controllers_gazebo.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo/controllers_gazebo_headless.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm/pr2d2.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/gazebo/controllers_gazebo_arm.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/gazebo/controllers_gazebo_arm_headless.xml
pkg/trunk/robot_descriptions/wg_robot_description/single_link_test/gazebo/controllers_gazebo_single_link.xml
Added Paths:
-----------
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_mechanism_control.h
pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_mechanism_control.cpp
Removed Paths:
-------------
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_actuators.h
pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_xml_multi_link.model
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt 2008-10-03 23:09:54 UTC (rev 5037)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt 2008-10-03 23:30:33 UTC (rev 5038)
@@ -17,12 +17,14 @@
rospack_add_library(Ros_PTZ src/Ros_PTZ.cc)
rospack_add_library(P3D src/P3D.cc)
rospack_add_library(F3D src/F3D.cc)
-rospack_add_library(gazebo_actuators src/gazebo_actuators.cpp)
+rospack_add_library(gazebo_mechanism_control src/gazebo_mechanism_control.cpp)
rospack_add_library(gazebo_battery src/gazebo_battery.cpp)
# This target requires adding player to the manifest.xml.
#rospack_add_executable(player_pr2 src/player/Pr2_Player.cc)
#target_link_libraries(player_pr2 playerc++)
+rospack_add_rostest(test/testslide.xml)
+rospack_add_rostest(test/testcameras.xml)
+rospack_add_rostest(test/testscan.xml)
-
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/doc.dox
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/doc.dox 2008-10-03 23:09:54 UTC (rev 5037)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/doc.dox 2008-10-03 23:30:33 UTC (rev 5038)
@@ -4,7 +4,7 @@
@htmlinclude manifest.html
Here are the gazebo plugins which are currently implemented:
- \li \ref gazebo::GazeboActuators "ROS Mechanism Controls Plugin for Actuators"
+ \li \ref gazebo::GazeboMechanismControl "ROS Mechanism Controls Plugin for MechanismControl"
\li \ref gazebo::Ros_Time "ROS Time Plugin"
\li \ref gazebo::Ros_Camera "ROS Camera Plugin"
\li \ref gazebo::Ros_Laser "ROS Laser Scanner Plugin"
@@ -164,7 +164,7 @@
<include>actuators_single_link.xml</include>
<include>transmissions_single_link.xml</include>
- <!-- setup for GazeboActuators plugin -->
+ <!-- setup for GazeboMechanismControl plugin -->
<map name="transmissions_gazebo_actuators" flag="gazebo"> <!-- we can set a name too, but the convertor only cares about the flag -->
<verbatim key="transmissions_gazebo_actuators" includes="true"> <!-- The key attribute is needed noly if multiple <xml> tags are in the same <map> tag -->
<!-- PR2_ACTARRAY -->
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh 2008-10-03 23:09:54 UTC (rev 5037)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh 2008-10-03 23:30:33 UTC (rev 5038)
@@ -44,6 +44,7 @@
This controller requires to a model as its parent. The plugin broadcasts a body's pose and rates through ROS std_msgs::TransformWithRateStamped message. In the example below, the plubin broadcasts pose and rate of a body named \b body_name over ROS topic name \b body_pose_groud_truth.
+ \li Example Usage:
\verbatim
<model:physical name="some_fancy_model">
<controller:P3D name="p3d_controller" plugin="libP3D.so">
@@ -60,11 +61,28 @@
\endverbatim
\{
- */
/// \brief P3D controller
/// \li Starts a ROS node if none exists.
/// \li This controller simulates a 6 dof position and rate sensor, publishes std_msgs::TransformWithRateStamped.msg ROS topic.
+ /// .
+ \li Example Usage:
+ \verbatim
+ <model:physical name="some_fancy_model">
+ <controller:P3D name="p3d_controller" plugin="libP3D.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <bodyName>body_name</bodyName>
+ <topicName>body_pose_ground_truth</topicName>
+ <frameName>map</frameName>
+ <xyzOffsets>25.65 25.65 0</xyzOffsets> <!-- option to initialize odometry for fake localization-->
+ <rpyOffsets>0 0 0</rpyOffsets>
+ <interface:position name="p3d_position_iface"/>
+ </controller:P3D>
+ </model:phyiscal>
+ \endverbatim
+ */
+
class P3D : public Controller
{
/// \brief Constructor
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Block_Laser.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Block_Laser.hh 2008-10-03 23:09:54 UTC (rev 5037)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Block_Laser.hh 2008-10-03 23:30:33 UTC (rev 5038)
@@ -46,6 +46,7 @@
This is a controller that gathers range data from a ray sensor, and returns results via publishing ROS topic for point clouds.
+ \li Example Usage:
\verbatim
<model:physical name="ray_model">
<body:empty name="ray_body_name">
@@ -83,12 +84,50 @@
\endverbatim
\{
-*/
/// \brief ROS laser block simulation.
/// \li Starts a ROS node if none exists.
/// \li This controller simulates a block of laser range detections.
/// Resulting point cloud (std_msgs::PointCloudFloat32.msg) is published as a ROS topic.
+/// .
+ \li Example Usage:
+ \verbatim
+ <model:physical name="ray_model">
+ <body:empty name="ray_body_name">
+ <sensor:ray name="ray_sensor">
+ <rayCount>30</rayCount>
+ <rangeCount>30</rangeCount>
+ <laserCount>1</laserCount>
+
+ <origin>0.0 0.0 0.05</origin>
+ <displayRays>false</displayRays>
+
+ <minAngle>-15</minAngle>
+ <maxAngle> 15</maxAngle>
+
+ <minRange>0.05</minRange>
+ <maxRange>100.0</maxRange>
+ <updateRate>10.0</updateRate>
+
+ <verticalRayCount>30</verticalRayCount>
+ <verticalRangeCount>30</verticalRangeCount>
+ <verticalMinAngle>-20</verticalMinAngle>
+ <verticalMaxAngle> 0</verticalMaxAngle>
+
+ <controller:ros_block_laser name="ray_block_controller" plugin="libRos_Block_Laser.so">
+ <gaussianNoise>0.005</gaussianNoise>
+ <alwaysOn>true</alwaysOn>
+ <updateRate>10.0</updateRate>
+ <topicName>full_cloud</topicName>
+ <frameName>ray_model</frameName>
+ <interface:laser name="ray_block_iface" />
+ </controller:ros_block_laser>
+ </sensor:ray>
+ </body:empty>
+ </model:phyiscal>
+ \endverbatim
+*/
+
class Ros_Block_Laser : public Controller
{
/// \brief Constructor
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Camera.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Camera.hh 2008-10-03 23:09:54 UTC (rev 5037)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Camera.hh 2008-10-03 23:30:33 UTC (rev 5038)
@@ -43,6 +43,7 @@
This is a controller that collects data from a Camera Sensor and populates a libgazebo camera interface as well as publish a ROS std_msgs::Image (under the field \b \<topicName\>). This controller should only be used as a child of a camera sensor (see example below.
+ \li Example Usage:
\verbatim
<model:physical name="camera_model">
<body:empty name="camera_body_name">
@@ -59,11 +60,32 @@
\endverbatim
\{
-*/
+
/// \brief Ros_Camera Controller.
/// \li Starts a ROS node if none exists. \n
-/// \li Simulates a generic camera and broadcast std_msgs::Image topic over ROS. \n
+/// \li Simulates a generic camera and broadcast std_msgs::Image topic over ROS.
+/// .
+
+
+ \li Example Usage:
+ \verbatim
+ <model:physical name="camera_model">
+ <body:empty name="camera_body_name">
+ <sensor:camera name="camera_sensor">
+ <controller:ros_camera name="controller-name" plugin="libRos_Camera.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>15.0</updateRate>
+ <topicName>camera_name/image</topicName>
+ <frameName>camera_body_name</frameName>
+ </controller:ros_camera>
+ </sensor:camera>
+ </body:empty>
+ </model:phyiscal>
+ \endverbatim
+
+*/
+
class Ros_Camera : public Controller
{
/// \brief Constructor
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Laser.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Laser.hh 2008-10-03 23:09:54 UTC (rev 5037)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Laser.hh 2008-10-03 23:30:33 UTC (rev 5038)
@@ -46,6 +46,7 @@
This controller gathers range data from a simulated ray sensor, publishes range data through
std_msgs::LaserScan ROS topic.
+ \li Example Usage:
\verbatim
<model:physical name="ray_model">
<body:empty name="ray_body_name">
@@ -74,11 +75,40 @@
\endverbatim
\{
-*/
/// \brief ROS laser scan controller.
/// \li Starts a ROS node if none exists.
/// \li Simulates a laser range sensor and publish std_msgs::LaserScan.msg over ROS.
+/// .
+ \li Example Usage:
+ \verbatim
+ <model:physical name="ray_model">
+ <body:empty name="ray_body_name">
+ <sensor:ray name="ray_sensor">
+ <origin>0.0 0.0 0.0</origin>
+ <rayCount>683</rayCount>
+ <rangeCount>683</rangeCount>
+ <laserCount>1</laserCount>
+ <displayRays>false</displayRays>
+ <minAngle>-45</minAngle>
+ <maxAngle> 45</maxAngle>
+ <minRange>0.05</minRange>
+ <maxRange>10.0</maxRange>
+ <updateRate>10.0</updateRate>
+ <controller:ros_laser name="ros_ray_sensor_controller" plugin="libRos_Laser.so">
+ <gaussianNoise>0.005</gaussianNoise>
+ <alwaysOn>true</alwaysOn>
+ <updateRate>15.0</updateRate>
+ <topicName>ray_scan</topicName>
+ <frameName>ray_model</frameName>
+ <interface:laser name="ros_ray_sensor_iface" />
+ </controller:ros_laser>
+ </sensor:ray>
+ </body:empty>
+ </model:phyiscal>
+ \endverbatim
+*/
+
class Ros_Laser : public Controller
{
/// \brief Constructor
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_PTZ.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_PTZ.hh 2008-10-03 23:09:54 UTC (rev 5037)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_PTZ.hh 2008-10-03 23:30:33 UTC (rev 5038)
@@ -50,6 +50,7 @@
This is a controller that controls a pan, tilt, zoom unit
+ \li Example Usage:
\verbatim
<model:physical name="ptz_model">
<controller:Ros_PTZ name="ptz_controller" plugin="libRos_PTZ.so">
@@ -65,7 +66,6 @@
\endverbatim
\{
-*/
/// \brief ROS Pan/Tilt/Zoom Camera Controller
/// \li Starts a ROS node if none exists.
@@ -73,6 +73,21 @@
/// - publish state information (PT angles) to ROS topic: \e camera_name/ptz_state
/// - subscribe to ROS topic: \e camera_name/ptz_cmd
///
+ \li Example Usage:
+ \verbatim
+ <model:physical name="ptz_model">
+ <controller:Ros_PTZ name="ptz_controller" plugin="libRos_PTZ.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>15.0</updateRate>
+ <panJoint>ptz_pan_joint_name</panJoint>
+ <tiltJoint>ptz_tilt_joint_name</tiltJoint>
+ <commandTopicName>camera_name/ptz_cmd</commandTopicName>
+ <stateTopicName>camera_name/ptz_state</stateTopicName>
+ <interface:ptz name="ptz_iface" />
+ </controller:Ros_PTZ>
+ </model:phyiscal>
+ \endverbatim
+*/
class Ros_PTZ : public Controller
{
/// \brief Constructor
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Time.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Time.hh 2008-10-03 23:09:54 UTC (rev 5037)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Time.hh 2008-10-03 23:30:33 UTC (rev 5038)
@@ -46,6 +46,7 @@
This is a controller that broadcasts simulator time over ros::time
+ \li Example Usage:
\verbatim
<model:physical name="robot_model1">
@@ -68,11 +69,33 @@
\{
*/
+/**
+ * \brief ROS Time Controller
+ * \li Starts a ROS node if none exists
+ * \li broadcast simulator time over rostools::Time.
+ * .
+ \li Example Usage:
+ \verbatim
+ <model:physical name="robot_model1">
-/// \brief ROS Time Controller
-/// \li Starts a ROS node if none exists
-/// \li broadcast simulator time over rostools::Time.
-///
+ <controller:ros_time name="ros_time" plugin="libRos_Time.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <interface:audio name="dummy_ros_time_iface_should_not_be_here"/>
+ </controller:ros_time>
+
+ <xyz>0.0 0.0 0.02</xyz>
+ <rpy>0.0 0.0 0.0 </rpy>
+
+ <!-- base, torso and arms -->
+ <include embedded="true">
+ <xi:include href="pr2_xml.model" />
+ </include>
+
+ </model:physical>
+ \endverbatim
+
+**/
class Ros_Time : public Controller
{
/// \brief Constructor
Deleted: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_actuators.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_actuators.h 2008-10-03 23:09:54 UTC (rev 5037)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_actuators.h 2008-10-03 23:30:33 UTC (rev 5038)
@@ -1,165 +0,0 @@
-/*
- * Copyright (c) 2008, Willow Garage, Inc.
- * All rights reserved.
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions are met:
- *
- * * Redistributions of source code must retain the above copyright
- * notice, this list of conditions and the following disclaimer.
- * * Redistributions in binary form must reproduce the above copyright
- * notice, this list of conditions and the following disclaimer in the
- * documentation and/or other materials provided with the distribution.
- * * Neither the name of the Willow Garage, Inc. nor the names of its
- * contributors may be used to endorse or promote products derived from
- * this software without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
- * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
- * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
- * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
- * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
- * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
- * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
- * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
- * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
- * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- */
-
-#ifndef GAZEBO_ACTUATORS_H
-#define GAZEBO_ACTUATORS_H
-
-#include <vector>
-#include <map>
-#include <gazebo/Controller.hh>
-#include <gazebo/Entity.hh>
-#include <gazebo/Model.hh>
-#include "hardware_interface/hardware_interface.h"
-#include "mechanism_control/mechanism_control.h"
-#include "mechanism_model/robot.h"
-#include "tinyxml/tinyxml.h"
-
-
-namespace gazebo
-{
-class HingeJoint;
-class XMLConfigNode;
-
-/// @addtogroup gazebo_dynamic_plugins Gazebo ROS Dynamic Plugins
-/// @{
-/** \defgroup gazebo_actuators GazeboActuators class
-
- \brief GazeboActuators Plugin
-
- This is a controller that provides interface between simulator and the Robot Mechanism Control.
- GazeboActuators requires model as its parent.
-
- \verbatim
- <model:physical name="ray_model">
- <!-- GazeboActuators -->
- <controller:gazebo_actuators name="gazebo_actuators" plugin="libgazebo_actuators.so">
- <alwaysOn>true</alwaysOn>
- <updateRate>1000.0</updateRate>
- <robot filename="pr2.xml" /> <!-- gazebo_actuators use this file to extract mechanism model -->
- <gazebo_physics filename="gazebo_joints.xml" /> <!-- for simulator/physics specific settigs, currently just damping -->
- <interface:audio name="gazebo_actuators_dummy_iface" />
- </controller:gazebo_actuators>
- </model:phyiscal>
- \endverbatim
-
-\{
-
-
-*/
-
-/**
- * Gazebo simulator provides joint level control for mechanisms. In order to work with mechanisms in real life
- * at the level of actuators, a plugin is required.
- * As implemented here in GazeboActuators, this plugin abstracts the definitions of
- * actuators and transmissions. It parses the \e robot.xml, \e actuators.xml
- * and \e transmissions.xml, then sets up an abstract layer of actuators. The entire chain of command from
- * controllers to actuators to simulated mechanism joints and back are implemented in this plugin.
- *
- * - On the software/controller side:
- * -# The plugin maintians a list of \c fake-actuators as described by \e actuators.xml, from which
- * the actuator's \b encoder-value is transmitted to \b joint-state via \e transmissions.xml
- * -# The controller reads \b joint-state from \c Mechanism-State and sends \b joint-error-value
- * to the PID controller, then issues the resulting \b joint-torque-command to \c Mechanism-Model
- * -# \b joint-torque-command is converted to \b actuator-current-command
- * via transmission definition from \e transmissions.xml
- * - On the Hardware side in the simulator
- * -# The plugin maintains a list of \c fake-actuators as described by \e actuators.xml,
- * from which the simulator reads the \b actuator-current-command, reverse maps to \b joint-torque-command
- * and stores in a set of \c fake-mechanism-states
- * -# The \b Joint-torque-command is sent to simulated joint in ODE
- * -# \b Simulator-joint-state is obtained from ODE and stored in \c fake-mechanism-states.
- * -# \c Fake-mechanism-state's \b joint-state is converted to
- * \b actuator-encoder values and stored in \c fake-actuators as defined by \e transmissions.xml
- * - On the software/controller side:
- * -# [loops around] \b Actuator-encoder-value is transmitted to \b joint-state via \e transmissions.xml
- * -# Controller reads \b joint-state and issues a \b joint-torque-command
- * .
- *
- * @image html "http://pr.willowgarage.com/wiki/gazebo_plugin?action=AttachFile&do=get&target=gazebo_mcn.jpg" "Gazebo Mechanism Control Model"
- *
-**/
-
-
-class GazeboActuators : public gazebo::Controller
-{
-public:
- GazeboActuators(Entity *parent);
- virtual ~GazeboActuators();
-
-protected:
- // Inherited from gazebo::Controller
- virtual void LoadChild(XMLConfigNode *node);
- virtual void InitChild();
- virtual void UpdateChild();
- virtual void FiniChild();
-
-private:
-
- Model *parent_model_;
- HardwareInterface hw_;
- MechanismControl mc_;
- MechanismControlNode mcn_;
-
- TiXmlDocument config_;
-
- /// @todo The fake state helps Gazebo run the transmissions backwards, so
- /// that it can figure out what its joints should do based on the
- /// actuator values.
- mechanism::RobotState *fake_state_;
- std::vector<gazebo::Joint*> joints_;
-
- // added for joint damping coefficients
- std::vector<double> joints_damping_;
- std::map<std::string,double> joints_damping_map_;
-
- /*
- * \brief read pr2.xml for actuators, and pass tinyxml node to mechanism control node's initXml.
- */
- void ReadPr2Xml(XMLConfigNode *node);
-
- /*
- * \brief read gazebo_joints.xml for joint damping and additional simulation parameters for joints
- */
- void ReadGazeboPhysics(XMLConfigNode *node);
-
- /*
- * \brief pointer to ros node
- */
- ros::node *rosnode_;
-
-
-};
-
-/** \} */
-/// @}
-
-}
-
-#endif
-
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_battery.h
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_battery.h 2008-10-03 23:09:54 UTC (rev 5037)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_battery.h 2008-10-03 23:30:33 UTC (rev 5038)
@@ -76,6 +76,16 @@
* \li return battery state and diagnostic message over ROS topic.
* .
*
+ \verbatim
+ <model:physical name="ray_model">
+ <!-- GazeboBattery -->
+ <controller:gazebo_battery name="gazebo_battery" plugin="libgazebo_battery.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <interface:audio name="gazebo_battery_dummy_iface" />
+ </controller:gazebo_battery>
+ </model:phyiscal>
+ \endverbatim
**/
Copied: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_mechanism_control.h (from rev 5026, pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_actuators.h)
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_mechanism_control.h (rev 0)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/gazebo_mechanism_control.h 2008-10-03 23:30:33 UTC (rev 5038)
@@ -0,0 +1,179 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef GAZEBO_MECHANISM_CONTROL_H
+#define GAZEBO_MECHANISM_CONTROL_H
+
+#include <vector>
+#include <map>
+#include <gazebo/Controller.hh>
+#include <gazebo/Entity.hh>
+#include <gazebo/Model.hh>
+#include "hardware_interface/hardware_interface.h"
+#include "mechanism_control/mechanism_control.h"
+#include "mechanism_model/robot.h"
+#include "tinyxml/tinyxml.h"
+
+
+namespace gazebo
+{
+class HingeJoint;
+class XMLConfigNode;
+
+/// @addtogroup gazebo_dynamic_plugins Gazebo ROS Dynamic Plugins
+/// @{
+/** \defgroup gazebo_mechanism_control GazeboMechanismControl class
+
+ \brief GazeboMechanismControl Plugin
+
+ This is a controller that provides interface between simulator and the Robot Mechanism Control.
+ GazeboMechanismControl requires model as its parent.
+
+ \li Example Usage:
+ \verbatim
+ <model:physical name="ray_model">
+ <!-- GazeboMechanismControl -->
+ <controller:gazebo_mechanism_control name="gazebo_mechanism_control" plugin="libgazebo_mechanism_control.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <robot filename="pr2.xml" /> <!-- gazebo_mechanism_control use this file to extract mechanism model -->
+ <gazebo_physics filename="gazebo_joints.xml" /> <!-- for simulator/physics specific settigs, currently just damping -->
+ <interface:audio name="gazebo_mechanism_control_dummy_iface" />
+ </controller:gazebo_mechanism_control>
+ </model:phyiscal>
+ \endverbatim
+
+\{
+
+
+*/
+
+/**
+ * Gazebo simulator provides joint level control for mechanisms. In order to work with mechanisms in real life
+ * at the level of actuators, a plugin is required.
+ * As implemented here in GazeboMechanismControl, this plugin abstracts the definitions of
+ * actuators and transmissions. It parses the \e robot.xml, \e actuators.xml
+ * and \e transmissions.xml, then sets up an abstract layer of actuators. The entire chain of command from
+ * controllers to actuators to simulated mechanism joints and back are implemented in this plugin.
+ *
+ * - On the software/controller side:
+ * -# The plugin maintians a list of \c fake-actuators as described by \e actuators.xml, from which
+ * the actuator's \b encoder-value is transmitted to \b joint-state via \e transmissions.xml
+ * -# The controller reads \b joint-state from \c Mechanism-State and sends \b joint-error-value
+ * to the PID controller, then issues the resulting \b joint-torque-command to \c Mechanism-Model
+ * -# \b joint-torque-command is converted to \b actuator-current-command
+ * via transmission definition from \e transmissions.xml
+ * - On the Hardware side in the simulator
+ * -# The plugin maintains a list of \c fake-actuators as described by \e actuators.xml,
+ * from which the simulator reads the \b actuator-current-command, reverse maps to \b joint-torque-command
+ * and stores in a set of \c fake-mechanism-states
+ * -# The \b Joint-torque-command is sent to simulated joint in ODE
+ * -# \b Simulator-joint-state is obtained from ODE and stored in \c fake-mechanism-states.
+ * -# \c Fake-mechanism-state's \b joint-state is converted to
+ * \b actuator-encoder values and stored in \c fake-actuators as defined by \e transmissions.xml
+ * - On the software/controller side:
+ * -# [loops around] \b Actuator-encoder-value is transmitted to \b joint-state via \e transmissions.xml
+ * -# Controller reads \b joint-state and issues a \b joint-torque-command
+ * .
+ *
+ * @image html "http://pr.willowgarage.com/wiki/gazebo_plugin?action=AttachFile&do=get&target=gazebo_mcn.jpg" "Gazebo Mechanism Control Model"
+ *
+ \li Example Usage:
+ \verbatim
+ <model:physical name="ray_model">
+ <!-- GazeboMechanismControl -->
+ <controller:gazebo_mechanism_control name="gazebo_mechanism_control" plugin="libgazebo_mechanism_control.so">
+ <alwaysOn>true</alwaysOn>
+ <updateRate>1000.0</updateRate>
+ <robot filename="pr2.xml" /> <!-- gazebo_mechanism_control use this file to extract mechanism model -->
+ <gazebo_physics filename="gazebo_joints.xml" /> <!-- for simulator/physics specific settigs, currently just damping -->
+ <interface:audio name="gazebo_mechanism_control_du...
[truncated message content] |
|
From: <rob...@us...> - 2008-10-04 00:34:01
|
Revision: 5047
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5047&view=rev
Author: rob_wheeler
Date: 2008-10-04 00:33:58 +0000 (Sat, 04 Oct 2008)
Log Message:
-----------
Return calibration data in radians.
Set WG0X safety disable when command enabled
Modified Paths:
--------------
pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/wg0x.h
pkg/trunk/drivers/motor/ethercat_hardware/src/wg0x.cpp
pkg/trunk/mechanism/mechanism_control/msg/ActuatorState.msg
pkg/trunk/mechanism/mechanism_model/src/simple_transmission.cpp
Modified: pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/wg0x.h
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/wg0x.h 2008-10-04 00:26:17 UTC (rev 5046)
+++ pkg/trunk/drivers/motor/ethercat_hardware/include/ethercat_hardware/wg0x.h 2008-10-04 00:33:58 UTC (rev 5047)
@@ -237,7 +237,7 @@
enum
{
MODE_OFF = 0x00, MODE_CURRENT = 0x01, MODE_ENABLE = 0x02,
- MODE_UNDERVOLTAGE = 0x04, MODE_RESET = 0x80
+ MODE_UNDERVOLTAGE = 0x04, MODE_SAFETY_RESET = 0x10, MODE_RESET = 0x80
};
enum
Modified: pkg/trunk/drivers/motor/ethercat_hardware/src/wg0x.cpp
===================================================================
--- pkg/trunk/drivers/motor/ethercat_hardware/src/wg0x.cpp 2008-10-04 00:26:17 UTC (rev 5046)
+++ pkg/trunk/drivers/motor/ethercat_hardware/src/wg0x.cpp 2008-10-04 00:33:58 UTC (rev 5047)
@@ -222,6 +222,7 @@
node->log(ros::FATAL, "Unable to load configuration information");
return -1;
}
+ printf("Device #%02d: Serial #: %05d\n", sh_->get_ring_position(), config_info_.device_serial_number_);
if (readEeprom(sh_) < 0)
{
@@ -303,7 +304,7 @@
memset(c, 0, sizeof(WG0XCommand));
c->programmed_current_ = int(command.current_ / config_info_.nominal_current_scale_);
- c->mode_ = command.enable_ ? (MODE_ENABLE | MODE_CURRENT) : MODE_OFF;
+ c->mode_ = command.enable_ ? (MODE_ENABLE | MODE_CURRENT | MODE_SAFETY_RESET) : MODE_OFF;
c->checksum_ = rotateRight8(computeChecksum(c, sizeof(WG0XCommand) - 1));
}
@@ -353,8 +354,8 @@
/ (this_status->timestamp_ - prev_status->timestamp_) * 1e+6;
state.velocity_ = state.encoder_velocity_ / actuator_info_.pulses_per_revolution_ * 2 * M_PI;
state.calibration_reading_ = this_status->calibration_reading_ & LIMIT_SENSOR_0_STATE;
- state.last_calibration_high_transition_ = double(this_status->last_calibration_high_transition_) / actuator_info_.pulses_per_revolution_;
- state.last_calibration_low_transition_ = double(this_status->last_calibration_low_transition_) / actuator_info_.pulses_per_revolution_;
+ state.last_calibration_high_transition_ = double(this_status->last_calibration_high_transition_) / actuator_info_.pulses_per_revolution_ * 2 * M_PI;
+ state.last_calibration_low_transition_ = double(this_status->last_calibration_low_transition_) / actuator_info_.pulses_per_revolution_ * 2 * M_PI;
state.is_enabled_ = this_status->mode_ != MODE_OFF;
state.run_stop_hit_ = (this_status->mode_ & MODE_UNDERVOLTAGE) != 0;
Modified: pkg/trunk/mechanism/mechanism_control/msg/ActuatorState.msg
===================================================================
--- pkg/trunk/mechanism/mechanism_control/msg/ActuatorState.msg 2008-10-04 00:26:17 UTC (rev 5046)
+++ pkg/trunk/mechanism/mechanism_control/msg/ActuatorState.msg 2008-10-04 00:33:58 UTC (rev 5047)
@@ -5,8 +5,8 @@
float64 encoder_velocity
float64 velocity
byte calibration_reading
-int32 last_calibration_high_transition
-int32 last_calibration_low_transition
+float64 last_calibration_high_transition
+float64 last_calibration_low_transition
byte is_enabled
byte run_stop_hit
float64 last_requested_current
Modified: pkg/trunk/mechanism/mechanism_model/src/simple_transmission.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_model/src/simple_transmission.cpp 2008-10-04 00:26:17 UTC (rev 5046)
+++ pkg/trunk/mechanism/mechanism_model/src/simple_transmission.cpp 2008-10-04 00:33:58 UTC (rev 5047)
@@ -58,11 +58,13 @@
TiXmlElement *ael = elt->FirstChildElement("actuator");
const char *actuator_name = ael ? ael->Attribute("name") : NULL;
- if (!actuator_name || robot->getActuatorIndex(actuator_name) < 0)
+ Actuator *a;
+ if (!actuator_name || (a = robot->getActuator(actuator_name)) == NULL )
{
fprintf(stderr, "SimpleTransmission could not find actuator named \"%s\"\n", actuator_name);
return false;
}
+ a->command_.enable_ = true;
actuator_names_.push_back(actuator_name);
mechanical_reduction_ = atof(elt->FirstChildElement("mechanicalReduction")->GetText());
@@ -95,7 +97,6 @@
assert(as.size() == 1);
assert(js.size() == 1);
as[0]->command_.effort_ = js[0]->commanded_effort_ / mechanical_reduction_;
- as[0]->command_.enable_ = true;
}
void SimpleTransmission::propagateEffortBackwards(
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-09-29 18:38:17
|
Revision: 4771
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4771&view=rev
Author: tfoote
Date: 2008-09-29 18:37:43 +0000 (Mon, 29 Sep 2008)
Log Message:
-----------
adding stl compatibility 2dnav-stage stack. I have tested this code with both vectors branch and trunk for 2dnav-stage. it works for both.
Modified Paths:
--------------
pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
pkg/trunk/util/laser_scan_utils/src/laser_scan.cc
pkg/trunk/util/rosTF/src/rosTF.cpp
pkg/trunk/util/tf/include/tf/transform_listener.h
pkg/trunk/util/tf/test/tf_unittest.cpp
Modified: pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-09-29 18:18:55 UTC (rev 4770)
+++ pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-09-29 18:37:43 UTC (rev 4771)
@@ -245,7 +245,7 @@
assert(mc_->model_.joints_.size() == mechanism_state_.get_joint_states_size());
for (unsigned int i = 0; i < mc_->model_.joints_.size(); ++i)
{
- mechanism_control::JointState *out = mechanism_state_.joint_states + i;
+ mechanism_control::JointState *out = &mechanism_state_.joint_states[i];
mechanism::JointState *in = &mc_->state_->joint_states_[i];
out->name = mc_->model_.joints_[i]->name_;
out->position = in->position_;
@@ -256,7 +256,7 @@
for (unsigned int i = 0; i < mc_->hw_->actuators_.size(); ++i)
{
- mechanism_control::ActuatorState *out = mechanism_state_.actuator_states + i;
+ mechanism_control::ActuatorState *out = &mechanism_state_.actuator_states[i];
ActuatorState *in = &mc_->hw_->actuators_[i]->state_;
out->name = mc_->hw_->actuators_[i]->name_;
out->encoder_count = in->encoder_count_;
Modified: pkg/trunk/util/laser_scan_utils/src/laser_scan.cc
===================================================================
--- pkg/trunk/util/laser_scan_utils/src/laser_scan.cc 2008-09-29 18:18:55 UTC (rev 4770)
+++ pkg/trunk/util/laser_scan_utils/src/laser_scan.cc 2008-09-29 18:37:43 UTC (rev 4771)
@@ -35,13 +35,13 @@
void LaserProjection::projectLaser(const std_msgs::LaserScan& scan_in, std_msgs::PointCloud & cloud_out, double range_cutoff, bool preservative)
{
- NEWMAT::Matrix ranges(2, scan_in.ranges_size);
+ NEWMAT::Matrix ranges(2, scan_in.get_ranges_size());
double * matPointer = ranges.Store();
// Fill the ranges matrix
- for (unsigned int index = 0; index < scan_in.ranges_size; index++)
+ for (unsigned int index = 0; index < scan_in.get_ranges_size(); index++)
{
matPointer[index] = (double) scan_in.ranges[index];
- matPointer[index+scan_in.ranges_size] = (double) scan_in.ranges[index];
+ matPointer[index+scan_in.get_ranges_size()] = (double) scan_in.ranges[index];
}
@@ -51,12 +51,12 @@
//Stuff the output cloud
cloud_out.header = scan_in.header;
- cloud_out.set_pts_size(scan_in.ranges_size);
- if (scan_in.intensities_size > 0)
+ cloud_out.set_pts_size(scan_in.get_ranges_size());
+ if (scan_in.get_intensities_size() > 0)
{
cloud_out.set_chan_size(1);
cloud_out.chan[0].name ="intensities";
- cloud_out.chan[0].set_vals_size(scan_in.intensities_size);
+ cloud_out.chan[0].set_vals_size(scan_in.get_intensities_size());
}
double* outputMat = output.Store();
@@ -67,25 +67,25 @@
range_cutoff = std::min(range_cutoff, (double)scan_in.range_max);
unsigned int count = 0;
- for (unsigned int index = 0; index< scan_in.ranges_size; index++)
+ for (unsigned int index = 0; index< scan_in.get_ranges_size(); index++)
{
if (!preservative){ //Default behaviour will throw out invalid data
if ((matPointer[index] < range_cutoff) &&
(matPointer[index] > scan_in.range_min)) //only valid
{
cloud_out.pts[count].x = outputMat[index];
- cloud_out.pts[count].y = outputMat[index + scan_in.ranges_size];
+ cloud_out.pts[count].y = outputMat[index + scan_in.get_ranges_size()];
cloud_out.pts[count].z = 0.0;
- if (scan_in.intensities_size >= index) /// \todo optimize and catch length difference better
+ if (scan_in.get_intensities_size() >= index) /// \todo optimize and catch length difference better
cloud_out.chan[0].vals[count] = scan_in.intensities[index];
count++;
}
}
else { //Keep all points
cloud_out.pts[count].x = outputMat[index];
- cloud_out.pts[count].y = outputMat[index + scan_in.ranges_size];
+ cloud_out.pts[count].y = outputMat[index + scan_in.get_ranges_size()];
cloud_out.pts[count].z = 0.0;
- if (scan_in.intensities_size >= index) /// \todo optimize and catch length difference better
+ if (scan_in.get_intensities_size() >= index) /// \todo optimize and catch length difference better
cloud_out.chan[0].vals[count] = scan_in.intensities[index];
count++;
}
@@ -99,13 +99,13 @@
};
void LaserProjection::projectLaser(const std_msgs::LaserScan& scan_in, std_msgs::PointCloudFloat32 & cloud_out, double range_cutoff, bool preservative)
{
- NEWMAT::Matrix ranges(2, scan_in.ranges_size);
+ NEWMAT::Matrix ranges(2, scan_in.get_ranges_size());
double * matPointer = ranges.Store();
// Fill the ranges matrix
- for (unsigned int index = 0; index < scan_in.ranges_size; index++)
+ for (unsigned int index = 0; index < scan_in.get_ranges_size(); index++)
{
matPointer[index] = (double) scan_in.ranges[index];
- matPointer[index+scan_in.ranges_size] = (double) scan_in.ranges[index];
+ matPointer[index+scan_in.get_ranges_size()] = (double) scan_in.ranges[index];
}
@@ -115,12 +115,12 @@
//Stuff the output cloud
cloud_out.header = scan_in.header;
- cloud_out.set_pts_size(scan_in.ranges_size);
- if (scan_in.intensities_size > 0)
+ cloud_out.set_pts_size(scan_in.get_ranges_size());
+ if (scan_in.get_intensities_size() > 0)
{
cloud_out.set_chan_size(1);
cloud_out.chan[0].name ="intensities";
- cloud_out.chan[0].set_vals_size(scan_in.intensities_size);
+ cloud_out.chan[0].set_vals_size(scan_in.get_intensities_size());
}
double* outputMat = output.Store();
@@ -131,25 +131,25 @@
range_cutoff = std::min(range_cutoff, (double)scan_in.range_max);
unsigned int count = 0;
- for (unsigned int index = 0; index< scan_in.ranges_size; index++)
+ for (unsigned int index = 0; index< scan_in.get_ranges_size(); index++)
{
if (!preservative){ //Default behaviour will throw out invalid data
if ((matPointer[index] < range_cutoff) &&
(matPointer[index] > scan_in.range_min)) //only valid
{
cloud_out.pts[count].x = outputMat[index];
- cloud_out.pts[count].y = outputMat[index + scan_in.ranges_size];
+ cloud_out.pts[count].y = outputMat[index + scan_in.get_ranges_size()];
cloud_out.pts[count].z = 0.0;
- if (scan_in.intensities_size >= index) /// \todo optimize and catch length difference better
+ if (scan_in.get_intensities_size() >= index) /// \todo optimize and catch length difference better
cloud_out.chan[0].vals[count] = scan_in.intensities[index];
count++;
}
}
else { //Keep all points
cloud_out.pts[count].x = outputMat[index];
- cloud_out.pts[count].y = outputMat[index + scan_in.ranges_size];
+ cloud_out.pts[count].y = outputMat[index + scan_in.get_ranges_size()];
cloud_out.pts[count].z = 0.0;
- if (scan_in.intensities_size >= index) /// \todo optimize and catch length difference better
+ if (scan_in.get_intensities_size() >= index) /// \todo optimize and catch length difference better
cloud_out.chan[0].vals[count] = scan_in.intensities[index];
count++;
}
@@ -213,7 +213,7 @@
temp_scan_ = scan_in; //HACK to store all metadata
/** \todo check for length of intensities too */
- unsigned int iterations = std::min(scan_in.ranges_size, num_ranges_);
+ unsigned int iterations = std::min(scan_in.get_ranges_size(), num_ranges_);
for (unsigned int index = 0; index < iterations; index ++)
{
range_data_(current_packet_num_+1, index+1)= (double) scan_in.ranges[index];
@@ -238,7 +238,7 @@
NEWMAT::ColumnVector iColumn;
- unsigned int iterations = std::min(scan_result.ranges_size, num_ranges_);
+ unsigned int iterations = std::min(scan_result.get_ranges_size(), num_ranges_);
/** \todo Resize output cloud/check length */
for (unsigned int index = 0; index < iterations; index ++)
{
Modified: pkg/trunk/util/rosTF/src/rosTF.cpp
===================================================================
--- pkg/trunk/util/rosTF/src/rosTF.cpp 2008-09-29 18:18:55 UTC (rev 4770)
+++ pkg/trunk/util/rosTF/src/rosTF.cpp 2008-09-29 18:37:43 UTC (rev 4771)
@@ -101,12 +101,12 @@
{
cloudOut.header = scanIn.header;
cloudOut.header.frame_id = target_frame;
- cloudOut.set_pts_size(scanIn.ranges_size);
- if (scanIn.intensities_size > 0)
+ cloudOut.set_pts_size(scanIn.get_ranges_size());
+ if (scanIn.get_intensities_size() > 0)
{
cloudOut.set_chan_size(1);
cloudOut.chan[0].name ="intensities";
- cloudOut.chan[0].set_vals_size(scanIn.intensities_size);
+ cloudOut.chan[0].set_vals_size(scanIn.get_intensities_size());
}
libTF::TFPoint pointIn;
@@ -119,7 +119,7 @@
projector_.projectLaser(scanIn, intermediate, -1.0, true);
unsigned int count = 0;
- for (unsigned int i = 0; i < scanIn.ranges_size; i++)
+ for (unsigned int i = 0; i < scanIn.get_ranges_size(); i++)
{
if (scanIn.ranges[i] < scanIn.range_max
&& scanIn.ranges[i] > scanIn.range_min) //only when valid
@@ -135,7 +135,7 @@
cloudOut.pts[count].y = pointOut.y;
cloudOut.pts[count].z = pointOut.z;
- if (scanIn.intensities_size >= i) /// \todo optimize and catch length difference better
+ if (scanIn.get_intensities_size() >= i) /// \todo optimize and catch length difference better
cloudOut.chan[0].vals[count] = scanIn.intensities[i];
count++;
}
@@ -151,7 +151,7 @@
void rosTFClient::receiveArray()
{
- for (unsigned int i = 0; i < tfArrayIn.eulers_size; i++)
+ for (unsigned int i = 0; i < tfArrayIn.get_eulers_size(); i++)
{
try{
setWithEulers(tfArrayIn.eulers[i].header.frame_id, tfArrayIn.eulers[i].parent, tfArrayIn.eulers[i].x, tfArrayIn.eulers[i].y, tfArrayIn.eulers[i].z, tfArrayIn.eulers[i].yaw, tfArrayIn.eulers[i].pitch, tfArrayIn.eulers[i].roll, tfArrayIn.eulers[i].header.stamp.sec * 1000000000ULL + tfArrayIn.eulers[i].header.stamp.nsec);
@@ -163,7 +163,7 @@
};
}
//std::cout << "received euler frame: " << tfArrayIn.eulers[i].header.frame_id << " with parent:" << tfArrayIn.eulers[i].parent << "time " << tfArrayIn.eulers[i].header.stamp.sec * 1000000000ULL + eulerIn.header.stamp.nsec << std::endl;
- for (unsigned int i = 0; i < tfArrayIn.dhparams_size; i++)
+ for (unsigned int i = 0; i < tfArrayIn.get_dhparams_size(); i++)
{
try{
setWithDH(tfArrayIn.dhparams[i].header.frame_id, tfArrayIn.dhparams[i].parent, tfArrayIn.dhparams[i].length, tfArrayIn.dhparams[i].twist, tfArrayIn.dhparams[i].offset, tfArrayIn.dhparams[i].angle, tfArrayIn.dhparams[i].header.stamp.sec * 1000000000ULL + tfArrayIn.dhparams[i].header.stamp.nsec);
@@ -176,7 +176,7 @@
// std::cout << "recieved DH frame: " << tfArrayIn.dhparams[i].header.frame_id << " with parent:" << tfArrayIn.dhparams[i].parent << std::endl;
}
- for (unsigned int i = 0; i < tfArrayIn.quaternions_size; i++)
+ for (unsigned int i = 0; i < tfArrayIn.get_quaternions_size(); i++)
{
try{
setWithQuaternion(tfArrayIn.quaternions[i].header.frame_id, tfArrayIn.quaternions[i].parent, tfArrayIn.quaternions[i].xt, tfArrayIn.quaternions[i].yt, tfArrayIn.quaternions[i].zt, tfArrayIn.quaternions[i].xr, tfArrayIn.quaternions[i].yr, tfArrayIn.quaternions[i].zr, tfArrayIn.quaternions[i].w, tfArrayIn.quaternions[i].header.stamp.sec * 1000000000ULL + tfArrayIn.quaternions[i].header.stamp.nsec);
@@ -188,18 +188,18 @@
};
// std::cout << "recieved quaternion frame: " << tfArrayIn.quaternions[i].header.frame_id << " with parent:" << tfArrayIn.quaternions[i].parent << std::endl;
}
- for (unsigned int i = 0; i < tfArrayIn.matrices_size; i++)
+ for (unsigned int i = 0; i < tfArrayIn.get_matrices_size(); i++)
{
try{
- if (tfArrayIn.matrices[i].matrix_size != 16)
+ if (tfArrayIn.matrices[i].get_matrix_size() != 16)
{
- std::cerr << "recieved matrix not of size 16, it was "<< tfArrayIn.matrices[i].matrix_size;
+ std::cerr << "recieved matrix not of size 16, it was "<< tfArrayIn.matrices[i].get_matrix_size();
return;
}
NEWMAT::Matrix tempMatrix(4,4);
- tempMatrix << tfArrayIn.matrices[i].matrix;
+ tempMatrix << (tfArrayIn.matrices[i].matrix[0]);
setWithMatrix(tfArrayIn.matrices[i].header.frame_id, tfArrayIn.matrices[i].parent, tempMatrix, tfArrayIn.matrices[i].header.stamp.sec * 1000000000ULL + tfArrayIn.matrices[i].header.stamp.nsec);
Modified: pkg/trunk/util/tf/include/tf/transform_listener.h
===================================================================
--- pkg/trunk/util/tf/include/tf/transform_listener.h 2008-09-29 18:18:55 UTC (rev 4770)
+++ pkg/trunk/util/tf/include/tf/transform_listener.h 2008-09-29 18:37:43 UTC (rev 4771)
@@ -93,7 +93,7 @@
///\todo Remove : for backwards compatability only
void receiveArray()
{
- for (unsigned int i = 0; i < tfArrayIn.eulers_size; i++)
+ for (unsigned int i = 0; i < tfArrayIn.get_eulers_size(); i++)
{
try{
// setWithEulers(tfArrayIn.eulers[i].header.frame_id, tfArrayIn.eulers[i].parent, tfArrayIn.eulers[i].x, tfArrayIn.eulers[i].y, tfArrayIn.eulers[i].z, tfArrayIn.eulers[i].yaw, tfArrayIn.eulers[i].pitch, tfArrayIn.eulers[i].roll, tfArrayIn.eulers[i].header.stamp.sec * 1000000000ULL + tfArrayIn.eulers[i].header.stamp.nsec);
@@ -110,12 +110,12 @@
};
}
//std::cout << "received euler frame: " << tfArrayIn.eulers[i].header.frame_id << " with parent:" << tfArrayIn.eulers[i].parent << "time " << tfArrayIn.eulers[i].header.stamp.sec * 1000000000ULL + eulerIn.header.stamp.nsec << std::endl;
- for (unsigned int i = 0; i < tfArrayIn.dhparams_size; i++)
+ for (unsigned int i = 0; i < tfArrayIn.get_dhparams_size(); i++)
{
std::cerr << "receiveArray: setWithDH failed No longer supported" << std::endl;
}
- for (unsigned int i = 0; i < tfArrayIn.quaternions_size; i++)
+ for (unsigned int i = 0; i < tfArrayIn.get_quaternions_size(); i++)
{
try{
// setWithQuaternion(tfArrayIn.quaternions[i].header.frame_id, tfArrayIn.quaternions[i].parent, tfArrayIn.quaternions[i].xt, tfArrayIn.quaternions[i].yt, tfArrayIn.quaternions[i].zt, tfArrayIn.quaternions[i].xr, tfArrayIn.quaternions[i].yr, tfArrayIn.quaternions[i].zr, tfArrayIn.quaternions[i].w, tfArrayIn.quaternions[i].header.stamp.sec * 1000000000ULL + tfArrayIn.quaternions[i].header.stamp.nsec);
@@ -132,7 +132,7 @@
};
// std::cout << "recieved quaternion frame: " << tfArrayIn.quaternions[i].header.frame_id << " with parent:" << tfArrayIn.quaternions[i].parent << std::endl;
}
- for (unsigned int i = 0; i < tfArrayIn.matrices_size; i++)
+ for (unsigned int i = 0; i < tfArrayIn.get_matrices_size(); i++)
{
std::cerr << "receiveArray: setWithMatrix failed No longer supported" << std::endl;
}
Modified: pkg/trunk/util/tf/test/tf_unittest.cpp
===================================================================
--- pkg/trunk/util/tf/test/tf_unittest.cpp 2008-09-29 18:18:55 UTC (rev 4770)
+++ pkg/trunk/util/tf/test/tf_unittest.cpp 2008-09-29 18:37:43 UTC (rev 4771)
@@ -166,6 +166,15 @@
}
}
+ Stamped<btTransform> inpose (btTransform(btQuaternion(0,0,0), btVector3(0,0,0)), runs, "child");
+ Stamped<btTransform> outpose;
+ outpose.data_.setIdentity(); //to make sure things are getting mutated
+ mTR.transformStamped("child",inpose, outpose);
+ EXPECT_NEAR(outpose.data_.getOrigin().x(), 0, epsilon);
+ EXPECT_NEAR(outpose.data_.getOrigin().y(), 0, epsilon);
+ EXPECT_NEAR(outpose.data_.getOrigin().z(), 0, epsilon);
+
+
}
TEST(tf, TransformVector3Cartesian)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-09-29 23:11:57
|
Revision: 4791
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4791&view=rev
Author: tfoote
Date: 2008-09-29 23:11:18 +0000 (Mon, 29 Sep 2008)
Log Message:
-----------
2dnav-gazebo is now stl msg compliant
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_position_controller.cpp
pkg/trunk/drivers/cam/axis_cam/src/axis_cam/axis_cam.cpp
pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Stereo_Camera.cc
pkg/trunk/highlevel/highlevel_controllers/src/MoveArm.cpp
pkg/trunk/highlevel/highlevel_controllers/src/MoveEndEffector.cpp
pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
pkg/trunk/motion_planning/kinematic_planning/include/RKPBasicRequest.h
pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp
pkg/trunk/trajectory_rollout/src/governor_node.cpp
pkg/trunk/vision/scan_utils/include/octree.h
pkg/trunk/vision/scan_utils/src/listen_node/scanListenNode.cpp
pkg/trunk/vision/scan_utils/src/smartScan.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/planning_visualizer.cpp
pkg/trunk/visualization/wx_camera_panel/src/wx_camera_panel/CameraPanel.cpp
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_position_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_position_controller.cpp 2008-09-29 23:08:57 UTC (rev 4790)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/arm_position_controller.cpp 2008-09-29 23:11:18 UTC (rev 4791)
@@ -304,7 +304,7 @@
bool ArmPositionControllerNode::setJointPosSingle(const pr2_mechanism_controllers::JointPosCmd & cmd)
{
- for(unsigned int i=0;i<cmd.positions_size;++i)
+ for(unsigned int i=0;i<cmd.get_positions_size();++i)
std::cout<<cmd.positions[i]<<' ';
std::cout<<std::flush;
c_->setJointPosCmd(cmd);
@@ -332,14 +332,14 @@
// msg
std::cout<<"waypoint "<<std::flush;
- for(unsigned int i=0;i<cmd.positions_size;++i)
+ for(unsigned int i=0;i<cmd.get_positions_size();++i)
std::cout << cmd.names[i] << "==" << cmd.positions[i] <<' ';
std::cout<<std::flush;
std::cout<<" headless"<<std::endl;
// Removes any error margin info so that the controller returns true on target reached and frees any blocking call
- cmd.set_margins_size(cmd.positions_size);
- for(unsigned int i=0;i<cmd.positions_size;++i)
+ cmd.set_margins_size(cmd.get_positions_size());
+ for(unsigned int i=0;i<cmd.get_positions_size();++i)
cmd.margins[i]=-1;
c_->setJointPosCmd(cmd);
Modified: pkg/trunk/drivers/cam/axis_cam/src/axis_cam/axis_cam.cpp
===================================================================
--- pkg/trunk/drivers/cam/axis_cam/src/axis_cam/axis_cam.cpp 2008-09-29 23:08:57 UTC (rev 4790)
+++ pkg/trunk/drivers/cam/axis_cam/src/axis_cam/axis_cam.cpp 2008-09-29 23:11:18 UTC (rev 4791)
@@ -103,7 +103,7 @@
}
images.images[0].set_data_size(jpeg_size);
- memcpy(images.images[0].data, jpeg, jpeg_size);
+ memcpy(&images.images[0].data[0], jpeg, jpeg_size);
images.images[0].compression = "jpeg";
@@ -159,7 +159,7 @@
images.images[0].set_data_size(jpeg_size);
- memcpy(images.images[0].data, jpeg, jpeg_size);
+ memcpy(&images.images[0].data[0], jpeg, jpeg_size);
images.images[0].compression = "jpeg";
if (!codec->inflate_header())
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Stereo_Camera.cc
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Stereo_Camera.cc 2008-09-29 23:08:57 UTC (rev 4790)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Stereo_Camera.cc 2008-09-29 23:11:18 UTC (rev 4791)
@@ -355,7 +355,9 @@
uint32_t buf_size = (width) * (height) * (depth);
this->imageMsg[camera].set_data_size(buf_size);
- this->imageMsg[camera].data = (unsigned char*)rgb_src;
+ ///\todo FIXME checkme John
+ memcpy(&(this->imageMsg[camera].data[0]), rgb_src, buf_size);
+ // this->imageMsg[camera].data = (unsigned char*)rgb_src;
// publish to ros
if (camera==0)
Modified: pkg/trunk/highlevel/highlevel_controllers/src/MoveArm.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/MoveArm.cpp 2008-09-29 23:08:57 UTC (rev 4790)
+++ pkg/trunk/highlevel/highlevel_controllers/src/MoveArm.cpp 2008-09-29 23:11:18 UTC (rev 4791)
@@ -198,7 +198,7 @@
req.start_state.set_vals_size(45);
//initializing full value state
- for (unsigned int i = 0 ; i < req.start_state.vals_size ; ++i) {
+ for (unsigned int i = 0 ; i < req.start_state.get_vals_size() ; ++i) {
req.start_state.vals[i] = 0.0;
}
Modified: pkg/trunk/highlevel/highlevel_controllers/src/MoveEndEffector.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/MoveEndEffector.cpp 2008-09-29 23:08:57 UTC (rev 4790)
+++ pkg/trunk/highlevel/highlevel_controllers/src/MoveEndEffector.cpp 2008-09-29 23:11:18 UTC (rev 4791)
@@ -192,7 +192,7 @@
req.start_state.set_vals_size(45);
//initializing full value state
- for (unsigned int i = 0 ; i < req.start_state.vals_size ; ++i) {
+ for (unsigned int i = 0 ; i < req.start_state.get_vals_size() ; ++i) {
req.start_state.vals[i] = 0.0;
}
Modified: pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-09-29 23:08:57 UTC (rev 4790)
+++ pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-09-29 23:11:18 UTC (rev 4791)
@@ -247,7 +247,7 @@
assert(mc_->model_.joints_.size() == publisher_.msg_.get_joint_states_size());
for (unsigned int i = 0; i < mc_->model_.joints_.size(); ++i)
{
- mechanism_control::JointState *out = publisher_.msg_.joint_states + i;
+ mechanism_control::JointState *out = &publisher_.msg_.joint_states[i];
mechanism::JointState *in = &mc_->state_->joint_states_[i];
out->name = mc_->model_.joints_[i]->name_;
out->position = in->position_;
@@ -258,7 +258,7 @@
for (unsigned int i = 0; i < mc_->hw_->actuators_.size(); ++i)
{
- mechanism_control::ActuatorState *out = publisher_.msg_.actuator_states + i;
+ mechanism_control::ActuatorState *out = &publisher_.msg_.actuator_states[i];
ActuatorState *in = &mc_->hw_->actuators_[i]->state_;
out->name = mc_->hw_->actuators_[i]->name_;
out->encoder_count = in->encoder_count_;
Modified: pkg/trunk/motion_planning/kinematic_planning/include/RKPBasicRequest.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/RKPBasicRequest.h 2008-09-29 23:08:57 UTC (rev 4790)
+++ pkg/trunk/motion_planning/kinematic_planning/include/RKPBasicRequest.h 2008-09-29 23:11:18 UTC (rev 4791)
@@ -132,7 +132,7 @@
if (model->groupID >= 0)
{
/* set the pose of the whole robot */
- model->kmodel->computeTransforms(start_state.vals);
+ model->kmodel->computeTransforms(&start_state.vals[0]);
model->collisionSpace->updateRobotModel(model->collisionSpaceID);
/* extract the components needed for the start state of the desired group */
Modified: pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp 2008-09-29 23:08:57 UTC (rev 4790)
+++ pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp 2008-09-29 23:11:18 UTC (rev 4791)
@@ -165,7 +165,7 @@
if (model->groupID >= 0)
{
/* set the pose of the whole robot */
- model->kmodel->computeTransforms(req.start_state.vals);
+ model->kmodel->computeTransforms(&req.start_state.vals[0]);
model->collisionSpace->updateRobotModel(model->collisionSpaceID);
/* extract the components needed for the start state of the desired group */
Modified: pkg/trunk/trajectory_rollout/src/governor_node.cpp
===================================================================
--- pkg/trunk/trajectory_rollout/src/governor_node.cpp 2008-09-29 23:08:57 UTC (rev 4790)
+++ pkg/trunk/trajectory_rollout/src/governor_node.cpp 2008-09-29 23:11:18 UTC (rev 4791)
@@ -83,7 +83,7 @@
//update the global plan from the message
vector<std_msgs::Point2DFloat32> plan;
- for(unsigned int i = 0; i < plan_msg_.plan.path_size; ++i){
+ for(unsigned int i = 0; i < plan_msg_.plan.get_path_size(); ++i){
plan.push_back(plan_msg_.plan.path[i]);
}
tc_.updatePlan(plan);
Modified: pkg/trunk/vision/scan_utils/include/octree.h
===================================================================
--- pkg/trunk/vision/scan_utils/include/octree.h 2008-09-29 23:08:57 UTC (rev 4790)
+++ pkg/trunk/vision/scan_utils/include/octree.h 2008-09-29 23:11:18 UTC (rev 4791)
@@ -805,11 +805,11 @@
if (msg.uses_timestamps) mUsesTimestamps = true;
else mUsesTimestamps = false;
- memcpy((char*)&mEmptyValue, msg.empty_value, sizeof(mEmptyValue) );
+ memcpy((char*)&mEmptyValue, &msg.empty_value[0], sizeof(mEmptyValue) );
unsigned int size = msg.get_structure_data_size();
char *data = new char[size];
- memcpy(data, msg.structure_data, size);
+ memcpy(data, &msg.structure_data[0], size);
bool result = deserialize(data,size);
delete[] data;
if (!result) {
@@ -836,13 +836,13 @@
else msg.uses_timestamps = 0;
msg.set_empty_value_size(sizeof(mEmptyValue));
- memcpy(msg.empty_value, (char*)&mEmptyValue, sizeof(mEmptyValue));
+ memcpy(&msg.empty_value[0], (char*)&mEmptyValue, sizeof(mEmptyValue));
unsigned int size;
char *data;
serialize(&data, &size);
msg.set_structure_data_size(size);
- memcpy(msg.structure_data, data, size);
+ memcpy(&msg.structure_data[0], data, size);
delete [] data;
}
Modified: pkg/trunk/vision/scan_utils/src/listen_node/scanListenNode.cpp
===================================================================
--- pkg/trunk/vision/scan_utils/src/listen_node/scanListenNode.cpp 2008-09-29 23:08:57 UTC (rev 4790)
+++ pkg/trunk/vision/scan_utils/src/listen_node/scanListenNode.cpp 2008-09-29 23:11:18 UTC (rev 4791)
@@ -86,7 +86,7 @@
}
SmartScan *retScan = new SmartScan();
- retScan->setPoints( cloud.get_pts_size(), cloud.pts);
+ retScan->setPoints( cloud.get_pts_size(), &cloud.pts[0]);
return retScan;
}
Modified: pkg/trunk/vision/scan_utils/src/smartScan.cpp
===================================================================
--- pkg/trunk/vision/scan_utils/src/smartScan.cpp 2008-09-29 23:08:57 UTC (rev 4790)
+++ pkg/trunk/vision/scan_utils/src/smartScan.cpp 2008-09-29 23:11:18 UTC (rev 4791)
@@ -164,7 +164,7 @@
void SmartScan::setFromRosCloud(const std_msgs::PointCloudFloat32 &cloud)
{
//for the moment we ignore intensity values
- setPoints( cloud.get_pts_size(), cloud.pts);
+ setPoints( cloud.get_pts_size(), &cloud.pts[0]);
}
/*! Returns the SmartScan as a ROS PointCloudFloat32 message.
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/planning_visualizer.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/planning_visualizer.cpp 2008-09-29 23:08:57 UTC (rev 4790)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/planning_visualizer.cpp 2008-09-29 23:11:18 UTC (rev 4791)
@@ -204,7 +204,7 @@
current_state_ = -1;
current_state_time_ = state_display_time_ + 1.0f;
- kinematic_model_->computeTransforms(displaying_kinematic_path_message_.start_state.vals);
+ kinematic_model_->computeTransforms(&displaying_kinematic_path_message_.start_state.vals[0]);
robot_->update( kinematic_model_, target_frame_ );
}
@@ -221,7 +221,7 @@
if ( (size_t)current_state_ < displaying_kinematic_path_message_.path.get_states_size() )
{
int group_id = kinematic_model_->getGroupID( displaying_kinematic_path_message_.model_name );
- kinematic_model_->computeTransforms(displaying_kinematic_path_message_.path.states[ current_state_ ].vals, group_id);
+ kinematic_model_->computeTransforms(&displaying_kinematic_path_message_.path.states[ current_state_ ].vals[0], group_id);
robot_->update( kinematic_model_, target_frame_ );
causeRender();
Modified: pkg/trunk/visualization/wx_camera_panel/src/wx_camera_panel/CameraPanel.cpp
===================================================================
--- pkg/trunk/visualization/wx_camera_panel/src/wx_camera_panel/CameraPanel.cpp 2008-09-29 23:08:57 UTC (rev 4790)
+++ pkg/trunk/visualization/wx_camera_panel/src/wx_camera_panel/CameraPanel.cpp 2008-09-29 23:11:18 UTC (rev 4791)
@@ -368,7 +368,7 @@
delete [] image_data_;
const uint32_t dataSize = image_message_.get_data_size();
image_data_ = new uint8_t[ dataSize ];
- memcpy( image_data_, image_message_.data, dataSize );
+ memcpy( image_data_, &image_message_.data[0], dataSize );
delete image_;
wxMemoryInputStream memoryStream( image_data_, dataSize );
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-10-01 23:27:55
|
Revision: 4904
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4904&view=rev
Author: hsujohnhsu
Date: 2008-10-01 23:27:41 +0000 (Wed, 01 Oct 2008)
Log Message:
-----------
commented out pose compensation in Gazebo.
Modified Paths:
--------------
pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
pkg/trunk/robot_descriptions/gazebo_robot_description/src/urdf2gazebo.cpp
pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/pr2_dual_link.xml
Modified: pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
===================================================================
--- pkg/trunk/3rdparty/gazebo/gazebo_patch.diff 2008-10-01 23:16:51 UTC (rev 4903)
+++ pkg/trunk/3rdparty/gazebo/gazebo_patch.diff 2008-10-01 23:27:41 UTC (rev 4904)
@@ -556,6 +556,14 @@
// Return the ID of this body
dBodyID Body::GetId() const
{
+@@ -505,6 +590,7 @@
+ */
+ void Body::UpdateCoM()
+ {
++ return;
+ const dMass *lmass;
+ Pose3d oldPose, newPose, pose;
+ std::map< std::string, Geom* >::iterator giter;
Index: server/physics/HingeJoint.cc
===================================================================
--- server/physics/HingeJoint.cc (revision 7049)
@@ -1168,18 +1176,6 @@
this->RTTMode="PBuffer";
}
}
-Index: server/gui/StatusBar.cc
-===================================================================
---- server/gui/StatusBar.cc (revision 7049)
-+++ server/gui/StatusBar.cc (working copy)
-@@ -25,6 +25,7 @@
- */
-
- #include <stdio.h>
-+#include <string.h>
- #include <FL/Fl_Value_Output.H>
- #include <FL/Fl_Output.H>
- #include <FL/Fl_Button.H>
Index: server/Model.cc
===================================================================
--- server/Model.cc (revision 7049)
@@ -1251,6 +1247,18 @@
return this->UpdateChild();
}
+Index: server/gui/StatusBar.cc
+===================================================================
+--- server/gui/StatusBar.cc (revision 7049)
++++ server/gui/StatusBar.cc (working copy)
+@@ -25,6 +25,7 @@
+ */
+
+ #include <stdio.h>
++#include <string.h>
+ #include <FL/Fl_Value_Output.H>
+ #include <FL/Fl_Output.H>
+ #include <FL/Fl_Button.H>
Index: server/World.hh
===================================================================
--- server/World.hh (revision 7049)
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/src/urdf2gazebo.cpp
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/src/urdf2gazebo.cpp 2008-10-01 23:16:51 UTC (rev 4903)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/src/urdf2gazebo.cpp 2008-10-01 23:27:41 UTC (rev 4904)
@@ -315,7 +315,7 @@
for (int j = 0 ; j < 3 ; ++j)
{
// undo Gazebo's shift of object anchor to geom xyz, stay in body cs
- tmpAnchor[j] = (link->joint->anchor)[j] - 0.0*(link->inertial->com)[j] - 0.5*(link->collision->xyz)[j]; /// @todo compensate for gazebo's error. John is fixing this one
+ tmpAnchor[j] = (link->joint->anchor)[j] - 0.0*(link->inertial->com)[j] - 0.0*(link->collision->xyz)[j]; /// @todo compensate for gazebo's error. John is fixing this one
}
addKeyValue(joint, "anchorOffset", values2str(3, tmpAnchor));
Modified: pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/pr2_dual_link.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/pr2_dual_link.xml 2008-10-01 23:16:51 UTC (rev 4903)
+++ pkg/trunk/robot_descriptions/wg_robot_description/dual_link_test/pr2_dual_link.xml 2008-10-01 23:27:41 UTC (rev 4904)
@@ -50,7 +50,7 @@
<link name="link1">
<parent name="base_block" />
- <origin xyz="0 0 1" rpy="0 0 0" />
+ <origin xyz="0 0 2" rpy="0 0 0" />
<joint name="link1_joint" />
<inertial >
<mass value="10" />
@@ -85,7 +85,7 @@
<joint name="link2_joint" />
<inertial >
<mass value="10" />
- <com xyz="0 0 0" />
+ <com xyz="0.0 0 0" />
<inertia ixx="0.1" ixy="0.0" ixz="0.0" iyy="0.1" iyz="0.0" izz="0.1" />
</inertial>
<visual >
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2008-10-03 05:59:05
|
Revision: 4993
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4993&view=rev
Author: gerkey
Date: 2008-10-03 05:59:04 +0000 (Fri, 03 Oct 2008)
Log Message:
-----------
moved issue_kinematic_planning to deprecated because it uses old message formats and doesn't build
Added Paths:
-----------
pkg/trunk/deprecated/issue_kinematic_plan/
Removed Paths:
-------------
pkg/trunk/motion_planning/issue_kinematic_plan/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tpr...@us...> - 2008-10-05 21:05:38
|
Revision: 5066
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=5066&view=rev
Author: tpratkanis
Date: 2008-10-05 21:05:36 +0000 (Sun, 05 Oct 2008)
Log Message:
-----------
Fix bug 389: not using named path output in the name-value service.
Modified Paths:
--------------
pkg/trunk/highlevel/highlevel_controllers/src/MoveArm.cpp
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/robot_srvs/srv/NamedKinematicPlanState.srv
Added Paths:
-----------
pkg/trunk/robot_msgs/msg/NamedKinematicPath.msg
Modified: pkg/trunk/highlevel/highlevel_controllers/src/MoveArm.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/MoveArm.cpp 2008-10-05 20:16:37 UTC (rev 5065)
+++ pkg/trunk/highlevel/highlevel_controllers/src/MoveArm.cpp 2008-10-05 21:05:36 UTC (rev 5066)
@@ -295,14 +295,15 @@
/**
* @brief Iterate over all published joint values we match on
+ * @todo Does not handle joints with multiple axes.
*/
bool MoveArm::withinBounds(unsigned waypointIndex){
double sum_joint_diff = 0.0;
mechanismState.lock();
- for(unsigned int i=0; i<jointNames_.size(); i++){
+ for(unsigned int i=0; i<plan.path.states[waypointIndex].get_joints_size(); i++){
double value;
- if(readJointValue(mechanismState, jointNames_[i], value))
- sum_joint_diff += fabs(value - plan.path.states[waypointIndex].vals[i]);
+ if(readJointValue(mechanismState, plan.path.states[waypointIndex].names[i], value))
+ sum_joint_diff += fabs(value - plan.path.states[waypointIndex].joints[i].vals[0]);
}
mechanismState.unlock();
@@ -325,21 +326,26 @@
setCommandParameters(armCommand);
- std::cout << "Dispatching state for waypoint [" << currentWaypoint << "]: "
- << plan.path.states[currentWaypoint].vals[0] << " "
- << plan.path.states[currentWaypoint].vals[1] << " "
- << plan.path.states[currentWaypoint].vals[2] << " "
- << plan.path.states[currentWaypoint].vals[3] << " "
- << plan.path.states[currentWaypoint].vals[4] << " "
- << plan.path.states[currentWaypoint].vals[5] << " "
- << plan.path.states[currentWaypoint].vals[6] << std::endl;
+ std::cout << "Dispatching state for waypoint [" << currentWaypoint << "]: ";
+
+ for(unsigned int i=0; i<plan.path.states[currentWaypoint].get_joints_size(); i++){
+ std::cout << plan.path.states[currentWaypoint].names[i] << " (";
+ for (unsigned int k=0; k<plan.path.states[currentWaypoint].joints[i].get_vals_size(); k++) {
+ std::cout << plan.path.states[currentWaypoint].joints[i].vals[k] << ",";
+ }
+ std::cout << ") ";
+ }
+ std::cout << std::endl;
+
publish(armCmdTopic, armCommand);
return true;
}
-
+/**
+ * @todo Multi-axis joints.
+ */
void MoveArm::setCommandParameters(pr2_mechanism_controllers::JointPosCmd& armCommand){
static const double TOLERANCE(0.25);
@@ -349,8 +355,8 @@
armCommand.set_margins_size(jointNames_.size());
for(unsigned int i = 0; i < jointNames_.size(); i++){
- armCommand.names[i] = jointNames_[i];
- armCommand.positions[i] = plan.path.states[currentWaypoint].vals[i];
+ armCommand.names[i] = plan.path.states[currentWaypoint].names[i];
+ armCommand.positions[i] = plan.path.states[currentWaypoint].joints[i].vals[0];
armCommand.margins[i] = TOLERANCE;
}
}
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-10-05 20:16:37 UTC (rev 5065)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-10-05 21:05:36 UTC (rev 5066)
@@ -290,8 +290,32 @@
req.allowed_time = reqn.allowed_time;
req.threshold = reqn.threshold;
+
+
+ //Acutally run the service.
bool r = m_requestState.execute(m_models, req, res.path, res.distance);
- resn.path = res.path;
+
+
+ //Copy the path.
+ resn.path.set_states_size(res.path.get_states_size());
+
+ for (unsigned int j = 0; j < resn.path.get_states_size(); j++) {
+ nparam = 0;
+ resn.path.states[j].set_names_size(subspaceJointMap.size());
+ resn.path.states[j].set_joints_size(subspaceJointMap.size());
+ for (unsigned int i = 0; i < subspaceJointMap.size(); i++) {
+ resn.path.states[j].names[i] = subspaceJointMap[i]->name;
+ resn.path.states[j].joints[i].set_vals_size(subspaceJointMap[i]->usedParams);
+ for (unsigned int k = 0; k < subspaceJointMap[i]->usedParams; k++) {
+ resn.path.states[j].joints[i].vals[k] = res.path.states[j].vals[nparam];
+ nparam++;
+ }
+ }
+ }
+
+
+
+ //Simply copy the other results.
resn.distance = res.distance;
return r;
}
Added: pkg/trunk/robot_msgs/msg/NamedKinematicPath.msg
===================================================================
--- pkg/trunk/robot_msgs/msg/NamedKinematicPath.msg (rev 0)
+++ pkg/trunk/robot_msgs/msg/NamedKinematicPath.msg 2008-10-05 21:05:36 UTC (rev 5066)
@@ -0,0 +1,3 @@
+# The definition of a kinematic path. Simply a list of states
+
+NamedKinematicState[] states
Modified: pkg/trunk/robot_srvs/srv/NamedKinematicPlanState.srv
===================================================================
--- pkg/trunk/robot_srvs/srv/NamedKinematicPlanState.srv 2008-10-05 20:16:37 UTC (rev 5065)
+++ pkg/trunk/robot_srvs/srv/NamedKinematicPlanState.srv 2008-10-05 21:05:36 UTC (rev 5066)
@@ -45,7 +45,7 @@
# includes start and goal states) to define the motions for the
# robot. If more intermediate states are needed, linear interpolation
# is to be assumed.
-robot_msgs/KinematicPath path
+robot_msgs/NamedKinematicPath path
# The threshold that was actually achieved. If the planner did not have enough time,
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|