Skip to content

Commit 93a0e24

Browse files
sloretzsloretz
committed
Merged in set_velocity (pull request #427)
Set velocity tutorial Approved-by: Tully Foote <[email protected]> Approved-by: Louise Poubel <[email protected]>
2 parents 10c6854 + 881a994 commit 93a0e24

12 files changed

+1156
-0
lines changed

‎manifest.xml

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -831,6 +831,12 @@
831831
<skill>intermediate</skill>
832832
</tutorial>
833833

834+
<tutorial title="Setting Velocity on Joints and Links" ref="set_velocity">
835+
<markdown version="7.0">set_velocity/tutorial_7.md</markdown>
836+
<description> This tutorial describes how to programatically set velocities on links and joints</description>
837+
<skill>advanced</skill>
838+
</tutorial>
839+
834840
<!-- scripting -->
835841
<tutorial title="GazeboJs installation" ref='gazebojs_install'>
836842
<markdown version="4.0+">gazebojs_install/tutorial.md</markdown>

‎set_velocity/examples/set_vel_plugin/CMakeLists.txt

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,15 @@
1+
cmake_minimum_required(VERSION 2.6)
2+
project(SetVelPlugin)
3+
4+
find_package(gazebo REQUIRED)
5+
6+
include_directories(include ${GAZEBO_INCLUDE_DIRS})
7+
link_directories(${GAZEBO_LIBRARY_DIRS})
8+
list(APPEND CMAKE_CXX_FLAGS ${GAZEBO_CXX_FLAGS})
9+
10+
add_library(SetLinkVelocityPlugin SHARED src/SetLinkVelocityPlugin.cpp)
11+
target_link_libraries(SetLinkVelocityPlugin ${GAZEBO_LIBRARIES})
12+
13+
add_library(SetJointVelocityPlugin SHARED src/SetJointVelocityPlugin.cpp)
14+
target_link_libraries(SetJointVelocityPlugin ${GAZEBO_LIBRARIES})
15+

‎set_velocity/examples/set_vel_plugin/include/ode_perfect_angular.hh

Lines changed: 69 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,69 @@
1+
#include <gazebo/gazebo.hh>
2+
#include <gazebo/physics/Model.hh>
3+
#include <gazebo/physics/Link.hh>
4+
#include <gazebo/physics/World.hh>
5+
#include <gazebo/physics/Joint.hh>
6+
#include <gazebo/physics/PhysicsIface.hh>
7+
#include <gazebo/physics/PhysicsEngine.hh>
8+
9+
#ifndef GAZEBO_TUTORIALS_SET_VELOCITY_ODE_PERFECT_ANGULAR_HH_
10+
#define GAZEBO_TUTORIALS_SET_VELOCITY_ODE_PERFECT_ANGULAR_HH_
11+
12+
using namespace gazebo;
13+
14+
namespace gazebo
15+
{
16+
//////////////////////////////////////////////////
17+
/// \brief uses joint motors to perfectly control velocity (ODE only)
18+
class OdePerfectAngularVelController
19+
{
20+
public: OdePerfectAngularVelController()
21+
{
22+
}
23+
24+
public: ~OdePerfectAngularVelController()
25+
{
26+
this->Stop();
27+
}
28+
29+
/// \brief start controlling the angular velocity of an object
30+
/// \param[in] _link the link to move at the given velocity
31+
/// \param[in] _vel world velocity to move the link at
32+
/// \param[in] _maxTorque Maximum torque to apply each timestep
33+
public: void Start(
34+
physics::LinkPtr _link, math::Vector3 _vel, double _maxTorque)
35+
{
36+
this->Stop();
37+
// create revolute joint with the world as a parent
38+
physics::ModelPtr model = _link->GetModel();
39+
physics::WorldPtr world = physics::get_world("default");
40+
physics::PhysicsEnginePtr engine = world->GetPhysicsEngine();
41+
this->joint = engine->CreateJoint("revolute");
42+
this->joint->SetName(model->GetName() + "__perfect_ang_joint__");
43+
physics::LinkPtr worldLink =
44+
boost::dynamic_pointer_cast<physics::Link>(world->GetByName("world"));
45+
math::Pose jointOrigin;
46+
this->joint->Load(worldLink, _link, jointOrigin);
47+
this->joint->Init();
48+
double magnitude = _vel.GetLength();
49+
this->joint->SetAxis(0, _vel.Normalize());
50+
this->joint->SetParam("fmax", 0, _maxTorque);
51+
this->joint->SetParam("vel", 0, magnitude);
52+
}
53+
54+
public: void Stop()
55+
{
56+
//remove the joint from the world
57+
if (this->joint)
58+
{
59+
this->joint->SetParam("fmax", 0, 0.0);
60+
this->joint->Detach();
61+
this->joint->Fini();
62+
this->joint.reset();
63+
}
64+
}
65+
66+
private: physics::JointPtr joint;
67+
};
68+
}
69+
#endif

‎set_velocity/examples/set_vel_plugin/include/ode_perfect_linear.hh

Lines changed: 67 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,67 @@
1+
#include <gazebo/gazebo.hh>
2+
#include <gazebo/physics/Model.hh>
3+
#include <gazebo/physics/Link.hh>
4+
#include <gazebo/physics/World.hh>
5+
#include <gazebo/physics/Joint.hh>
6+
#include <gazebo/physics/PhysicsIface.hh>
7+
#include <gazebo/physics/PhysicsEngine.hh>
8+
9+
#ifndef GAZEBO_TUTORIALS_SET_VELOCITY_ODE_PERFECT_LINEAR_HH_
10+
#define GAZEBO_TUTORIALS_SET_VELOCITY_ODE_PERFECT_LINEAR_HH_
11+
12+
namespace gazebo
13+
{
14+
//////////////////////////////////////////////////
15+
/// \brief uses joint motors to perfectly control velocity (ODE only)
16+
class OdePerfectLinearVelController
17+
{
18+
public: OdePerfectLinearVelController()
19+
{
20+
}
21+
22+
public: ~OdePerfectLinearVelController()
23+
{
24+
this->Stop();
25+
}
26+
27+
/// \brief start controlling the linear velocity of an object
28+
/// \param[in] _link the link to move at the given velocity
29+
/// \param[in] _vel world velocity to move the link at
30+
/// \param[in] _maxForce Maximum force to apply each timestep
31+
public: void Start(
32+
physics::LinkPtr _link, math::Vector3 _vel, double _maxForce)
33+
{
34+
this->Stop();
35+
// create prismatic joint with the world as a parent
36+
physics::ModelPtr model = _link->GetModel();
37+
physics::WorldPtr world = physics::get_world("default");
38+
physics::PhysicsEnginePtr engine = world->GetPhysicsEngine();
39+
this->joint = engine->CreateJoint("prismatic");
40+
this->joint->SetName(model->GetName() + "__perfect_lin_joint__");
41+
physics::LinkPtr worldLink = boost::dynamic_pointer_cast<physics::Link>(
42+
world->GetByName("world"));
43+
math::Pose jointOrigin;
44+
this->joint->Load(worldLink, _link, jointOrigin);
45+
this->joint->Init();
46+
double magnitude = _vel.GetLength();
47+
this->joint->SetAxis(0, _vel.Normalize());
48+
this->joint->SetParam("fmax", 0, _maxForce);
49+
this->joint->SetParam("vel", 0, magnitude);
50+
}
51+
52+
public: void Stop()
53+
{
54+
//remove the joint from the world
55+
if (joint)
56+
{
57+
this->joint->SetParam("fmax", 0, 0.0);
58+
this->joint->Detach();
59+
this->joint->Fini();
60+
this->joint.reset();
61+
}
62+
}
63+
64+
private: physics::JointPtr joint;
65+
};
66+
}
67+
#endif

‎set_velocity/examples/set_vel_plugin/include/ode_perfect_velocity.hh

Lines changed: 115 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,115 @@
1+
#include <gazebo/gazebo.hh>
2+
#include <gazebo/physics/Model.hh>
3+
#include <gazebo/physics/Link.hh>
4+
#include <gazebo/physics/World.hh>
5+
#include <gazebo/physics/Joint.hh>
6+
#include <gazebo/physics/PhysicsIface.hh>
7+
#include <gazebo/physics/PhysicsEngine.hh>
8+
9+
#ifndef GAZEBO_TUTORIALS_SET_VELOCITY_ODE_PERFECT_VELOCITY_HH_
10+
#define GAZEBO_TUTORIALS_SET_VELOCITY_ODE_PERFECT_VELOCITY_HH_
11+
12+
namespace gazebo
13+
{
14+
//////////////////////////////////////////////////
15+
/// \brief uses joint motors to perfectly control velocity (ODE only)
16+
class OdePerfectVelocityController
17+
{
18+
public: OdePerfectVelocityController()
19+
{
20+
}
21+
22+
public: ~OdePerfectVelocityController()
23+
{
24+
this->Stop();
25+
}
26+
27+
public: void Start(physics::LinkPtr _link, math::Vector3 _linearVel,
28+
math::Vector3 _angularVel, double _maxForce, double _maxTorque)
29+
{
30+
this->Stop();
31+
physics::ModelPtr model = _link->GetModel();
32+
physics::WorldPtr world = physics::get_world("default");
33+
physics::PhysicsEnginePtr engine = world->GetPhysicsEngine();
34+
35+
// Create a phantom link
36+
this->phantomLink = model->CreateLink("__perfect_phantom_link__");
37+
sdf::ElementPtr config(new sdf::Element);
38+
config->Copy(_link->GetSDF()->Clone());
39+
config->GetAttribute("name")->Set("__perfect_phantom_link__");
40+
// Remove visuals/collisions/inertials
41+
while (config->HasElement("visual"))
42+
{
43+
config->RemoveChild(config->GetElement("visual"));
44+
}
45+
while (config->HasElement("collision"))
46+
{
47+
config->RemoveChild(config->GetElement("collision"));
48+
}
49+
while (config->HasElement("inertial"))
50+
{
51+
config->RemoveChild(config->GetElement("inertial"));
52+
}
53+
this->phantomLink->Load(config);
54+
this->phantomLink->Init();
55+
56+
// create prismatic joint with parent "world" and child phantomLink
57+
this->prismaticJoint = engine->CreateJoint("prismatic");
58+
this->prismaticJoint->SetName(
59+
model->GetName() + "__perfect_vel_lin_joint__");
60+
physics::LinkPtr worldLink = boost::dynamic_pointer_cast<physics::Link>(
61+
world->GetByName("world"));
62+
math::Pose prismaticJointOrigin;
63+
this->prismaticJoint->Load(worldLink, this->phantomLink,
64+
prismaticJointOrigin);
65+
this->prismaticJoint->Init();
66+
double linearMagnitude = _linearVel.GetLength();
67+
this->prismaticJoint->SetAxis(0, _linearVel.Normalize());
68+
this->prismaticJoint->SetParam("fmax", 0, _maxForce);
69+
this->prismaticJoint->SetParam("vel", 0, linearMagnitude);
70+
71+
// create revolute joint with parent phantomLink and child _link
72+
this->revoluteJoint = engine->CreateJoint("revolute");
73+
this->revoluteJoint->SetName(
74+
model->GetName() + "__perfect_vel_ang_joint__");
75+
math::Pose revoluteJointOrigin;
76+
this->revoluteJoint->Load(this->phantomLink, _link,
77+
revoluteJointOrigin);
78+
this->revoluteJoint->Init();
79+
double angularMagnitude = _angularVel.GetLength();
80+
this->revoluteJoint->SetAxis(0, _angularVel.Normalize());
81+
this->revoluteJoint->SetParam("fmax", 0, _maxTorque);
82+
this->revoluteJoint->SetParam("vel", 0, angularMagnitude);
83+
}
84+
85+
public: void Stop()
86+
{
87+
//remove everything from the world
88+
if (this->prismaticJoint)
89+
{
90+
this->prismaticJoint->SetParam("fmax", 0, 0.0);
91+
prismaticJoint->Detach();
92+
this->prismaticJoint->Fini();
93+
this->prismaticJoint.reset();
94+
}
95+
if (this->revoluteJoint)
96+
{
97+
this->revoluteJoint->SetParam("fmax", 0, 0.0);
98+
revoluteJoint->Detach();
99+
this->revoluteJoint->Fini();
100+
this->revoluteJoint.reset();
101+
}
102+
if (this->phantomLink)
103+
{
104+
this->phantomLink->Fini();
105+
this->phantomLink.reset();
106+
}
107+
}
108+
109+
private: physics::JointPtr prismaticJoint;
110+
private: physics::JointPtr revoluteJoint;
111+
private: physics::LinkPtr phantomLink;
112+
};
113+
}
114+
#endif
115+

‎set_velocity/examples/set_vel_plugin/include/pid_link.hh

Lines changed: 114 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,114 @@
1+
#include <gazebo/gazebo.hh>
2+
#include <gazebo/common/PID.hh>
3+
#include <gazebo/common/Time.hh>
4+
#include <gazebo/physics/Link.hh>
5+
6+
#ifndef GAZEBO_TUTORIALS_SET_VELOCITY_PID_LINK_HH_
7+
#define GAZEBO_TUTORIALS_SET_VELOCITY_PID_LINK_HH_
8+
9+
using namespace gazebo;
10+
11+
//////////////////////////////////////////////////
12+
// \brief use a PID controller to apply forces to achieve a target velocity
13+
class PIDLinkVelocityController
14+
{
15+
public: PIDLinkVelocityController()
16+
{
17+
}
18+
19+
public: ~PIDLinkVelocityController()
20+
{
21+
this->Stop();
22+
}
23+
24+
public: void Start(physics::LinkPtr _link, math::Vector3 _linearVel,
25+
math::Vector3 _angularVel, double _maxForce, double _maxTorque)
26+
{
27+
this->Stop();
28+
this->link = _link;
29+
this->targetLinearVel = _linearVel;
30+
this->targetAngularVel = _angularVel;
31+
32+
// Hard coded gains. Tune these for your own application!
33+
double linear_p = 100.0;
34+
double linear_i = 0.0;
35+
double linear_d = 0.0;
36+
double linear_imax = 123456789.0;
37+
double angular_p = 100.0;
38+
double angular_i = 0.0;
39+
double angular_d = 0.0;
40+
double angular_imax = 123456789.0;
41+
42+
// Add a PID controller for each DoF
43+
for (int i = 0; i < 3; i++)
44+
{
45+
common::PID controller_translation(linear_p, linear_i, linear_d,
46+
linear_imax, -linear_imax, _maxForce, -_maxForce);
47+
common::PID controller_rotation(angular_p, angular_i, angular_d,
48+
angular_imax, -angular_imax, _maxTorque, -_maxTorque);
49+
this->controllers.push_back(controller_translation);
50+
this->controllers.push_back(controller_rotation);
51+
}
52+
53+
this->updateConnection = event::Events::ConnectWorldUpdateBegin(
54+
std::bind(&PIDLinkVelocityController::Update, this,
55+
std::placeholders::_1));
56+
}
57+
58+
public: void Stop()
59+
{
60+
this->lastSimTime.Set(0.0);
61+
this->link.reset();
62+
this->controllers.clear();
63+
this->updateConnection.reset();
64+
}
65+
66+
public: void Update(const common::UpdateInfo &_info)
67+
{
68+
common::Time curTime = _info.simTime;
69+
if (this->lastSimTime.Double() < 0.000001)
70+
{
71+
// First update, need a second one to get change time
72+
}
73+
if (curTime < this->lastSimTime)
74+
{
75+
// Time moved backwards (World reset?)
76+
this->Stop();
77+
}
78+
else
79+
{
80+
// Get change in time between updates
81+
double dt = (curTime - lastSimTime).Double();
82+
83+
// Calculate the error between actual and target velocity
84+
math::Vector3 curLinearVel = this->link->GetWorldLinearVel();
85+
math::Vector3 curAngularVel = this->link->GetWorldAngularVel();
86+
math::Vector3 linearError = curLinearVel - this->targetLinearVel;
87+
math::Vector3 angularError = curAngularVel - this->targetAngularVel;
88+
89+
// Get forces to apply from controllers
90+
math::Vector3 worldForce;
91+
math::Vector3 worldTorque;
92+
worldForce.x = this->controllers[0].Update(linearError.x, dt);
93+
worldTorque.x = this->controllers[1].Update(angularError.x, dt);
94+
worldForce.y = this->controllers[2].Update(linearError.y, dt);
95+
worldTorque.y = this->controllers[3].Update(angularError.y, dt);
96+
worldForce.z = this->controllers[4].Update(linearError.z, dt);
97+
worldTorque.z = this->controllers[5].Update(angularError.z, dt);
98+
99+
// Add those forces to the body
100+
this->link->AddForce(worldForce);
101+
this->link->AddTorque(worldTorque);
102+
}
103+
lastSimTime = curTime;
104+
}
105+
106+
private: physics::LinkPtr link;
107+
private: std::vector<common::PID> controllers;
108+
private: event::ConnectionPtr updateConnection;
109+
private: math::Vector3 targetLinearVel;
110+
private: math::Vector3 targetAngularVel;
111+
private: common::Time lastSimTime;
112+
};
113+
114+
#endif

0 commit comments

Comments
 (0)