From c9c8c9c3277fd4738643788d1bc8f2058266a2dc Mon Sep 17 00:00:00 2001 From: AndyZe Date: Thu, 27 May 2021 00:00:52 -0500 Subject: [PATCH] Implement "spring back" behavior. Use inertial reference frame. (#14) * Delete 2 unused variables * First shot at saving a feedforward pose * Do calcs in an inertial frame of reference Delete unused functions * This seems to work great! * Delete 'motion_scale' hacky param --- .../admittance_controller/admittance_rule.hpp | 44 ++++------ admittance_controller/src/admittance_rule.cpp | 88 ++++++++----------- 2 files changed, 50 insertions(+), 82 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index 6c0ca1c8bd..4f2a6d4cb1 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -84,11 +84,12 @@ class AdmittanceRule bool hardware_state_has_offset_ = false; // IK related parameters + // ik_base_frame should be stationary so vel/accel calculations are correct std::string ik_base_frame_; std::string ik_tip_frame_; std::string ik_group_name_; - // Generally most quantities are transformed to and calculations are done in control_frame + // Admittance calcs (displacement etc) are done in this frame. Usually the tool or end-effector std::string control_frame_; // Gravity points down (neg. Z) in the world frame std::string fixed_world_frame_; @@ -151,21 +152,21 @@ class AdmittanceRule geometry_msgs::msg::WrenchStamped measured_wrench_control_frame_; geometry_msgs::msg::WrenchStamped measured_wrench_endeffector_frame_; - geometry_msgs::msg::PoseStamped origin_ik_tip_; - geometry_msgs::msg::PoseStamped origin_endeffector_; - geometry_msgs::msg::PoseStamped current_pose_base_frame_; - geometry_msgs::msg::PoseStamped current_pose_control_frame_; + geometry_msgs::msg::PoseStamped current_pose_ik_base_frame_; - geometry_msgs::msg::WrenchStamped target_force_control_frame_; - geometry_msgs::msg::PoseStamped target_pose_control_frame_; + // This is the feedforward pose. Where should the end effector be with no wrench applied? + geometry_msgs::msg::PoseStamped feedforward_pose_ik_base_frame_; - geometry_msgs::msg::PoseStamped desired_pose_control_frame_; + geometry_msgs::msg::WrenchStamped target_force_ik_base_frame_; + geometry_msgs::msg::PoseStamped target_pose_ik_base_frame_; + + geometry_msgs::msg::PoseStamped desired_pose_ik_base_frame_; geometry_msgs::msg::TransformStamped relative_desired_pose_; // Pre-reserved update-loop variables std::array measured_wrench_control_frame_arr_; - std::array target_pose_control_frame_arr_; - std::array current_pose_control_frame_arr_; + std::array target_pose_ik_base_frame_arr_; + std::array current_pose_ik_base_frame_arr_; std::array angles_error_; @@ -179,16 +180,16 @@ class AdmittanceRule private: template controller_interface::return_type - transform_message_to_control_frame(const MsgType & message_in, MsgType & message_out) + transform_message_to_ik_base_frame(const MsgType & message_in, MsgType & message_out) { - if (control_frame_ != message_in.header.frame_id) { + if (ik_base_frame_ != message_in.header.frame_id) { try { geometry_msgs::msg::TransformStamped transform = tf_buffer_->lookupTransform( - control_frame_, message_in.header.frame_id, tf2::TimePointZero); + ik_base_frame_, message_in.header.frame_id, tf2::TimePointZero); tf2::doTransform(message_in, message_out, transform); } catch (const tf2::TransformException & e) { // TODO(destogl): Use RCLCPP_ERROR_THROTTLE - RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + control_frame_ + "' and '" + message_in.header.frame_id + "'."); + RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + ik_base_frame_ + "' and '" + message_in.header.frame_id + "'."); return controller_interface::return_type::ERROR; } } else { @@ -208,21 +209,6 @@ class AdmittanceRule output_tf = input_tf * transform; tf2::toMsg(output_tf, output); } - - template - void - transform_ik_tip_to_control_frame(const Type & base_to_ik_tip, Type & base_to_toollink) - { - direct_transform(base_to_ik_tip, ik_tip_to_control_frame_tf_, base_to_toollink); - } - - template - void - transform_control_to_ik_tip_frame(const Type & base_to_toollink, Type & base_to_ik_tip) - { - direct_transform(base_to_toollink, control_frame_to_ik_tip_tf_, base_to_ik_tip); - } - }; } // namespace admittance_controller diff --git a/admittance_controller/src/admittance_rule.cpp b/admittance_controller/src/admittance_rule.cpp index 4ed9a44db3..4e085b9685 100644 --- a/admittance_controller/src/admittance_rule.cpp +++ b/admittance_controller/src/admittance_rule.cpp @@ -173,15 +173,17 @@ controller_interface::return_type AdmittanceRule::configure(rclcpp::Node::Shared // Initialize IK ik_ = std::make_shared(node, ik_group_name_); + reset(); + return controller_interface::return_type::OK; } controller_interface::return_type AdmittanceRule::reset() { measured_wrench_control_frame_arr_.fill(0.0); - target_pose_control_frame_arr_.fill(0.0); + target_pose_ik_base_frame_arr_.fill(0.0); - current_pose_control_frame_arr_.fill(0.0); + current_pose_ik_base_frame_arr_.fill(0.0); angles_error_.fill(0.0); @@ -189,7 +191,8 @@ controller_interface::return_type AdmittanceRule::reset() desired_velocity_previous_arr_.fill(0.0); desired_acceleration_previous_arr_.fill(0.0); - get_pose_of_control_frame_in_base_frame(current_pose_base_frame_); + get_pose_of_control_frame_in_base_frame(current_pose_ik_base_frame_); + feedforward_pose_ik_base_frame_ = current_pose_ik_base_frame_; // Initialize ik_tip and tool_frame transformations - those are fixed transformations tf2::Stamped tf2_transform; @@ -216,26 +219,22 @@ controller_interface::return_type AdmittanceRule::update( trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_state ) { - // Convert inputs to control frame - transform_message_to_control_frame(target_pose, target_pose_control_frame_); + // Convert inputs to ik_base frame (assumed stationary) + transform_message_to_ik_base_frame(target_pose, target_pose_ik_base_frame_); + // TODO(andyz): what if there is a hardware offset? if (!hardware_state_has_offset_) { - get_pose_of_control_frame_in_base_frame(current_pose_base_frame_); - transform_message_to_control_frame(current_pose_base_frame_, current_pose_control_frame_); + get_pose_of_control_frame_in_base_frame(current_pose_ik_base_frame_); } - // TODO(destogl): Can this work properly, when considering offset between states and commands? -// else { -// current_pose_control_frame_ = desired_pose_control_frame_; -// } // Convert all data to arrays for simpler calculation - convert_message_to_array(target_pose_control_frame_, target_pose_control_frame_arr_); - convert_message_to_array(current_pose_control_frame_, current_pose_control_frame_arr_); + convert_message_to_array(target_pose_ik_base_frame_, target_pose_ik_base_frame_arr_); + convert_message_to_array(current_pose_ik_base_frame_, current_pose_ik_base_frame_arr_); std::array pose_error; for (auto i = 0u; i < 6; ++i) { - pose_error[i] = current_pose_control_frame_arr_[i] - target_pose_control_frame_arr_[i]; + pose_error[i] = current_pose_ik_base_frame_arr_[i] - target_pose_ik_base_frame_arr_[i]; if (i >= 3) { pose_error[i] = angles::normalize_angle(pose_error[i]); } @@ -261,7 +260,7 @@ controller_interface::return_type AdmittanceRule::update( std::vector target_ik_tip_deltas_vec(6); // Get feed-forward cartesian deltas in the ik_base frame. - // Since this is MoveIt's working frame, the transform is identity. + // Since ik_base is MoveIt's working frame, the transform is identity. geometry_msgs::msg::TransformStamped transform_ik_base_to_desired_frame; transform_ik_base_to_desired_frame.header.frame_id = ik_base_frame_; transform_ik_base_to_desired_frame.transform.rotation.w = 1; @@ -274,32 +273,22 @@ controller_interface::return_type AdmittanceRule::update( return controller_interface::return_type::ERROR; } - // Get current robot pose - if (!hardware_state_has_offset_) { - get_pose_of_control_frame_in_base_frame(current_pose_base_frame_); - } - // TODO(destogl): Can this work properly, when considering offset between states and commands? -// else { -// current_pose_control_frame_ = desired_pose_control_frame_; -// } - - // Add deltas to current pose to get the desired pose - geometry_msgs::msg::PoseStamped target_pose_ik_base_frame = current_pose_base_frame_; - target_pose_ik_base_frame.pose.position.x += target_ik_tip_deltas_vec.at(0); - target_pose_ik_base_frame.pose.position.y += target_ik_tip_deltas_vec.at(1); - target_pose_ik_base_frame.pose.position.z += target_ik_tip_deltas_vec.at(2); - - tf2::Quaternion q(target_pose_ik_base_frame.pose.orientation.x, target_pose_ik_base_frame.pose.orientation.y, target_pose_ik_base_frame.pose.orientation.z, target_pose_ik_base_frame.pose.orientation.w); + // Add deltas to previously-desired pose to get the next desired pose + feedforward_pose_ik_base_frame_.pose.position.x += target_ik_tip_deltas_vec.at(0); + feedforward_pose_ik_base_frame_.pose.position.y += target_ik_tip_deltas_vec.at(1); + feedforward_pose_ik_base_frame_.pose.position.z += target_ik_tip_deltas_vec.at(2); + + tf2::Quaternion q(feedforward_pose_ik_base_frame_.pose.orientation.x, feedforward_pose_ik_base_frame_.pose.orientation.y, feedforward_pose_ik_base_frame_.pose.orientation.z, feedforward_pose_ik_base_frame_.pose.orientation.w); tf2::Quaternion q_rot; q_rot.setRPY(target_ik_tip_deltas_vec.at(3), target_ik_tip_deltas_vec.at(4), target_ik_tip_deltas_vec.at(5)); q = q_rot * q; q.normalize(); - target_pose_ik_base_frame.pose.orientation.w = q.w(); - target_pose_ik_base_frame.pose.orientation.x = q.x(); - target_pose_ik_base_frame.pose.orientation.y = q.y(); - target_pose_ik_base_frame.pose.orientation.z = q.z(); + feedforward_pose_ik_base_frame_.pose.orientation.w = q.w(); + feedforward_pose_ik_base_frame_.pose.orientation.x = q.x(); + feedforward_pose_ik_base_frame_.pose.orientation.y = q.y(); + feedforward_pose_ik_base_frame_.pose.orientation.z = q.z(); - return update(current_joint_state, measured_wrench, target_pose_ik_base_frame, period, desired_joint_state); + return update(current_joint_state, measured_wrench, feedforward_pose_ik_base_frame_, period, desired_joint_state); } controller_interface::return_type AdmittanceRule::update( @@ -312,7 +301,7 @@ controller_interface::return_type AdmittanceRule::update( ) { // TODO(destogl): Implement this update - // transform_message_to_control_frame(**input_force_cmd, force_cmd_ctrl_frame_); + // transform_message_to_ik_base_frame(**input_force_cmd, force_cmd_ctrl_frame_); // TODO(destogl) reuse things from other update return controller_interface::return_type::OK; @@ -322,11 +311,12 @@ controller_interface::return_type AdmittanceRule::get_controller_state( control_msgs::msg::AdmittanceControllerState & state_message) { // state_message.input_force_control_frame = target_force_control_frame_; - state_message.input_pose_control_frame = target_pose_control_frame_; + // TODO(andyz): update msg fields +// state_message.input_pose_control_frame = target_pose_ik_base_frame_; state_message.measured_wrench = measured_wrench_; state_message.measured_wrench_filtered = measured_wrench_filtered_; state_message.measured_wrench_control_frame = measured_wrench_control_frame_; - state_message.desired_pose = desired_pose_control_frame_; + state_message.desired_pose = desired_pose_ik_base_frame_; state_message.relative_desired_pose = relative_desired_pose_; return controller_interface::return_type::OK; @@ -388,7 +378,7 @@ void AdmittanceRule::process_wrench_measurements( // measured_wrench_filtered_ = measured_wrench_; // } - transform_message_to_control_frame(measured_wrench_filtered_, measured_wrench_control_frame_); + transform_message_to_ik_base_frame(measured_wrench_filtered_, measured_wrench_control_frame_); convert_message_to_array(measured_wrench_control_frame_, measured_wrench_control_frame_arr_); // If at least one measured force is nan set all to 0 @@ -428,25 +418,17 @@ controller_interface::return_type AdmittanceRule::calculate_desired_joint_state( trajectory_msgs::msg::JointTrajectoryPoint & desired_joint_state ) { - // Do clean conversion to the goal pose using transform and not messing with Euler angles convert_array_to_message(relative_desired_pose_arr_, relative_desired_pose_); - tf2::doTransform(current_pose_control_frame_, desired_pose_control_frame_, relative_desired_pose_); - transform_ik_tip_to_control_frame(desired_pose_control_frame_.pose, desired_pose_control_frame_.pose); - // Calculate desired Cartesian displacement of the robot - geometry_msgs::msg::TransformStamped control_frame_to_ik_base; - try { - control_frame_to_ik_base = tf_buffer_->lookupTransform(control_frame_, ik_base_frame_, tf2::TimePointZero); - } catch (const tf2::TransformException & e) { - // TODO(destogl): Use RCLCPP_ERROR_THROTTLE - RCLCPP_ERROR(rclcpp::get_logger("AdmittanceRule"), "LookupTransform failed between '" + ik_base_frame_ + "' and '" + control_frame_ + "'."); - return controller_interface::return_type::ERROR; - } + // Since ik_base is MoveIt's working frame, the transform is identity. + geometry_msgs::msg::TransformStamped transform_ik_base_to_desired_frame; + transform_ik_base_to_desired_frame.header.frame_id = ik_base_frame_; + transform_ik_base_to_desired_frame.transform.rotation.w = 1; // Use Jacobian-based IK std::vector relative_desired_pose_vec(relative_desired_pose_arr_.begin(), relative_desired_pose_arr_.end()); if (ik_->convertCartesianDeltasToJointDeltas( - relative_desired_pose_vec, control_frame_to_ik_base, relative_desired_joint_state_vec_)){ + relative_desired_pose_vec, transform_ik_base_to_desired_frame, relative_desired_joint_state_vec_)){ for (auto i = 0u; i < desired_joint_state.positions.size(); ++i) { desired_joint_state.positions[i] = current_joint_state.positions[i] + relative_desired_joint_state_vec_[i]; desired_joint_state.velocities[i] = relative_desired_joint_state_vec_[i] / period.seconds();