diff --git a/franka_control/launch/franka_control.launch b/franka_control/launch/franka_control.launch index df7b1437e..b7c001767 100644 --- a/franka_control/launch/franka_control.launch +++ b/franka_control/launch/franka_control.launch @@ -1,7 +1,7 @@ - + diff --git a/franka_example_controllers/CMakeLists.txt b/franka_example_controllers/CMakeLists.txt index 2ad6b6b09..f4ddfdfe0 100644 --- a/franka_example_controllers/CMakeLists.txt +++ b/franka_example_controllers/CMakeLists.txt @@ -72,6 +72,7 @@ catkin_package( ) add_library(franka_example_controllers + src/hiro_joint_velocity_example_controller.cpp src/elbow_example_controller.cpp src/cartesian_pose_example_controller.cpp src/cartesian_velocity_example_controller.cpp @@ -79,7 +80,9 @@ add_library(franka_example_controllers src/joint_velocity_example_controller.cpp src/model_example_controller.cpp src/joint_impedance_example_controller.cpp + src/hiro_joint_impedance_example_controller.cpp src/cartesian_impedance_example_controller.cpp + src/hiro_cartesian_impedance_example_controller.cpp src/force_example_controller.cpp src/dual_arm_cartesian_impedance_example_controller.cpp src/teleop_joint_pd_example_controller.cpp diff --git a/franka_example_controllers/config/franka_example_controllers.yaml b/franka_example_controllers/config/franka_example_controllers.yaml index b8ec8cb0d..068c2c983 100644 --- a/franka_example_controllers/config/franka_example_controllers.yaml +++ b/franka_example_controllers/config/franka_example_controllers.yaml @@ -9,6 +9,18 @@ joint_velocity_example_controller: - $(arg arm_id)_joint5 - $(arg arm_id)_joint6 - $(arg arm_id)_joint7 + +hiro_joint_velocity_example_controller: + type: franka_example_controllers/HIROJointVelocityExampleController + arm_id: $(arg arm_id) + joint_names: + - $(arg arm_id)_joint1 + - $(arg arm_id)_joint2 + - $(arg arm_id)_joint3 + - $(arg arm_id)_joint4 + - $(arg arm_id)_joint5 + - $(arg arm_id)_joint6 + - $(arg arm_id)_joint7 joint_position_example_controller: type: franka_example_controllers/JointPositionExampleController @@ -49,6 +61,39 @@ force_example_controller: - $(arg arm_id)_joint6 - $(arg arm_id)_joint7 +hiro_joint_impedance_example_controller: + type: franka_example_controllers/HIROJointImpedanceExampleController + arm_id: $(arg arm_id) + joint_names: + - $(arg arm_id)_joint1 + - $(arg arm_id)_joint2 + - $(arg arm_id)_joint3 + - $(arg arm_id)_joint4 + - $(arg arm_id)_joint5 + - $(arg arm_id)_joint6 + - $(arg arm_id)_joint7 + k_gains: + - 10.0 + - 10.0 + - 10.0 + - 10.0 + - 5.0 + - 3.0 + - 1.0 + d_gains: + - 5.0 + - 5.0 + - 5.0 + - 2.0 + - 2.0 + - 2.0 + - 1.0 + radius: 0.1 + acceleration_time: 2.0 + vel_max: 0.15 + publish_rate: 10.0 + coriolis_factor: 1.0 + joint_impedance_example_controller: type: franka_example_controllers/JointImpedanceExampleController arm_id: $(arg arm_id) @@ -94,6 +139,18 @@ cartesian_impedance_example_controller: - $(arg arm_id)_joint6 - $(arg arm_id)_joint7 +hiro_cartesian_impedance_example_controller: + type: franka_example_controllers/HIROCartesianImpedanceExampleController + arm_id: $(arg arm_id) + joint_names: + - $(arg arm_id)_joint1 + - $(arg arm_id)_joint2 + - $(arg arm_id)_joint3 + - $(arg arm_id)_joint4 + - $(arg arm_id)_joint5 + - $(arg arm_id)_joint6 + - $(arg arm_id)_joint7 + dual_arm_cartesian_impedance_example_controller: type: franka_example_controllers/DualArmCartesianImpedanceExampleController right: diff --git a/franka_example_controllers/franka_example_controllers_plugin.xml b/franka_example_controllers/franka_example_controllers_plugin.xml index 208bb6b67..8a20a1238 100644 --- a/franka_example_controllers/franka_example_controllers_plugin.xml +++ b/franka_example_controllers/franka_example_controllers_plugin.xml @@ -3,6 +3,11 @@ A controller that executes a short motion based on joint velocities to demonstrate correct usage + + + + Starts ros message controller interface for the HIRO lab + @@ -34,11 +39,21 @@ A controller that tracks a cartesian path with a joint impedance controller that compensates coriolis torques. The torque commands are compared to measured torques in Console outputs. + + + A controller that tracks a cartesian path with a joint impedance controller that compensates coriolis torques. The torque commands are compared to measured torques in Console outputs. + + A controller that renders a spring damper system in cartesian space. Compliance parameters and the equilibrium pose can be modified online with dynamic reconfigure and an interactive marker respectively. + + + A controller that renders a spring damper system in cartesian space. Compliance parameters and the equilibrium pose can be modified online with dynamic reconfigure and an interactive marker respectively. + + A PI controller that applies a force corresponding to a user-provided desired mass in the z axis. The desired mass value can be modified online with dynamic reconfigure. diff --git a/franka_example_controllers/include/franka_example_controllers/hiro_cartesian_impedance_example_controller.h b/franka_example_controllers/include/franka_example_controllers/hiro_cartesian_impedance_example_controller.h new file mode 100644 index 000000000..e8c6b2f52 --- /dev/null +++ b/franka_example_controllers/include/franka_example_controllers/hiro_cartesian_impedance_example_controller.h @@ -0,0 +1,71 @@ +// Copyright (c) 2017 Franka Emika GmbH +// Use of this source code is governed by the Apache-2.0 license, see LICENSE +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace franka_example_controllers { + +class HIROCartesianImpedanceExampleController : public controller_interface::MultiInterfaceController< + franka_hw::FrankaModelInterface, + hardware_interface::EffortJointInterface, + franka_hw::FrankaStateInterface> { + public: + bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) override; + void starting(const ros::Time&) override; + void update(const ros::Time&, const ros::Duration& period) override; + + private: + // Saturation + Eigen::Matrix saturateTorqueRate( + const Eigen::Matrix& tau_d_calculated, + const Eigen::Matrix& tau_J_d); // NOLINT (readability-identifier-naming) + + std::unique_ptr state_handle_; + std::unique_ptr model_handle_; + std::vector joint_handles_; + + double filter_params_{0.005}; + double nullspace_stiffness_{20.0}; + double nullspace_stiffness_target_{20.0}; + const double delta_tau_max_{1.0}; + Eigen::Matrix cartesian_stiffness_; + Eigen::Matrix cartesian_stiffness_target_; + Eigen::Matrix cartesian_damping_; + Eigen::Matrix cartesian_damping_target_; + Eigen::Matrix q_d_nullspace_; + Eigen::Vector3d position_d_; + Eigen::Quaterniond orientation_d_; + std::mutex position_and_orientation_d_target_mutex_; + Eigen::Vector3d position_d_target_; + Eigen::Quaterniond orientation_d_target_; + + // Dynamic reconfigure + std::unique_ptr> + dynamic_server_compliance_param_; + ros::NodeHandle dynamic_reconfigure_compliance_param_node_; + void complianceParamCallback(franka_example_controllers::compliance_paramConfig& config, + uint32_t level); + + // Equilibrium pose subscriber + ros::Subscriber sub_equilibrium_pose_; + void equilibriumPoseCallback(const geometry_msgs::PoseStampedConstPtr& msg); +}; + +} // namespace franka_example_controllers diff --git a/franka_example_controllers/include/franka_example_controllers/hiro_joint_impedance_example_controller.h b/franka_example_controllers/include/franka_example_controllers/hiro_joint_impedance_example_controller.h new file mode 100644 index 000000000..4491fe920 --- /dev/null +++ b/franka_example_controllers/include/franka_example_controllers/hiro_joint_impedance_example_controller.h @@ -0,0 +1,70 @@ +// Copyright (c) 2017 Franka Emika GmbH +// Use of this source code is governed by the Apache-2.0 license, see LICENSE +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace franka_example_controllers { + +class HIROJointImpedanceExampleController : public controller_interface::MultiInterfaceController< + franka_hw::FrankaModelInterface, + hardware_interface::EffortJointInterface, + franka_hw::FrankaPoseCartesianInterface> { + public: + bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) override; + void starting(const ros::Time&) override; + void update(const ros::Time&, const ros::Duration& period) override; + + private: + // Saturation + std::array saturateTorqueRate( + const std::array& tau_d_calculated, + const std::array& tau_J_d); // NOLINT (readability-identifier-naming) + + std::unique_ptr cartesian_pose_handle_; + std::unique_ptr model_handle_; + std::vector joint_handles_; + + static constexpr double kDeltaTauMax{1.0}; + double radius_{0.1}; + double acceleration_time_{2.0}; + double vel_max_{0.05}; + double angle_{0.0}; + double vel_current_{0.0}; + + std::vector k_gains_; + std::vector d_gains_; + double coriolis_factor_{1.0}; + std::array dq_filtered_; + std::array initial_pose_; + + franka_hw::TriggerRate rate_trigger_{1.0}; + std::array last_tau_d_{}; + realtime_tools::RealtimePublisher torques_publisher_; + // Added by Caleb E. + std::mutex joint_position_and_velocity_d_target_mutex_; + std::vector joint_positions_{0,0,0,0,0,0,0}; + bool callback_done_once = false; + // void jointCommandCb(const sensor_msgs::JointState::ConstPtr& joint_pos_commands); + void jointCommandCb(const std_msgs::Float32MultiArray::ConstPtr& joint_pos_commands); + ros::Subscriber sub_command_; +}; + +} // namespace franka_example_controllers diff --git a/franka_example_controllers/include/franka_example_controllers/hiro_joint_velocity_example_controller.h b/franka_example_controllers/include/franka_example_controllers/hiro_joint_velocity_example_controller.h new file mode 100644 index 000000000..a36790fce --- /dev/null +++ b/franka_example_controllers/include/franka_example_controllers/hiro_joint_velocity_example_controller.h @@ -0,0 +1,47 @@ +// Copyright (c) 2017 Franka Emika GmbH +// Use of this source code is governed by the Apache-2.0 license, see LICENSE +#pragma once + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace franka_example_controllers { + +class HIROJointVelocityExampleController : public controller_interface::MultiInterfaceController< + hardware_interface::VelocityJointInterface, + franka_hw::FrankaStateInterface> { + public: + bool init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& node_handle) override; + void update(const ros::Time&, const ros::Duration& period) override; + void starting(const ros::Time&) override; + void stopping(const ros::Time&) override; + + private: + hardware_interface::VelocityJointInterface* velocity_joint_interface_; + std::vector velocity_joint_handles_; + ros::Duration elapsed_time_; + bool first_sample_taken; + void jointCommandCb(const sensor_msgs::JointState::ConstPtr& joint_pos_commands); + ros::Subscriber sub_command_; + std::unique_ptr state_handle_; + int last_time_called; + // std::array joint_velocities{}; + std::vector joint_positions_{0,0,0,0,0,0,0}; + float p_ = 1.9; + float d_ = 0.0; + // float curr_error_; + std::vector prev_error_{0,0,0,0,0,0,0}; + std::vector curr_error_{0,0,0,0,0,0,0}; + bool callback_done_once = false; + // franka::RobotState robot_state +}; + +} // namespace franka_example_controllers diff --git a/franka_example_controllers/launch/hiro_cartesian_impedance_example_controller.launch b/franka_example_controllers/launch/hiro_cartesian_impedance_example_controller.launch new file mode 100644 index 000000000..d3eb89a7b --- /dev/null +++ b/franka_example_controllers/launch/hiro_cartesian_impedance_example_controller.launch @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/franka_example_controllers/launch/hiro_joint_impedance_example_controller.launch b/franka_example_controllers/launch/hiro_joint_impedance_example_controller.launch new file mode 100644 index 000000000..d00680246 --- /dev/null +++ b/franka_example_controllers/launch/hiro_joint_impedance_example_controller.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/franka_example_controllers/launch/hiro_joint_velocity_example_controller.launch b/franka_example_controllers/launch/hiro_joint_velocity_example_controller.launch new file mode 100644 index 000000000..cff112b5e --- /dev/null +++ b/franka_example_controllers/launch/hiro_joint_velocity_example_controller.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/franka_example_controllers/launch/joint_velocity_example_controller.launch b/franka_example_controllers/launch/joint_velocity_example_controller.launch index 1b9a68a94..1a6f63a26 100644 --- a/franka_example_controllers/launch/joint_velocity_example_controller.launch +++ b/franka_example_controllers/launch/joint_velocity_example_controller.launch @@ -1,6 +1,6 @@ - + diff --git a/franka_example_controllers/launch/move_to_start.launch b/franka_example_controllers/launch/move_to_start.launch index fd9ce5032..ab7883f84 100644 --- a/franka_example_controllers/launch/move_to_start.launch +++ b/franka_example_controllers/launch/move_to_start.launch @@ -1,7 +1,7 @@ - + diff --git a/franka_example_controllers/src/hiro_cartesian_impedance_example_controller.cpp b/franka_example_controllers/src/hiro_cartesian_impedance_example_controller.cpp new file mode 100644 index 000000000..85e387e13 --- /dev/null +++ b/franka_example_controllers/src/hiro_cartesian_impedance_example_controller.cpp @@ -0,0 +1,248 @@ +// Copyright (c) 2017 Franka Emika GmbH +// Use of this source code is governed by the Apache-2.0 license, see LICENSE +#include + +#include +#include + +#include +#include +#include +#include + +#include + +namespace franka_example_controllers { + +bool HIROCartesianImpedanceExampleController::init(hardware_interface::RobotHW* robot_hw, + ros::NodeHandle& node_handle) { + std::vector cartesian_stiffness_vector; + std::vector cartesian_damping_vector; + + sub_equilibrium_pose_ = node_handle.subscribe( + "equilibrium_pose", 20, &HIROCartesianImpedanceExampleController::equilibriumPoseCallback, this, + ros::TransportHints().reliable().tcpNoDelay()); + + std::string arm_id; + if (!node_handle.getParam("arm_id", arm_id)) { + ROS_ERROR_STREAM("CartesianImpedanceExampleController: Could not read parameter arm_id"); + return false; + } + std::vector joint_names; + if (!node_handle.getParam("joint_names", joint_names) || joint_names.size() != 7) { + ROS_ERROR( + "CartesianImpedanceExampleController: Invalid or no joint_names parameters provided, " + "aborting controller init!"); + return false; + } + + auto* model_interface = robot_hw->get(); + if (model_interface == nullptr) { + ROS_ERROR_STREAM( + "CartesianImpedanceExampleController: Error getting model interface from hardware"); + return false; + } + try { + model_handle_ = std::make_unique( + model_interface->getHandle(arm_id + "_model")); + } catch (hardware_interface::HardwareInterfaceException& ex) { + ROS_ERROR_STREAM( + "CartesianImpedanceExampleController: Exception getting model handle from interface: " + << ex.what()); + return false; + } + + auto* state_interface = robot_hw->get(); + if (state_interface == nullptr) { + ROS_ERROR_STREAM( + "CartesianImpedanceExampleController: Error getting state interface from hardware"); + return false; + } + try { + state_handle_ = std::make_unique( + state_interface->getHandle(arm_id + "_robot")); + } catch (hardware_interface::HardwareInterfaceException& ex) { + ROS_ERROR_STREAM( + "CartesianImpedanceExampleController: Exception getting state handle from interface: " + << ex.what()); + return false; + } + + auto* effort_joint_interface = robot_hw->get(); + if (effort_joint_interface == nullptr) { + ROS_ERROR_STREAM( + "CartesianImpedanceExampleController: Error getting effort joint interface from hardware"); + return false; + } + for (size_t i = 0; i < 7; ++i) { + try { + joint_handles_.push_back(effort_joint_interface->getHandle(joint_names[i])); + } catch (const hardware_interface::HardwareInterfaceException& ex) { + ROS_ERROR_STREAM( + "CartesianImpedanceExampleController: Exception getting joint handles: " << ex.what()); + return false; + } + } + + dynamic_reconfigure_compliance_param_node_ = + ros::NodeHandle(node_handle.getNamespace() + "/dynamic_reconfigure_compliance_param_node"); + + dynamic_server_compliance_param_ = std::make_unique< + dynamic_reconfigure::Server>( + + dynamic_reconfigure_compliance_param_node_); + dynamic_server_compliance_param_->setCallback( + boost::bind(&HIROCartesianImpedanceExampleController::complianceParamCallback, this, _1, _2)); + + position_d_.setZero(); + orientation_d_.coeffs() << 0.0, 0.0, 0.0, 1.0; + position_d_target_.setZero(); + orientation_d_target_.coeffs() << 0.0, 0.0, 0.0, 1.0; + + cartesian_stiffness_.setZero(); + cartesian_damping_.setZero(); + + return true; +} + +void HIROCartesianImpedanceExampleController::starting(const ros::Time& /*time*/) { + // compute initial velocity with jacobian and set x_attractor and q_d_nullspace + // to initial configuration + franka::RobotState initial_state = state_handle_->getRobotState(); + // get jacobian + std::array jacobian_array = + model_handle_->getZeroJacobian(franka::Frame::kEndEffector); + // convert to eigen + Eigen::Map> q_initial(initial_state.q.data()); + Eigen::Affine3d initial_transform(Eigen::Matrix4d::Map(initial_state.O_T_EE.data())); + + // set equilibrium point to current state + position_d_ = initial_transform.translation(); + orientation_d_ = Eigen::Quaterniond(initial_transform.rotation()); + position_d_target_ = initial_transform.translation(); + orientation_d_target_ = Eigen::Quaterniond(initial_transform.rotation()); + + // set nullspace equilibrium configuration to initial q + q_d_nullspace_ = q_initial; +} + +void HIROCartesianImpedanceExampleController::update(const ros::Time& /*time*/, + const ros::Duration& /*period*/) { + // get state variables + franka::RobotState robot_state = state_handle_->getRobotState(); + std::array coriolis_array = model_handle_->getCoriolis(); + std::array jacobian_array = + model_handle_->getZeroJacobian(franka::Frame::kEndEffector); + + // convert to Eigen::up + Eigen::Map> coriolis(coriolis_array.data()); + Eigen::Map> jacobian(jacobian_array.data()); + Eigen::Map> q(robot_state.q.data()); + Eigen::Map> dq(robot_state.dq.data()); + Eigen::Map> tau_J_d( // NOLINT (readability-identifier-naming) + robot_state.tau_J_d.data()); + Eigen::Affine3d transform(Eigen::Matrix4d::Map(robot_state.O_T_EE.data())); + Eigen::Vector3d position(transform.translation()); + Eigen::Quaterniond orientation(transform.rotation()); + + // compute error to desired pose + // position error + Eigen::Matrix error; + error.head(3) << position - position_d_; + + // orientation error + if (orientation_d_.coeffs().dot(orientation.coeffs()) < 0.0) { + orientation.coeffs() << -orientation.coeffs(); + } + // "difference" quaternion + Eigen::Quaterniond error_quaternion(orientation.inverse() * orientation_d_); + error.tail(3) << error_quaternion.x(), error_quaternion.y(), error_quaternion.z(); + // Transform to base frame + error.tail(3) << -transform.rotation() * error.tail(3); + + // compute control + // allocate variables + Eigen::VectorXd tau_task(7), tau_nullspace(7), tau_d(7); + + // pseudoinverse for nullspace handling + // kinematic pseuoinverse + Eigen::MatrixXd jacobian_transpose_pinv; + pseudoInverse(jacobian.transpose(), jacobian_transpose_pinv); + + // Cartesian PD control with damping ratio = 1 + tau_task << jacobian.transpose() * + (-cartesian_stiffness_ * error - cartesian_damping_ * (jacobian * dq)); + + // nullspace PD control with damping ratio = 1 + tau_nullspace << (Eigen::MatrixXd::Identity(7, 7) - + jacobian.transpose() * jacobian_transpose_pinv) * + (nullspace_stiffness_ * (q_d_nullspace_ - q) - + (2.0 * sqrt(nullspace_stiffness_)) * dq); + // Desired torque + tau_d << tau_task + tau_nullspace + coriolis; + // Saturate torque rate to avoid discontinuities + tau_d << saturateTorqueRate(tau_d, tau_J_d); + for (size_t i = 0; i < 7; ++i) { + joint_handles_[i].setCommand(tau_d(i)); + } + + // update parameters changed online either through dynamic reconfigure or through the interactive + // target by filtering + cartesian_stiffness_ = + filter_params_ * cartesian_stiffness_target_ + (1.0 - filter_params_) * cartesian_stiffness_; + cartesian_damping_ = + filter_params_ * cartesian_damping_target_ + (1.0 - filter_params_) * cartesian_damping_; + nullspace_stiffness_ = + filter_params_ * nullspace_stiffness_target_ + (1.0 - filter_params_) * nullspace_stiffness_; + std::lock_guard position_d_target_mutex_lock( + position_and_orientation_d_target_mutex_); + position_d_ = filter_params_ * position_d_target_ + (1.0 - filter_params_) * position_d_; + orientation_d_ = orientation_d_.slerp(filter_params_, orientation_d_target_); +} + +Eigen::Matrix HIROCartesianImpedanceExampleController::saturateTorqueRate( + const Eigen::Matrix& tau_d_calculated, + const Eigen::Matrix& tau_J_d) { // NOLINT (readability-identifier-naming) + Eigen::Matrix tau_d_saturated{}; + for (size_t i = 0; i < 7; i++) { + double difference = tau_d_calculated[i] - tau_J_d[i]; + tau_d_saturated[i] = + tau_J_d[i] + std::max(std::min(difference, delta_tau_max_), -delta_tau_max_); + } + return tau_d_saturated; +} + +void HIROCartesianImpedanceExampleController::complianceParamCallback( + franka_example_controllers::compliance_paramConfig& config, + uint32_t /*level*/) { + cartesian_stiffness_target_.setIdentity(); + cartesian_stiffness_target_.topLeftCorner(3, 3) + << config.translational_stiffness * Eigen::Matrix3d::Identity(); + cartesian_stiffness_target_.bottomRightCorner(3, 3) + << config.rotational_stiffness * Eigen::Matrix3d::Identity(); + cartesian_damping_target_.setIdentity(); + // Damping ratio = 1 + cartesian_damping_target_.topLeftCorner(3, 3) + << 2.0 * sqrt(config.translational_stiffness) * Eigen::Matrix3d::Identity(); + cartesian_damping_target_.bottomRightCorner(3, 3) + << 2.0 * sqrt(config.rotational_stiffness) * Eigen::Matrix3d::Identity(); + nullspace_stiffness_target_ = config.nullspace_stiffness; +} + +void HIROCartesianImpedanceExampleController::equilibriumPoseCallback( + const geometry_msgs::PoseStampedConstPtr& msg) { + std::lock_guard position_d_target_mutex_lock( + position_and_orientation_d_target_mutex_); + position_d_target_ << msg->pose.position.x, msg->pose.position.y, msg->pose.position.z; + Eigen::Quaterniond last_orientation_d_target(orientation_d_target_); + orientation_d_target_.coeffs() << msg->pose.orientation.x, msg->pose.orientation.y, + msg->pose.orientation.z, msg->pose.orientation.w; + if (last_orientation_d_target.coeffs().dot(orientation_d_target_.coeffs()) < 0.0) { + orientation_d_target_.coeffs() << -orientation_d_target_.coeffs(); + } +} + +} // namespace franka_example_controllers + +PLUGINLIB_EXPORT_CLASS(franka_example_controllers::HIROCartesianImpedanceExampleController, + controller_interface::ControllerBase) diff --git a/franka_example_controllers/src/hiro_joint_impedance_example_controller.cpp b/franka_example_controllers/src/hiro_joint_impedance_example_controller.cpp new file mode 100644 index 000000000..b58eb2298 --- /dev/null +++ b/franka_example_controllers/src/hiro_joint_impedance_example_controller.cpp @@ -0,0 +1,240 @@ +// Copyright (c) 2017 Franka Emika GmbH +// Use of this source code is governed by the Apache-2.0 license, see LICENSE +#include + +#include +#include + +#include +#include +#include + +#include + +namespace franka_example_controllers { + +bool HIROJointImpedanceExampleController::init(hardware_interface::RobotHW* robot_hw, + ros::NodeHandle& node_handle) { + std::string arm_id; + sub_command_ = node_handle.subscribe( + "/cu_joint_solution", 1, &HIROJointImpedanceExampleController::jointCommandCb, this); + + if (!node_handle.getParam("arm_id", arm_id)) { + ROS_ERROR("JointImpedanceExampleController: Could not read parameter arm_id"); + return false; + } + if (!node_handle.getParam("radius", radius_)) { + ROS_INFO_STREAM( + "JointImpedanceExampleController: No parameter radius, defaulting to: " << radius_); + } + if (std::fabs(radius_) < 0.005) { + ROS_INFO_STREAM("JointImpedanceExampleController: Set radius to small, defaulting to: " << 0.1); + radius_ = 0.1; + } + + if (!node_handle.getParam("vel_max", vel_max_)) { + ROS_INFO_STREAM( + "JointImpedanceExampleController: No parameter vel_max, defaulting to: " << vel_max_); + } + if (!node_handle.getParam("acceleration_time", acceleration_time_)) { + ROS_INFO_STREAM( + "JointImpedanceExampleController: No parameter acceleration_time, defaulting to: " + << acceleration_time_); + } + + std::vector joint_names; + if (!node_handle.getParam("joint_names", joint_names) || joint_names.size() != 7) { + ROS_ERROR( + "JointImpedanceExampleController: Invalid or no joint_names parameters provided, aborting " + "controller init!"); + return false; + } + + if (!node_handle.getParam("k_gains", k_gains_) || k_gains_.size() != 7) { + ROS_ERROR( + "JointImpedanceExampleController: Invalid or no k_gain parameters provided, aborting " + "controller init!"); + return false; + } + + if (!node_handle.getParam("d_gains", d_gains_) || d_gains_.size() != 7) { + ROS_ERROR( + "JointImpedanceExampleController: Invalid or no d_gain parameters provided, aborting " + "controller init!"); + return false; + } + + double publish_rate(30.0); + if (!node_handle.getParam("publish_rate", publish_rate)) { + ROS_INFO_STREAM("JointImpedanceExampleController: publish_rate not found. Defaulting to " + << publish_rate); + } + rate_trigger_ = franka_hw::TriggerRate(publish_rate); + + if (!node_handle.getParam("coriolis_factor", coriolis_factor_)) { + ROS_INFO_STREAM("JointImpedanceExampleController: coriolis_factor not found. Defaulting to " + << coriolis_factor_); + } + + auto* model_interface = robot_hw->get(); + if (model_interface == nullptr) { + ROS_ERROR_STREAM( + "JointImpedanceExampleController: Error getting model interface from hardware"); + return false; + } + try { + model_handle_ = std::make_unique( + model_interface->getHandle(arm_id + "_model")); + } catch (hardware_interface::HardwareInterfaceException& ex) { + ROS_ERROR_STREAM( + "JointImpedanceExampleController: Exception getting model handle from interface: " + << ex.what()); + return false; + } + + auto* cartesian_pose_interface = robot_hw->get(); + if (cartesian_pose_interface == nullptr) { + ROS_ERROR_STREAM( + "JointImpedanceExampleController: Error getting cartesian pose interface from hardware"); + return false; + } + try { + cartesian_pose_handle_ = std::make_unique( + cartesian_pose_interface->getHandle(arm_id + "_robot")); + } catch (hardware_interface::HardwareInterfaceException& ex) { + ROS_ERROR_STREAM( + "JointImpedanceExampleController: Exception getting cartesian pose handle from interface: " + << ex.what()); + return false; + } + + auto* effort_joint_interface = robot_hw->get(); + if (effort_joint_interface == nullptr) { + ROS_ERROR_STREAM( + "JointImpedanceExampleController: Error getting effort joint interface from hardware"); + return false; + } + for (size_t i = 0; i < 7; ++i) { + try { + joint_handles_.push_back(effort_joint_interface->getHandle(joint_names[i])); + } catch (const hardware_interface::HardwareInterfaceException& ex) { + ROS_ERROR_STREAM( + "JointImpedanceExampleController: Exception getting joint handles: " << ex.what()); + return false; + } + } + torques_publisher_.init(node_handle, "torque_comparison", 1); + + std::fill(dq_filtered_.begin(), dq_filtered_.end(), 0); + + return true; +} + +void HIROJointImpedanceExampleController::starting(const ros::Time& /*time*/) { + initial_pose_ = cartesian_pose_handle_->getRobotState().O_T_EE_d; +} + +// void HIROJointImpedanceExampleController::jointCommandCb(const std_msgs::Float32MultiArray::ConstPtr& joint_pos_commands) { +// std::lock_guard q_and_qdot_lock_mutex(joint_position_and_velocity_d_target_mutex_); +// for (int i = 0; i < 7; i++) joint_positions_[i] = joint_pos_commands->position[i]; +// this->callback_done_once = true; +// } + +void HIROJointImpedanceExampleController::jointCommandCb(const std_msgs::Float32MultiArray::ConstPtr& joint_pos_commands) { + std::lock_guard q_and_qdot_lock_mutex(joint_position_and_velocity_d_target_mutex_); + for (int i = 0; i < 7; i++) joint_positions_[i] = joint_pos_commands->data[i]; + this->callback_done_once = true; +} + +void HIROJointImpedanceExampleController::update(const ros::Time& /*time*/, + const ros::Duration& period) { + if (vel_current_ < vel_max_) { + vel_current_ += period.toSec() * std::fabs(vel_max_ / acceleration_time_); + } + vel_current_ = std::fmin(vel_current_, vel_max_); + + angle_ += period.toSec() * vel_current_ / std::fabs(radius_); + if (angle_ > 2 * M_PI) { + angle_ -= 2 * M_PI; + } + + double delta_y = radius_ * (1 - std::cos(angle_)); + double delta_z = radius_ * std::sin(angle_); + + std::array pose_desired = initial_pose_; + // pose_desired[13] += delta_y; + // pose_desired[14] += delta_z; + cartesian_pose_handle_->setCommand(pose_desired); + + franka::RobotState robot_state = cartesian_pose_handle_->getRobotState(); + std::array coriolis = model_handle_->getCoriolis(); + std::array gravity = model_handle_->getGravity(); + + double alpha = 0.99; + for (size_t i = 0; i < 7; i++) { + dq_filtered_[i] = (1 - alpha) * dq_filtered_[i] + alpha * robot_state.dq[i]; + } + + std::array tau_d_calculated; + if(this->callback_done_once){ + // std::cout<< "in CALLLBACK!!" << std::endl; + std::lock_guard q_and_qdot_lock_mutex(joint_position_and_velocity_d_target_mutex_); + for (size_t i = 0; i < 7; ++i) { + tau_d_calculated[i] = coriolis_factor_ * coriolis[i] + + k_gains_[i] * (joint_positions_[i] - robot_state.q[i]) + + d_gains_[i] * (0.5 *(joint_positions_[i] - robot_state.q[i]) - dq_filtered_[i]); + } + joint_position_and_velocity_d_target_mutex_.unlock(); + }else{ + for (size_t i = 0; i < 7; ++i) { + tau_d_calculated[i] = coriolis_factor_ * coriolis[i] + + k_gains_[i] * (robot_state.q_d[i] - robot_state.q[i]) + + d_gains_[i] * (robot_state.dq_d[i] - dq_filtered_[i]); + } + } + + // Maximum torque difference with a sampling rate of 1 kHz. The maximum torque rate is + // 1000 * (1 / sampling_time). + std::array tau_d_saturated = saturateTorqueRate(tau_d_calculated, robot_state.tau_J_d); + + for (size_t i = 0; i < 7; ++i) { + joint_handles_[i].setCommand(tau_d_saturated[i]); + } + + if (rate_trigger_() && torques_publisher_.trylock()) { + std::array tau_j = robot_state.tau_J; + std::array tau_error; + double error_rms(0.0); + for (size_t i = 0; i < 7; ++i) { + tau_error[i] = last_tau_d_[i] - tau_j[i]; + error_rms += std::sqrt(std::pow(tau_error[i], 2.0)) / 7.0; + } + torques_publisher_.msg_.root_mean_square_error = error_rms; + for (size_t i = 0; i < 7; ++i) { + torques_publisher_.msg_.tau_commanded[i] = last_tau_d_[i]; + torques_publisher_.msg_.tau_error[i] = tau_error[i]; + torques_publisher_.msg_.tau_measured[i] = tau_j[i]; + } + torques_publisher_.unlockAndPublish(); + } + + for (size_t i = 0; i < 7; ++i) { + last_tau_d_[i] = tau_d_saturated[i] + gravity[i]; + } +} + +std::array HIROJointImpedanceExampleController::saturateTorqueRate( + const std::array& tau_d_calculated, + const std::array& tau_J_d) { // NOLINT (readability-identifier-naming) + std::array tau_d_saturated{}; + for (size_t i = 0; i < 7; i++) { + double difference = tau_d_calculated[i] - tau_J_d[i]; + tau_d_saturated[i] = tau_J_d[i] + std::max(std::min(difference, kDeltaTauMax), -kDeltaTauMax); + } + return tau_d_saturated; +} + +} // namespace franka_example_controllers + +PLUGINLIB_EXPORT_CLASS(franka_example_controllers::HIROJointImpedanceExampleController, + controller_interface::ControllerBase) diff --git a/franka_example_controllers/src/hiro_joint_velocity_example_controller.cpp b/franka_example_controllers/src/hiro_joint_velocity_example_controller.cpp new file mode 100644 index 000000000..2f3d7dc3c --- /dev/null +++ b/franka_example_controllers/src/hiro_joint_velocity_example_controller.cpp @@ -0,0 +1,164 @@ +// Copyright (c) 2017 Franka Emika GmbH +// Use of this source code is governed by the Apache-2.0 license, see LICENSE +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +// #include + +namespace franka_example_controllers { + +using franka_gripper::homing; +using franka_gripper::HomingAction; +using franka_gripper::HomingGoalConstPtr; +using franka_gripper::HomingResult; + +// using franka_gripper::move; +// using franka_gripper::MoveAction; +// using franka_gripper::MoveGoalConstPtr; +// using franka_gripper::MoveResult; +// using franka::VacuumGripper; +// using franka::VacuumGripperState; + +bool HIROJointVelocityExampleController::init(hardware_interface::RobotHW* robot_hardware, + ros::NodeHandle& node_handle) { + + // Check if I can create the vac here. + // franka::VacuumGripper vacuum_gripper("192.168.0.198"); + // franka::VacuumGripperState vacuum_gripper_state = vacuum_gripper.readOnce(); + // std::cout << "***********Initial vacuum gripper state: " << vacuum_gripper_state << std::endl; + // std::cout << "Aftr the init state has been got!" << std::endl; + + // if (!vacuum_gripper.vacuum(100, std::chrono::milliseconds(1000))) { + // std::cout << "Failed to vacuum the object." << std::endl; + // return -1; + // } + + this->first_sample_taken = false; + // TODO: create a subscriber and callback so I can control the joint vel from a ros topic + sub_command_ = node_handle.subscribe( + "/relaxed_ik/joint_angle_solutions", 1, &HIROJointVelocityExampleController::jointCommandCb, this); + last_time_called = ros::Time::now().toSec(); + + velocity_joint_interface_ = robot_hardware->get(); + if (velocity_joint_interface_ == nullptr) { + ROS_ERROR( + "HIROJointVelocityExampleController: Error getting velocity joint interface from hardware!"); + return false; + } + + std::string arm_id; + if (!node_handle.getParam("arm_id", arm_id)) { + ROS_ERROR("HIROJointVelocityExampleController: Could not get parameter arm_id"); + return false; + } + + std::vector joint_names; + if (!node_handle.getParam("joint_names", joint_names)) { + ROS_ERROR("HIROJointVelocityExampleController: Could not parse joint names"); + } + if (joint_names.size() != 7) { + ROS_ERROR_STREAM("HIROJointVelocityExampleController: Wrong number of joint names, got " + << joint_names.size() << " instead of 7 names!"); + return false; + } + velocity_joint_handles_.resize(7); + for (size_t i = 0; i < 7; ++i) { + try { + velocity_joint_handles_[i] = velocity_joint_interface_->getHandle(joint_names[i]); + } catch (const hardware_interface::HardwareInterfaceException& ex) { + ROS_ERROR_STREAM( + "HIROJointVelocityExampleController: Exception getting joint handles: " << ex.what()); + return false; + } + } + + auto state_interface = robot_hardware->get(); + if (state_interface == nullptr) { + ROS_ERROR("HIROJointVelocityExampleController: Could not get state interface from hardware"); + return false; + } + + try { + state_handle_ = std::make_unique( + state_interface->getHandle(arm_id + "_robot")); + } catch (hardware_interface::HardwareInterfaceException& ex) { + ROS_ERROR_STREAM( + "PandaJointVelocityController: Exception getting state handle from interface: " << ex.what()); + return false; + } + + return true; +} + +void HIROJointVelocityExampleController::jointCommandCb(const sensor_msgs::JointState::ConstPtr& joint_pos_commands) { + for (int i = 0; i < 7; i++) joint_positions_[i] = joint_pos_commands->position[i]; + this->callback_done_once = true; + this->last_time_called = ros::Time::now().toSec(); + std::cout << this->last_time_called << std::endl; +} + +void HIROJointVelocityExampleController::starting(const ros::Time& /* time */) { + elapsed_time_ = ros::Duration(0.0); +} + +void HIROJointVelocityExampleController::update(const ros::Time& /* time */, + const ros::Duration& period) { + franka::RobotState robot_state = state_handle_->getRobotState(); + // Currently just a proportional movement. + // I am going to keep track of the diffrence so I can make this a PI loss first. + // Original -> 0.8 + // increasing p makes it feel a little more snappy. + //The following are okay with no overshoot: 1.5, 2.0, 3.0 + // TODO: We need to find the reason why there is a little grinding. + // Most likely need to smooth out some variable + elapsed_time_ += period; + if ((ros::Time::now().toSec() - this->last_time_called) > 3) { + for (int i = 0; i < 7; i++) velocity_joint_handles_[i].setCommand(0.0); + std::cout << "Set Vel to ZERO ******" << std::endl; + + } else if(this->callback_done_once) { // If command recieved, send the command to the controller + for (int i = 0; i < 7; i++) { + this->curr_error_[i] = (joint_positions_[i] - robot_state.q[i]); + double proportional = this->p_ * this->curr_error_[i]; + + if(!this->first_sample_taken){ + this->first_sample_taken = true; + velocity_joint_handles_[i].setCommand(proportional); + }else{ + double deriv = this->d_ * (this->curr_error_[i] - this->prev_error_[i]); + velocity_joint_handles_[i].setCommand(proportional); + } + + // Always set the previous error value + this->prev_error_[i] = (joint_positions_[i] - robot_state.q[i]); + } + } + else{ + for (int i = 0; i < 7; i++) velocity_joint_handles_[i].setCommand(0.0); + } +} + +void HIROJointVelocityExampleController::stopping(const ros::Time& /*time*/) { + // WARNING: DO NOT SEND ZERO VELOCITIES HERE AS IN CASE OF ABORTING DURING MOTION + // A JUMP TO ZERO WILL BE COMMANDED PUTTING HIGH LOADS ON THE ROBOT. LET THE DEFAULT + // BUILT-IN STOPPING BEHAVIOR SLOW DOWN THE ROBOT. +} + +} // namespace franka_example_controllers + +PLUGINLIB_EXPORT_CLASS(franka_example_controllers::HIROJointVelocityExampleController, + controller_interface::ControllerBase)