Skip to content

Commit 80d1422

Browse files
fix adm control (#32) (#33)
* fix adm rule * revert impedance rule to normal inverse() (instead of LLT-based inverses) * increase alpha pinv * check for singularities in impedance control * prepare admittance for singularity checks * add commented alternative method for acc estimation * fix missing var in impedance rule * change start position * use "end_effector_frame" instead of control frame (cherry picked from commit 5921f9a) Co-authored-by: Thibault Poignonec <79221188+tpoignonec@users.noreply.github.com>
1 parent cb643d9 commit 80d1422

12 files changed

+217
-72
lines changed

cartesian_vic_controller/config/example_admittance_controller_config.yaml

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -42,9 +42,9 @@ cartesian_impedance_controller:
4242
is_enabled: false
4343
name: ""
4444

45-
control:
45+
end_effector_frame:
4646
frame:
47-
id: interaction_point # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector
47+
id: interaction_point
4848
external: false # control frame exists within URDF kinematic chain
4949

5050
fixed_world_frame: # Gravity points down (neg. Z) in this frame (Usually: world or base_link)

cartesian_vic_controller/config/example_impedance_controller_config.yaml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -41,7 +41,7 @@ cartesian_impedance_controller:
4141
is_enabled: false
4242
name: ""
4343

44-
control:
44+
end_effector_frame:
4545
frame:
4646
id: interaction_point # Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector
4747
external: false # control frame exists within URDF kinematic chain

cartesian_vic_controller/include/cartesian_vic_controller/cartesian_vic_rule.hpp

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -206,6 +206,9 @@ class CartesianVicRule
206206
/// Jacobian pre-allocation (used to compute cartesian inertia matrix)
207207
Eigen::Matrix<double, 6, Eigen::Dynamic> J_private_;
208208

209+
/// Jacobian pre-allocation (used to compute cartesian inertia matrix)
210+
// Eigen::Matrix<double, 6, Eigen::Dynamic> J_dot_private_;
211+
209212
/// Filtered external torques
210213
Eigen::VectorXd filtered_external_torques_;
211214
};

cartesian_vic_controller/include/cartesian_vic_controller/cartesian_vic_state.hpp

Lines changed: 10 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -127,12 +127,12 @@ class VicInputData
127127
bool activate_nullspace_control = false;
128128
bool activate_gravity_compensation = false;
129129

130-
/// Name of the robot base frame
130+
/// Name of the control frame in which is expressed the cartesian pose/vel/acc/wrench reference
131131
std::string base_frame;
132132
/// Name of the compliance (i.e., vic) frame in which compliance parameters are specified
133133
std::string vic_frame;
134-
/// Name of the control frame in which is expressed the cartesian pose/vel/acc/wrench reference
135-
std::string control_frame;
134+
/// Name of the controlled robot end-effector frame
135+
std::string end_effector_frame;
136136
/// Name of the force/torque sensor frame in which is expressed the measured wrench
137137
std::string ft_sensor_frame;
138138

@@ -236,23 +236,16 @@ class VicState
236236
/// Transforms between frames used in the Vic controller
237237
struct VicTransforms
238238
{
239-
// transformation from force torque sensor frame to base link frame at reference joint angles
240-
Eigen::Isometry3d ref_base_ft_;
241-
// transformation from force torque sensor frame to base link frame at reference + vic
242-
// offset joint angles
239+
// transformation from fixed world frame to base link frame
240+
Eigen::Isometry3d world_base_;
241+
// transformation from force torque sensor frame to base link frame
243242
Eigen::Isometry3d base_ft_;
244-
// transformation from control frame to base link frame at reference + vic offset joint
245-
// angles
246-
Eigen::Isometry3d base_control_;
243+
// transformation from controlled end effector (EE) frame to base link frame
244+
Eigen::Isometry3d base_end_effector_;
245+
// transformation from vic frame to base link frame
247246
Eigen::Isometry3d base_vic_;
248-
// transformation from end effector frame to base link frame at reference + vic offset
249-
// joint angles
250-
Eigen::Isometry3d base_tip_;
251-
// transformation from center of gravity frame to base link frame at reference + vic offset
252-
// joint angles
247+
// transformation from CoG frame (i.e., post-sensor inertial frame) to base link frame
253248
Eigen::Isometry3d base_cog_;
254-
// transformation from world frame to base link frame
255-
Eigen::Isometry3d world_base_;
256249
};
257250

258251

