Skip to content

Commit 5554fd9

Browse files
cambelcambel
committed
Refactor ForwardDynamics code
1 parent 623fc9f commit 5554fd9

File tree

2 files changed

+76
-63
lines changed

2 files changed

+76
-63
lines changed

‎cartesian_controller_base/include/cartesian_controller_base/ForwardDynamicsSolver.h

+1
Original file line numberDiff line numberDiff line change
@@ -128,6 +128,7 @@ class ForwardDynamicsSolver : public IKSolver
128128

129129
//! Build a generic robot model for control
130130
bool buildGenericModel();
131+
bool buildUR5eModel();
131132

132133
// Forward dynamics
133134
std::shared_ptr<KDL::ChainJntToJacSolver> m_jnt_jacobian_solver;

‎cartesian_controller_base/src/ForwardDynamicsSolver.cpp

+75-63
Original file line numberDiff line numberDiff line change
@@ -72,39 +72,31 @@
7272
* \endcode
7373
*
7474
*/
75-
PLUGINLIB_EXPORT_CLASS(cartesian_controller_base::ForwardDynamicsSolver,
76-
cartesian_controller_base::IKSolver)
75+
PLUGINLIB_EXPORT_CLASS(cartesian_controller_base::ForwardDynamicsSolver, cartesian_controller_base::IKSolver)
7776

