void GazeboRosTest::UpdateChild()
{
// 状態アップデート
// Copies the commands from the mechanism joints into the gazebo joints.
ROS_ERROR("num=%d", this->joints_.size());
for (unsigned int i = 0; i < this->joints_.size(); ++i)
{
if (!this->joints_[i])
continue;
// double damping_coef = 100.0;
switch (this->joints_[i]->GetType())
{
case Joint::HINGE: {
// PD control
HingeJoint *hj = (HingeJoint*)this->joints_[i];
double current_velocity = hj->GetAngleRate();
double current_angle = hj->GetAngle();
double damping_force = dgain_ * (0 - current_velocity);
double diff_force = pgain_ * (cmd_[i] - current_angle);
double effort_command = diff_force + damping_force;
ROS_ERROR("c=%f,cmd=%f, f=%f, v=%f", current_angle, cmd_[i], effort_command, current_velocity);
// hj->SetTorque(effort_command);
hj->SetParam(dParamVel, effort_command);
break;
}
default:
abort();
}
}
}
void GazeboMechanismControl::UpdateChild()
{
assert(joints_.size() == fake_state_->joint_states_.size());
assert(joints_.size() == joints_damping_.size());
//--------------------------------------------------
// Pushes out simulation state
//--------------------------------------------------
// Copies the state from the gazebo joints into the mechanism joints.
for (unsigned int i = 0; i < joints_.size(); ++i)
{
if (!joints_[i])
continue;
fake_state_->joint_states_[i].applied_effort_ = fake_state_->joint_states_[i].commanded_effort_;
switch(joints_[i]->GetType())
{
case Joint::HINGE: {
HingeJoint *hj = (HingeJoint*)joints_[i];
fake_state_->joint_states_[i].position_ = hj->GetAngle();
fake_state_->joint_states_[i].velocity_ = hj->GetAngleRate();
break;
}
case Joint::SLIDER: {
SliderJoint *sj = (SliderJoint*)joints_[i];
fake_state_->joint_states_[i].position_ = sj->GetPosition();
fake_state_->joint_states_[i].velocity_ = sj->GetPositionRate();
break;
}
default:
abort();
}
}
// Reverses the transmissions to propagate the joint position into the actuators.
fake_state_->propagateStateBackwards();
//--------------------------------------------------
// Runs Mechanism Control
//--------------------------------------------------
hw_.current_time_ = Simulator::Instance()->GetSimTime();
try
{
mcn_.update();
}
catch (const char* c)
{
if (strcmp(c,"dividebyzero")==0)
std::cout << "WARNING:pid controller reports divide by zero error" << std::endl;
else
std::cout << "unknown const char* exception: " << c << std::endl;
}
//--------------------------------------------------
// Takes in actuation commands
//--------------------------------------------------
// Reverses the transmissions to propagate the actuator commands into the joints.
fake_state_->propagateEffortBackwards();
// Copies the commands from the mechanism joints into the gazebo joints.
for (unsigned int i = 0; i < joints_.size(); ++i)
{
if (!joints_[i])
continue;
double damping_force;
double effort = fake_state_->joint_states_[i].commanded_effort_;
switch (joints_[i]->GetType())
{
case Joint::HINGE:
damping_force = joints_damping_[i] * ((HingeJoint*)joints_[i])->GetAngleRate();
((HingeJoint*)joints_[i])->SetTorque(effort - damping_force);
break;
case Joint::SLIDER:
damping_force = joints_damping_[i] * ((SliderJoint*)joints_[i])->GetPositionRate();
((SliderJoint*)joints_[i])->SetSliderForce(effort - damping_force);
break;
default:
abort();
}
}
}