cartesian_vic_controller/include/cartesian_vic_controller/rules/vanilla_cartesian_admittance_rule.hpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -58,7 +58,7 @@ class VanillaCartesianAdmittanceRule : public CartesianVicRule
5858
Eigen::Matrix<double, Eigen::Dynamic, 6> J_pinv_;
5959
Eigen::Matrix<double, 6, Eigen::Dynamic> J_dot_;
6060

61-
double alpha_pinv_ = 0.000005;
61+
double alpha_pinv_ = 0.0005;
6262

6363
// Nullspace solver
6464
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> nullspace_projection_;

cartesian_vic_controller/include/cartesian_vic_controller/rules/vanilla_cartesian_impedance_rule.hpp

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -63,7 +63,7 @@ class VanillaCartesianImpedanceRule : public CartesianVicRule
6363

6464
Eigen::VectorXd raw_joint_command_effort_;
6565

66-
double alpha_pinv_ = 0.000005;
66+
double alpha_pinv_ = 0.0005;
6767

6868
// Nullspace solver
6969
Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> nullspace_projection_;

cartesian_vic_controller/src/cartesian_vic_controller_parameters.yaml

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -111,11 +111,11 @@ cartesian_vic_controller:
111111
description: "Specifies the name of the external torque sensor in the robot description which will be used in the vic calculation."
112112
}
113113

114-
control:
114+
end_effector_frame:
115115
frame:
116116
id: {
117117
type: string,
118-
description: "Specifies the robot control frame in which the reference pose and velocities are expressed."
118+
description: "Specifies the controlled robot end effector frame. Note that the pose and velocity are expressed w.r.t. the base frame (see dynamics plugin parameters)."
119119
}
120120

