From d91322a01a12bda00f8059c5ba599aaad9727c14 Mon Sep 17 00:00:00 2001 From: Kei Okada Date: Sat, 23 Mar 2019 12:20:14 +0900 Subject: [PATCH] update joint_trjectory controller with mimic joint support see https://github.com/k-okada/mimic_joint_gazebo_tutorial.git for example ``` gripper_controller: type: position_controllers/JointTrajectoryController joints: - right_finger_joint mimic_joints: - left_finger_joint gains: right_finger_joint: {p: 20.0, i: 0.1, d: 0.2} left_finger_joint: {p: 20.0, i: 0.1, d: 0.2} ``` we added `mimic_joints` tag for the list of the mimic joint, and you have to add gains section. Mimic joint params (muliplier/offset) is taken from urdf model --- .../joint_trajectory_controller.h | 11 +++ .../joint_trajectory_controller_impl.h | 78 +++++++++++++++++++ 2 files changed, 89 insertions(+) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.h b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.h index c812c6c64..1a35fd426 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.h +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.h @@ -222,6 +222,17 @@ class JointTrajectoryController : public controller_interface::Controller mimic_joints_; ///< Handles to mimic joints. + std::vector mimic_joint_names_; ///< Mimic joint names. + std::vector mimic_urdf_joints_; ///< URDF joint of mimic joints + std::vector mimic_joint_ids_; ///< index of target joint in joints_ vector + + typename Segment::State mimic_current_state_; ///< Preallocated workspace variable. + typename Segment::State mimic_desired_state_; ///< Preallocated workspace variable. + typename Segment::State mimic_state_error_; ///< Preallocated workspace variable. + HwIfaceAdapter mimic_hw_iface_adapter_; ///< Adapts desired trajectory state to HW interface. + virtual bool updateTrajectoryCommand(const JointTrajectoryConstPtr& msg, RealtimeGoalHandlePtr gh, std::string* error_string = 0); virtual void trajectoryCommandCB(const JointTrajectoryConstPtr& msg); virtual void goalCB(GoalHandle gh); diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h index 4febbb877..bde2c698a 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h @@ -279,6 +279,8 @@ bool JointTrajectoryController::init(HardwareInt ROS_DEBUG_STREAM_NAMED(name_, "Found " << not_if << "continuous joint '" << joint_names_[i] << "' in '" << this->getHardwareInterfaceType() << "'."); + + ROS_INFO_STREAM("Loaidng joint " << joint_names_[i] << " (" << i << ")"); } assert(joints_.size() == angle_wraparound_.size()); @@ -347,6 +349,56 @@ bool JointTrajectoryController::init(HardwareInt state_publisher_->unlock(); } + // + // List of mimic joints + // + if (controller_nh_.hasParam("mimic_joints")) + { + mimic_joint_names_ = getStrings(controller_nh_, "mimic_joints"); + const unsigned int n_mimic_joints = mimic_joint_names_.size(); + mimic_joints_.resize(n_mimic_joints); + mimic_joint_ids_.resize(n_mimic_joints); + mimic_urdf_joints_.resize(n_mimic_joints); + for (unsigned int i = 0; i < n_mimic_joints; ++i) + { + // Mimic Joint handle + try { + mimic_joints_[i] = hw->hardware_interface::ResourceManager::getHandle(mimic_joint_names_[i]); + } + catch (...) + { + ROS_ERROR_STREAM_NAMED(name_, "Could not find joint '" << mimic_joint_names_[i] << "' in '" << + this->getHardwareInterfaceType() << "'."); + return false; + } + mimic_urdf_joints_[i] = urdf->getJoint(mimic_joint_names_[i]); + if (!(mimic_urdf_joints_[i] && mimic_urdf_joints_[i]->mimic)) + { + ROS_ERROR_STREAM_NAMED(name_, "Could not find joint '" << mimic_joint_names_[i] << "' in URDF"); + } + mimic_joint_ids_[i] = std::distance(joint_names_.begin(), std::find(joint_names_.begin(), joint_names_.end(), mimic_urdf_joints_[i]->mimic->joint_name)); + if (mimic_joint_ids_[i] == joint_names_.size()) + { + ROS_ERROR_STREAM_NAMED(name_, "Could not find joint '" << mimic_urdf_joints_[i]->mimic->joint_name << "' in joint_names"); + } + ROS_INFO_STREAM("Loaidng mimic_joint " << mimic_joint_names_[i] << " = " << joint_names_[mimic_joint_ids_[i]] << " (" << mimic_joint_ids_[i] << ") * " << mimic_urdf_joints_[i]->mimic->multiplier << " + " << mimic_urdf_joints_[i]->mimic->offset); + } + // Hardware interface adapter + mimic_hw_iface_adapter_.init(mimic_joints_, controller_nh_); + + mimic_current_state_ = typename Segment::State(n_mimic_joints); + mimic_desired_state_ = typename Segment::State(n_mimic_joints); + mimic_state_error_ = typename Segment::State(n_mimic_joints); + + // Initialize trajectory with all joints + typename Segment::State mimic_current_joint_state_ = typename Segment::State(1); + for (unsigned int i = 0; i < n_mimic_joints; ++i) + { + mimic_current_joint_state_.position[0]= mimic_current_state_.position[i]; + mimic_current_joint_state_.velocity[0]= mimic_current_state_.velocity[i]; + } + } + return true; } @@ -493,6 +545,32 @@ update(const ros::Time& time, const ros::Duration& period) // Publish state publishState(time_data.uptime); + + // Mimic Joints + // Update current state and state error for mimic_joint + for (unsigned int i = 0; i < mimic_joints_.size(); ++i) + { + mimic_current_state_.position[i] = mimic_joints_[i].getPosition(); + mimic_current_state_.velocity[i] = mimic_joints_[i].getVelocity(); + // There's no acceleration data available in a joint handle + + unsigned int mimic_joint_id = mimic_joint_ids_[i]; + mimic_desired_state_.position[i] = desired_state_.position[mimic_joint_id] * mimic_urdf_joints_[i]->mimic->multiplier + mimic_urdf_joints_[i]->mimic->offset; + mimic_desired_state_.velocity[i] = desired_state_.velocity[mimic_joint_id]; + mimic_desired_state_.acceleration[i] = desired_state_.acceleration[mimic_joint_id]; + + mimic_state_error_.position[i] = angles::shortest_angular_distance(mimic_current_state_.position[i],mimic_desired_state_.position[i]); + mimic_state_error_.velocity[i] = mimic_desired_state_.velocity[i] - mimic_current_state_.velocity[i]; + mimic_state_error_.acceleration[i] = 0.0; + } + + // Send commands to mimic joints_ + if (mimic_joints_.size() > 0) + { + mimic_hw_iface_adapter_.updateCommand(time_data.uptime, time_data.period, + mimic_desired_state_, mimic_state_error_); + } + } template