7877
namespace cartesian_controller_base {
7978

8079
ForwardDynamicsSolver::ForwardDynamicsSolver() {}
8180

8281
ForwardDynamicsSolver::~ForwardDynamicsSolver() {}
8382

84-
trajectory_msgs::JointTrajectoryPoint
85-
ForwardDynamicsSolver::getJointControlCmds(ros::Duration period,
86-
const ctrl::Vector6D& net_force) {
83+
trajectory_msgs::JointTrajectoryPoint ForwardDynamicsSolver::getJointControlCmds(ros::Duration period, const ctrl::Vector6D &net_force) {
8784
// Compute joint space inertia matrix with actualized link masses
8885
buildGenericModel();
89-
m_jnt_space_inertia_solver->JntToMass(m_current_positions,
90-
m_jnt_space_inertia);
86+
m_jnt_space_inertia_solver->JntToMass(m_current_positions, m_jnt_space_inertia);
9187

9288
// Compute joint jacobian
9389
m_jnt_jacobian_solver->JntToJac(m_current_positions, m_jnt_jacobian);
9490

9591
// Compute joint accelerations according to: \f$ \ddot{q} = H^{-1} ( J^T f)
9692
// \f$
97-
m_current_accelerations.data = m_jnt_space_inertia.data.inverse() *
98-
m_jnt_jacobian.data.transpose() * net_force;
93+
m_current_accelerations.data = m_jnt_space_inertia.data.inverse() * m_jnt_jacobian.data.transpose() * net_force;
9994

10095
// Numerical time integration with the Euler forward method
101-
m_current_positions.data =
102-
m_last_positions.data + m_last_velocities.data * period.toSec();
103-
m_current_velocities.data =
104-
m_last_velocities.data + m_current_accelerations.data * period.toSec();
105-
m_current_velocities.data *=
106-
0.9; // 10 % global damping against unwanted null space motion.
107-
// Will cause exponential slow-down without input.
96+
m_current_positions.data = m_last_positions.data + m_last_velocities.data * period.toSec();
97+
m_current_velocities.data = m_last_velocities.data + m_current_accelerations.data * period.toSec();
98+
m_current_velocities.data *= 0.9; // 10 % global damping against unwanted null space motion.
99+
// Will cause exponential slow-down without input.
108100

109101
// Make sure positions stay in allowed margins
110102
applyJointVelocityLimits();
@@ -120,7 +112,7 @@ ForwardDynamicsSolver::getJointControlCmds(ros::Duration period,
120112
// by most hardware joint drivers as max. tolerated values. As a
121113
// consequence, the robot will move very slowly.
122114
}
123-
control_cmd.time_from_start = period; // valid for this duration
115+
control_cmd.time_from_start = period; // valid for this duration
124116

125117
// Update for the next cycle
126118
m_last_positions = m_current_positions;
@@ -129,69 +121,94 @@ ForwardDynamicsSolver::getJointControlCmds(ros::Duration period,
129121
return control_cmd;
130122
}
131123

132-
bool ForwardDynamicsSolver::init(ros::NodeHandle& nh, const KDL::Chain& chain,
133-
const KDL::JntArray& upper_pos_limits,
134-
const KDL::JntArray& lower_pos_limits,
135-
const KDL::JntArray& velocity_limits) {
136-
IKSolver::init(nh, chain, upper_pos_limits, lower_pos_limits,
137-
velocity_limits);
124+
bool ForwardDynamicsSolver::init(ros::NodeHandle &nh, const KDL::Chain &chain, const KDL::JntArray &upper_pos_limits, const KDL::JntArray &lower_pos_limits, const KDL::JntArray &velocity_limits) {
125+
IKSolver::init(nh, chain, upper_pos_limits, lower_pos_limits, velocity_limits);
138126

139127
if (!buildGenericModel()) {
140-
ROS_ERROR(
141-
"ForwardDynamicsSolver: Something went wrong in setting up the "
142-
"internal model.");
128+
ROS_ERROR("ForwardDynamicsSolver: Something went wrong in setting up the "
129+
"internal model.");
143130
return false;
144131
}
145132

146133
// Forward dynamics
147134
m_jnt_jacobian_solver.reset(new KDL::ChainJntToJacSolver(m_chain));
148-
m_jnt_space_inertia_solver.reset(
149-
new KDL::ChainDynParam(m_chain, KDL::Vector::Zero()));
135+
m_jnt_space_inertia_solver.reset(new KDL::ChainDynParam(m_chain, KDL::Vector::Zero()));
150136
m_jnt_jacobian.resize(m_number_joints);
151137
m_jnt_space_inertia.resize(m_number_joints);
152138

153139
// Connect dynamic reconfigure and overwrite the default values with values
154140
// on the parameter server. This is done automatically if parameters with
155141
// the according names exist.
156-
m_callback_type =
157-
std::bind(&ForwardDynamicsSolver::dynamicReconfigureCallback, this,
158-
std::placeholders::_1, std::placeholders::_2);
142+
m_callback_type = std::bind(&ForwardDynamicsSolver::dynamicReconfigureCallback, this, std::placeholders::_1, std::placeholders::_2);
159143

160-
m_dyn_conf_server.reset(new dynamic_reconfigure::Server<IKConfig>(
161-
ros::NodeHandle(nh.getNamespace() + "/solver/forward_dynamics")));
144+
m_dyn_conf_server.reset(new dynamic_reconfigure::Server<IKConfig>(ros::NodeHandle(nh.getNamespace() + "/solver/forward_dynamics")));
162145

163146
m_dyn_conf_server->setCallback(m_callback_type);
164147

165148
ROS_INFO("Forward dynamics solver initialized");
166-
ROS_INFO("Forward dynamics solver has control over %i joints",
167-
m_number_joints);
149+
ROS_INFO("Forward dynamics solver has control over %i joints", m_number_joints);
168150

169151
return true;
170152
}
171153

172-
bool ForwardDynamicsSolver::buildGenericModel() {
154+
bool ForwardDynamicsSolver::buildGenericModel()
155+
{
156+
// Set all masses and inertias to minimal (yet stable) values.
157+
double ip_min = 0.000001;
158+
for (size_t i = 0; i < m_chain.segments.size(); ++i)
159+
{
160+
// Fixed joint segment
161+
if (m_chain.segments[i].getJoint().getType() == KDL::Joint::None)
162+
{
163+
m_chain.segments[i].setInertia(
164+
KDL::RigidBodyInertia::Zero());
165+
}
166+
else // relatively moving segment
167+
{
168+
m_chain.segments[i].setInertia(
169+
KDL::RigidBodyInertia(
170+
m_min, // mass
171+
KDL::Vector::Zero(), // center of gravity
172+
KDL::RotationalInertia(
173+
ip_min, // ixx
174+
ip_min, // iyy
175+
ip_min // izz
176+
// ixy, ixy, iyz default to 0.0
177+
)));
178+
}
179+
}
180+
181+
// Only give the last segment a generic mass and inertia.
182+
// See https://arxiv.org/pdf/1908.06252.pdf for a motivation for this setting.
183+
double m = 1;
184+
double ip = 1;
185+
m_chain.segments[m_chain.segments.size()-1].setInertia(
186+
KDL::RigidBodyInertia(
187+
m,
188+
KDL::Vector::Zero(),
189+
KDL::RotationalInertia(ip, ip, ip)));
190+
191+
return true;
192+
}
193+
194+
bool ForwardDynamicsSolver::buildUR5eModel() {
173195
double mass[6] = {1.98, 3.4445, 1.437, 0.871, 0.805, 0.261};
174196
double centor_of_mass[6][3] = {
175-
{0.0, 0.0, -0.02}, {-0.11355, 0.0, 0.1157}, {-0.1632, 0.0, 0.0238},
176-
{0.0, -0.01, 0.0}, {0.0, 0.01, 0.0}, {0.0, 0.0, -0.02},
197+
{0.0, 0.0, -0.02}, {-0.11355, 0.0, 0.1157}, {-0.1632, 0.0, 0.0238}, {0.0, -0.01, 0.0}, {0.0, 0.01, 0.0}, {0.0, 0.0, -0.02},
177198
};
178199
double inertia_tensor[6][6] = {
179200
// xx, yy, zz, xy, xz, yz
180-
{0.008093166666666665, 0.008093166666666665, 0.005625, 0.0, 0.0, 0.0},
181-
{0.021728491912499998, 0.021728491912499998, 0.00961875, 0.0, 0.0, 0.0},
182-
{0.006544570199999999, 0.006544570199999999, 0.00354375, 0.0, 0.0, 0.0},
183-
{0.0020849999999999996, 0.0020849999999999996, 0.00225, 0.0, 0.0, 0.0},
184-
{0.0020849999999999996, 0.0020849999999999996, 0.00225, 0.0, 0.0, 0.0},
185-
{0.00013626666666666665, 0.00013626666666666665, 0.0001792, 0.0, 0.0,
186-
0.0},
201+
{0.008093166666666665, 0.008093166666666665, 0.005625, 0.0, 0.0, 0.0}, {0.021728491912499998, 0.021728491912499998, 0.00961875, 0.0, 0.0, 0.0},
202+
{0.006544570199999999, 0.006544570199999999, 0.00354375, 0.0, 0.0, 0.0}, {0.0020849999999999996, 0.0020849999999999996, 0.00225, 0.0, 0.0, 0.0},
203+
{0.0020849999999999996, 0.0020849999999999996, 0.00225, 0.0, 0.0, 0.0}, {0.00013626666666666665, 0.00013626666666666665, 0.0001792, 0.0, 0.0, 0.0},
187204
};
188205
// Set all masses and inertias to minimal (yet stable) values.
189-
double ip_min = 0.000001;
206+
// double ip_min = 0.000001;
190207
for (size_t i = 0; i < m_chain.segments.size(); ++i) {
191208
// Fixed joint segment
192209
if (m_chain.segments[i].getJoint().getType() == KDL::Joint::None) {
193210
m_chain.segments[i].setInertia(KDL::RigidBodyInertia::Zero());
194-
} else // relatively moving segment
211+
} else // relatively moving segment
195212
{
196213
m_chain.segments[i].setInertia(KDL::RigidBodyInertia(
197214
// m_min, // mass
@@ -201,15 +218,15 @@ bool ForwardDynamicsSolver::buildGenericModel() {
201218
// ip_min // izz
202219
// // ixy, ixy, iyz default to 0.0
203220
// )));
204-
mass[i], // mass
221+
mass[i], // mass
205222
KDL::Vector(centor_of_mass[i][0], centor_of_mass[i][1],
206-
centor_of_mass[i][2]), // center of gravity
207-
KDL::RotationalInertia(inertia_tensor[i][0], // ixx
208-
inertia_tensor[i][1], // iyy
209-
inertia_tensor[i][2], // izz
210-
inertia_tensor[i][3], // ixy
211-
inertia_tensor[i][4], // ixz
212-
inertia_tensor[i][5] // iyz
223+
centor_of_mass[i][2]), // center of gravity
224+
KDL::RotationalInertia(inertia_tensor[i][0], // ixx
225+
inertia_tensor[i][1], // iyy
226+
inertia_tensor[i][2], // izz
227+
inertia_tensor[i][3], // ixy
228+
inertia_tensor[i][4], // ixz
229+
inertia_tensor[i][5] // iyz
213230
)));
214231
}
215232
}
@@ -218,16 +235,11 @@ bool ForwardDynamicsSolver::buildGenericModel() {
218235
// See https://arxiv.org/pdf/1908.06252.pdf for a motivation for this setting.
219236
double m = 1;
220237
double ip = 1;
221-
m_chain.segments[m_chain.segments.size() - 1].setInertia(
222-
KDL::RigidBodyInertia(m, KDL::Vector::Zero(),
223-
KDL::RotationalInertia(ip, ip, ip)));
238+
m_chain.segments[m_chain.segments.size() - 1].setInertia(KDL::RigidBodyInertia(m, KDL::Vector::Zero(), KDL::RotationalInertia(ip, ip, ip)));
224239

225240
return true;
226241
}
227242

228-
void ForwardDynamicsSolver::dynamicReconfigureCallback(IKConfig& config,
229-
uint32_t level) {
230-
m_min = config.link_mass;
231-
}
243+
void ForwardDynamicsSolver::dynamicReconfigureCallback(IKConfig &config, uint32_t level) { m_min = config.link_mass; }
232244

233-
} // namespace cartesian_controller_base
245+
} // namespace cartesian_controller_base

0 commit comments

Comments
 (0)