121121
fixed_world_frame: # Gravity points down (neg. Z) in this frame (Usually: world or base_link)
@@ -149,7 +149,7 @@ cartesian_vic_controller:
149149
frame:
150150
id: {
151151
type: string,
152-
description: "Specifies the control frame used for vic calculation."
152+
description: "Specifies the frame in which the impedance matrices are expressed."
153153
}
154154
plugin_name: {
155155
type: string,

cartesian_vic_controller/src/cartesian_vic_rule.cpp

Lines changed: 24 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -94,6 +94,9 @@ CartesianVicRule::configure(
9494
J_private_ = \
9595
Eigen::Matrix<double, 6, Eigen::Dynamic>::Zero(6, num_joints);
9696

97+
// J_dot_private_ = \
98+
// Eigen::Matrix<double, 6, Eigen::Dynamic>::Zero(6, num_joints);
99+
97100
return controller_interface::return_type::OK;
98101
}
99102

@@ -191,7 +194,7 @@ void CartesianVicRule::apply_parameters_update()
191194
parameters_.vic.activate_gravity_compensation;
192195

193196
vic_state_.input_data.vic_frame = parameters_.vic.frame.id;
194-
vic_state_.input_data.control_frame = parameters_.control.frame.id;
197+
vic_state_.input_data.end_effector_frame = parameters_.end_effector_frame.frame.id;
195198
vic_state_.input_data.ft_sensor_frame = parameters_.ft_sensor.frame.id;
196199

197200
if (!use_streamed_interaction_parameters_) {
@@ -318,7 +321,7 @@ CartesianVicRule::update_compliant_frame_trajectory(
318321
// Update reference compliant frames
319322
bool success = true;
320323
for (unsigned int i = 0; i < N; i++) {
321-
// TODO(tpoignonec): Check the frame is correct (i.e., control w.r.t. base)!
324+
// TODO(tpoignonec): Check the frame is correct (i.e., end-effector w.r.t. base)!
322325
success &= \
323326
vic_state_.input_data.reference_compliant_frames.fill_desired_robot_state_from_msg(
324327
i,
@@ -449,7 +452,7 @@ CartesianVicRule::update_input_data(
449452
);
450453
success &= dynamics_->calculate_jacobian(
451454
vic_state_.input_data.joint_state_position,
452-
vic_state_.input_data.control_frame,
455+
vic_state_.input_data.end_effector_frame,
453456
J_private_
454457
);
455458
vic_state_.input_data.natural_cartesian_inertia = (J_private_ * \
@@ -571,6 +574,7 @@ bool CartesianVicRule::update_kinematics(
571574
}
572575

573576
// Update current robot joint velocity
577+
// auto previous_joint_velocity = vic_state_.input_data.joint_state_velocity;
574578
double cutoff_jnt_velocity = parameters_.filters.state_velocity_filter_cuttoff_freq;
575579
if (dt > 0 && cutoff_jnt_velocity > 0.0) {
576580
double jnt_velocity_filter_coefficient = 1.0 - exp(-dt * 2 * 3.14 * cutoff_jnt_velocity);
@@ -589,15 +593,15 @@ bool CartesianVicRule::update_kinematics(
589593
// Update current cartesian pose and velocity from robot joint states
590594
success &= dynamics_->calculate_link_transform(
591595
vic_state_.input_data.joint_state_position,
592-
vic_state_.input_data.control_frame,
596+
vic_state_.input_data.end_effector_frame,
593597
vic_state_.input_data.robot_current_pose
594598
);
595599

596600
auto last_robot_cartesian_velocity = vic_state_.input_data.robot_current_velocity;
597601
success = dynamics_->convert_joint_deltas_to_cartesian_deltas(
598602
vic_state_.input_data.joint_state_position,
599603
vic_state_.input_data.joint_state_velocity,
600-
vic_state_.input_data.control_frame,
604+
vic_state_.input_data.end_effector_frame,
601605
vic_state_.input_data.robot_current_velocity
602606
);
603607

@@ -606,6 +610,17 @@ bool CartesianVicRule::update_kinematics(
606610
if (dt > 0) {
607611
auto raw_acc = \
608612
(vic_state_.input_data.robot_current_velocity - last_robot_cartesian_velocity) / dt;
613+
/*
614+
auto joint_acc = (vic_state_.input_data.joint_state_velocity - previous_joint_velocity) / dt;
615+
success &= dynamics_->calculate_jacobian_derivative(
616+
vic_state_.input_data.joint_state_position,
617+
vic_state_.input_data.joint_state_velocity,
618+
vic_state_.input_data.end_effector_frame,
619+
J_dot_private_
620+
);
621+
auto raw_acc = J_private_ * joint_acc + J_dot_private_ *
622+
vic_state_.input_data.joint_state_velocity;
623+
*/
609624
if (cutoff_acceleration > 0.0) {
610625
double cutoff_acceleration = 30.0; // Hz
611626
double acceleration_filter_coefficient = 1.0 - exp(-dt * 2 * 3.14 * cutoff_acceleration);
@@ -635,12 +650,6 @@ bool CartesianVicRule::update_kinematics(
635650
// In theory, there is no reason to use it if no wrench is available, but you never know...
636651
vic_transforms_.base_ft_.setIdentity();
637652
}
638-
639-
success &= dynamics_->calculate_link_transform(
640-
vic_state_.input_data.joint_state_position,
641-
parameters_.dynamics.tip,
642-
vic_transforms_.base_tip_
643-
);
644653
success &= dynamics_->calculate_link_transform(
645654
vic_state_.input_data.joint_state_position,
646655
parameters_.fixed_world_frame.frame.id,
@@ -653,12 +662,12 @@ bool CartesianVicRule::update_kinematics(
653662
);
654663
success &= dynamics_->calculate_link_transform(
655664
vic_state_.input_data.joint_state_position,
656-
parameters_.control.frame.id,
657-
vic_transforms_.base_control_
665+
parameters_.end_effector_frame.frame.id,
666+
vic_transforms_.base_end_effector_
658667
);
659668
success &= dynamics_->calculate_link_transform(
660669
vic_state_.input_data.joint_state_position,
661-
parameters_.control.frame.id,
670+
parameters_.vic.frame.id,
662671
vic_transforms_.base_vic_
663672
);
664673
return true;
@@ -694,7 +703,7 @@ bool CartesianVicRule::process_wrench_measurements(
694703
new_wrench_world.block<3, 1>(0, 1) -= (rot_world_cog * cog_pos).cross(end_effector_weight);
695704

696705
/*
697-
// Wrench at interaction point (e.g., assumed to be control frame
706+
// Wrench at interaction point (assumed to be end-effector frame)
698707
F_ext.block<3, 1>(0, 0) = rot_base_control.transpose() * F_ext_base.block<3, 1>(0, 0);
699708
// Evaluate torques at new interaction point
700709
F_ext.block<3, 1>(3, 0) = rot_base_control.transpose() * (

cartesian_vic_controller/src/cartesian_vic_state.cpp

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -54,6 +54,8 @@ VicState::to_msg(cartesian_control_msgs::msg::VicControllerState & vic_state_msg
5454
success = false;
5555
}
5656

57+
// Fill frame names
58+
5759
// Fill desired compliance
5860
auto desired_frame_0 = \
5961
input_data.reference_compliant_frames.get_compliant_frame(0);

0 commit comments

Comments
 (0)