200 lines
8.0 KiB
C++
200 lines
8.0 KiB
C++
|
/*
|
||
|
|
* Copyright (C) 2012 Open Source Robotics Foundation
|
||
|
|
*
|
||
|
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
||
|
|
* you may not use this file except in compliance with the License.
|
||
|
|
* You may obtain a copy of the License at
|
||
|
|
*
|
||
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
||
|
|
*
|
||
|
|
* Unless required by applicable law or agreed to in writing, software
|
||
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
||
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||
|
|
* See the License for the specific language governing permissions and
|
||
|
|
* limitations under the License.
|
||
|
|
*
|
||
|
|
*/
|
||
|
|
|
||
|
|
#include "gazebo/physics/physics.hh"
|
||
|
|
#include "plugins/SphereAtlasDemoPlugin.hh"
|
||
|
|
|
||
|
|
using namespace gazebo;
|
||
|
|
|
||
|
|
GZ_REGISTER_MODEL_PLUGIN(SphereAtlasDemoPlugin)
|
||
|
|
|
||
|
|
/////////////////////////////////////////////////
|
||
|
|
SphereAtlasDemoPlugin::SphereAtlasDemoPlugin()
|
||
|
|
{
|
||
|
|
}
|
||
|
|
|
||
|
|
/////////////////////////////////////////////////
|
||
|
|
void SphereAtlasDemoPlugin::Load(physics::ModelPtr _model,
|
||
|
|
sdf::ElementPtr /*_sdf*/)
|
||
|
|
{
|
||
|
|
this->model = _model;
|
||
|
|
|
||
|
|
this->jointNames.push_back("l_leg_hpz");
|
||
|
|
this->jointNames.push_back("l_leg_hpx");
|
||
|
|
this->jointNames.push_back("l_leg_hpy");
|
||
|
|
this->jointNames.push_back("l_leg_kny");
|
||
|
|
this->jointNames.push_back("l_leg_aky");
|
||
|
|
this->jointNames.push_back("l_leg_akx");
|
||
|
|
this->jointNames.push_back("r_leg_hpz");
|
||
|
|
this->jointNames.push_back("r_leg_hpx");
|
||
|
|
this->jointNames.push_back("r_leg_hpy");
|
||
|
|
this->jointNames.push_back("r_leg_kny");
|
||
|
|
this->jointNames.push_back("r_leg_aky");
|
||
|
|
this->jointNames.push_back("r_leg_akx");
|
||
|
|
this->jointNames.push_back("l_arm_shy");
|
||
|
|
this->jointNames.push_back("l_arm_shx");
|
||
|
|
this->jointNames.push_back("l_arm_ely");
|
||
|
|
this->jointNames.push_back("l_arm_elx");
|
||
|
|
this->jointNames.push_back("l_arm_wry");
|
||
|
|
this->jointNames.push_back("l_arm_wrx");
|
||
|
|
this->jointNames.push_back("r_arm_shy");
|
||
|
|
this->jointNames.push_back("r_arm_shx");
|
||
|
|
this->jointNames.push_back("r_arm_ely");
|
||
|
|
this->jointNames.push_back("r_arm_elx");
|
||
|
|
this->jointNames.push_back("r_arm_wry");
|
||
|
|
this->jointNames.push_back("r_arm_wrx");
|
||
|
|
this->jointNames.push_back("neck_ry");
|
||
|
|
this->jointNames.push_back("back_bkz");
|
||
|
|
this->jointNames.push_back("back_bky");
|
||
|
|
this->jointNames.push_back("back_bkx");
|
||
|
|
|
||
|
|
this->jointKp.push_back( 1000.0); // l_leg_hpz NOLINT
|
||
|
|
this->jointKp.push_back( 1000.0); // l_leg_hpx NOLINT
|
||
|
|
this->jointKp.push_back( 2000.0); // l_leg_hpy NOLINT
|
||
|
|
this->jointKp.push_back( 5000.0); // l_leg_kny NOLINT
|
||
|
|
this->jointKp.push_back( 3000.0); // l_leg_aky NOLINT
|
||
|
|
this->jointKp.push_back( 1000.0); // l_leg_akx NOLINT
|
||
|
|
this->jointKp.push_back( 1000.0); // r_leg_hpz NOLINT
|
||
|
|
this->jointKp.push_back( 1000.0); // r_leg_hpx NOLINT
|
||
|
|
this->jointKp.push_back( 2000.0); // r_leg_hpy NOLINT
|
||
|
|
this->jointKp.push_back( 5000.0); // r_leg_kny NOLINT
|
||
|
|
this->jointKp.push_back( 3000.0); // r_leg_aky NOLINT
|
||
|
|
this->jointKp.push_back( 1000.0); // r_leg_akx NOLINT
|
||
|
|
this->jointKp.push_back( 2000.0); // l_arm_shy NOLINT
|
||
|
|
this->jointKp.push_back( 1000.0); // l_arm_shx NOLINT
|
||
|
|
this->jointKp.push_back( 200.0); // l_arm_ely NOLINT
|
||
|
|
this->jointKp.push_back( 200.0); // l_arm_elx NOLINT
|
||
|
|
this->jointKp.push_back( 50.0); // l_arm_wry NOLINT
|
||
|
|
this->jointKp.push_back( 100.0); // l_arm_wrx NOLINT
|
||
|
|
this->jointKp.push_back( 2000.0); // r_arm_shy NOLINT
|
||
|
|
this->jointKp.push_back( 1000.0); // r_arm_shx NOLINT
|
||
|
|
this->jointKp.push_back( 200.0); // r_arm_ely NOLINT
|
||
|
|
this->jointKp.push_back( 200.0); // r_arm_elx NOLINT
|
||
|
|
this->jointKp.push_back( 50.0); // r_arm_wry NOLINT
|
||
|
|
this->jointKp.push_back( 100.0); // r_arm_wrx NOLINT
|
||
|
|
this->jointKp.push_back( 1000.0); // neck_ry NOLINT
|
||
|
|
this->jointKp.push_back( 20000.0); // back_bkz NOLINT
|
||
|
|
this->jointKp.push_back(200000.0); // back_bky NOLINT
|
||
|
|
this->jointKp.push_back(200000.0); // back_bkx NOLINT
|
||
|
|
|
||
|
|
this->jointKd.push_back( 0.01); // l_leg_hpz NOLINT
|
||
|
|
this->jointKd.push_back( 1.00); // l_leg_hpx NOLINT
|
||
|
|
this->jointKd.push_back(10.00); // l_leg_hpy NOLINT
|
||
|
|
this->jointKd.push_back(10.00); // l_leg_kny NOLINT
|
||
|
|
this->jointKd.push_back( 2.00); // l_leg_aky NOLINT
|
||
|
|
this->jointKd.push_back( 1.00); // l_leg_akx NOLINT
|
||
|
|
this->jointKd.push_back( 0.01); // r_leg_hpz NOLINT
|
||
|
|
this->jointKd.push_back( 1.00); // r_leg_hpx NOLINT
|
||
|
|
this->jointKd.push_back(10.00); // r_leg_hpy NOLINT
|
||
|
|
this->jointKd.push_back(10.00); // r_leg_kny NOLINT
|
||
|
|
this->jointKd.push_back( 2.00); // r_leg_aky NOLINT
|
||
|
|
this->jointKd.push_back( 1.00); // r_leg_akx NOLINT
|
||
|
|
this->jointKd.push_back( 3.00); // l_arm_shy NOLINT
|
||
|
|
this->jointKd.push_back(10.00); // l_arm_shx NOLINT
|
||
|
|
this->jointKd.push_back( 3.00); // l_arm_ely NOLINT
|
||
|
|
this->jointKd.push_back( 3.00); // l_arm_elx NOLINT
|
||
|
|
this->jointKd.push_back( 0.10); // l_arm_wry NOLINT
|
||
|
|
this->jointKd.push_back( 0.20); // l_arm_wrx NOLINT
|
||
|
|
this->jointKd.push_back( 3.00); // r_arm_shy NOLINT
|
||
|
|
this->jointKd.push_back(10.00); // r_arm_shx NOLINT
|
||
|
|
this->jointKd.push_back( 3.00); // r_arm_ely NOLINT
|
||
|
|
this->jointKd.push_back( 3.00); // r_arm_elx NOLINT
|
||
|
|
this->jointKd.push_back( 0.10); // r_arm_wry NOLINT
|
||
|
|
this->jointKd.push_back( 0.20); // r_arm_wrx NOLINT
|
||
|
|
this->jointKd.push_back( 1.00); // neck_ry NOLINT
|
||
|
|
this->jointKd.push_back( 1.00); // back_bkz NOLINT
|
||
|
|
this->jointKd.push_back( 2.00); // back_bky NOLINT
|
||
|
|
this->jointKd.push_back( 1.00); // back_bkx NOLINT
|
||
|
|
|
||
|
|
this->qp.push_back(0.0);
|
||
|
|
this->qp.push_back(0.0);
|
||
|
|
this->qp.push_back(0.0);
|
||
|
|
this->qp.push_back(0.0);
|
||
|
|
this->qp.push_back(0.0);
|
||
|
|
this->qp.push_back(0.0);
|
||
|
|
this->qp.push_back(0.0);
|
||
|
|
this->qp.push_back(0.0);
|
||
|
|
this->qp.push_back(0.0);
|
||
|
|
this->qp.push_back(0.0);
|
||
|
|
this->qp.push_back(0.0);
|
||
|
|
this->qp.push_back(0.0);
|
||
|
|
this->qp.push_back(0.0);
|
||
|
|
this->qp.push_back(0.0);
|
||
|
|
this->qp.push_back(0.0);
|
||
|
|
this->qp.push_back(0.0);
|
||
|
|
this->qp.push_back(0.0);
|
||
|
|
this->qp.push_back(0.0);
|
||
|
|
this->qp.push_back(0.0);
|
||
|
|
this->qp.push_back(0.0);
|
||
|
|
this->qp.push_back(0.0);
|
||
|
|
this->qp.push_back(0.0);
|
||
|
|
this->qp.push_back(0.0);
|
||
|
|
this->qp.push_back(0.0);
|
||
|
|
this->qp.push_back(0.0);
|
||
|
|
this->qp.push_back(0.0);
|
||
|
|
this->qp.push_back(0.0);
|
||
|
|
this->qp.push_back(0.0);
|
||
|
|
|
||
|
|
for (unsigned int i = 0; i < this->jointNames.size(); ++i)
|
||
|
|
{
|
||
|
|
physics::JointPtr j = model->GetJoint(this->jointNames[i]);
|
||
|
|
this->joints.push_back(j);
|
||
|
|
}
|
||
|
|
|
||
|
|
this->updateConnection = event::Events::ConnectWorldUpdateBegin(
|
||
|
|
boost::bind(&SphereAtlasDemoPlugin::OnUpdate, this));
|
||
|
|
}
|
||
|
|
|
||
|
|
/////////////////////////////////////////////////
|
||
|
|
void SphereAtlasDemoPlugin::Init()
|
||
|
|
{
|
||
|
|
}
|
||
|
|
|
||
|
|
/////////////////////////////////////////////////
|
||
|
|
void SphereAtlasDemoPlugin::Reset()
|
||
|
|
{
|
||
|
|
gzlog << "SphereAtlasDemoPlugin: \n"
|
||
|
|
<< " This is not a typical usage of plugin Reset function,\n"
|
||
|
|
<< " we are doing this just for testing purposes.\n";
|
||
|
|
if (this->updateConnection)
|
||
|
|
{
|
||
|
|
event::Events::DisconnectWorldUpdateBegin(this->updateConnection);
|
||
|
|
this->updateConnection.reset();
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
/////////////////////////////////////////////////
|
||
|
|
void SphereAtlasDemoPlugin::OnUpdate()
|
||
|
|
{
|
||
|
|
common::Time currTime = this->model->GetWorld()->GetSimTime();
|
||
|
|
common::Time stepTime = currTime - this->prevUpdateTime;
|
||
|
|
this->prevUpdateTime = currTime;
|
||
|
|
double dt = stepTime.Double();
|
||
|
|
|
||
|
|
for (unsigned int j = 0; j < this->joints.size(); ++j)
|
||
|
|
{
|
||
|
|
double p = this->joints[j]->GetAngle(0).Radian();
|
||
|
|
double target = 0;
|
||
|
|
double perror = target - p;
|
||
|
|
double derror = (perror - this->qp[j])/dt;
|
||
|
|
this->qp[j] = perror; // save qp
|
||
|
|
double force = this->jointKp[j] * perror +
|
||
|
|
this->jointKd[j] * derror;
|
||
|
|
this->joints[j]->SetForce(0, force);
|
||
|
|
}
|
||
|
|
}
|