From bce6b0e4a5ab9bac6149240e1a67657f1f40860a Mon Sep 17 00:00:00 2001 From: shbang91 Date: Mon, 19 Aug 2024 14:53:43 -0500 Subject: [PATCH] Added teleop grasping for Draco3 in MuJoCo --- .../draco/sim/mujoco/ihwbc/mujoco_params.yaml | 8 ++--- config/draco/sim/mujoco/ihwbc/pnc.yaml | 10 +++++- .../draco_controller/draco_interface.cpp | 7 ++-- .../draco_controller/draco_interface.hpp | 1 + .../teleop_manipulation.cpp | 32 +++++++++++++++++++ .../teleop_manipulation.hpp | 4 +++ .../draco_controller/draco_state_provider.cpp | 5 +++ .../draco_controller/draco_state_provider.hpp | 3 ++ scripts/real_teleop.py | 2 +- simulator/mujoco/draco/main_manipulation.cc | 22 +++++++------ 10 files changed, 76 insertions(+), 18 deletions(-) diff --git a/config/draco/sim/mujoco/ihwbc/mujoco_params.yaml b/config/draco/sim/mujoco/ihwbc/mujoco_params.yaml index 22ed9b18..0f066543 100644 --- a/config/draco/sim/mujoco/ihwbc/mujoco_params.yaml +++ b/config/draco/sim/mujoco/ihwbc/mujoco_params.yaml @@ -36,8 +36,8 @@ actuator_gains: kp: 100 kd: 5 left_ezgripper_knuckle_palm_L1_1: - kp: 500 - kd: 10 + kp: 10 + kd: 5 neck_pitch: kp: 100 kd: 5 @@ -78,8 +78,8 @@ actuator_gains: kp: 100 kd: 5 right_ezgripper_knuckle_palm_L1_1: - kp: 500 - kd: 10 + kp: 10 + kd: 5 initial_config: base_pos_x: 0. diff --git a/config/draco/sim/mujoco/ihwbc/pnc.yaml b/config/draco/sim/mujoco/ihwbc/pnc.yaml index 38a8173a..35f51907 100644 --- a/config/draco/sim/mujoco/ihwbc/pnc.yaml +++ b/config/draco/sim/mujoco/ihwbc/pnc.yaml @@ -125,7 +125,7 @@ wbc: hand_pos_task: weight : [0., 0., 0.] weight_at_teleop : [50., 50., 50.] - kp : [1000., 1000., 1000.] + kp : [2000., 2000., 2000.] kd : [100., 100., 100.] hand_ori_task: @@ -201,6 +201,14 @@ state_machine: teleop_manipulation: transition_duration: 0.3 moving_duration: 0.05 + gripper_control: + left: + grasp: 0.33 + release: -0.33 + right: + grasp: 0.33 + release: -0.33 + dcm_walking: #t_additional_init_trans: 0. diff --git a/controller/draco_controller/draco_interface.cpp b/controller/draco_controller/draco_interface.cpp index 32a136da..99d9b66b 100644 --- a/controller/draco_controller/draco_interface.cpp +++ b/controller/draco_controller/draco_interface.cpp @@ -30,9 +30,9 @@ DracoInterface::DracoInterface() : Interface() { std::vector unactuated_joint_list = {"l_knee_fe_jp", "r_knee_fe_jp"}; robot_ = new PinocchioRobotSystem( - THIS_COM "robot_model/draco/draco_latest_collisions.urdf", - // THIS_COM "robot_model/draco/draco_latest_collisions_with_gripper.urdf", + // THIS_COM "robot_model/draco/draco_latest_collisions.urdf", // /*This is for draco with grippers*/ + THIS_COM "robot_model/draco/draco_latest_collisions_with_gripper.urdf", THIS_COM "robot_model/draco", false, false, &unactuated_joint_list); // robot_ = new PinocchioRobotSystem( // THIS_COM "robot_model/draco/draco_modified.urdf", @@ -109,6 +109,9 @@ void DracoInterface::GetCommand(void *sensor_data, void *command_data) { // get control command ctrl_arch_->GetCommand(draco_command); + // get gripper command + if (sp_->b_recv_gripper_cmd_) + draco_command->gripper_pos_cmd_ = sp_->gripper_pos_cmd_; #if B_USE_ZMQ if (sp_->count_ % sp_->data_save_freq_ == 0) { diff --git a/controller/draco_controller/draco_interface.hpp b/controller/draco_controller/draco_interface.hpp index 11d98f25..4172a89d 100644 --- a/controller/draco_controller/draco_interface.hpp +++ b/controller/draco_controller/draco_interface.hpp @@ -53,6 +53,7 @@ class DracoCommand { Eigen::VectorXd joint_pos_cmd_; Eigen::VectorXd joint_vel_cmd_; Eigen::VectorXd joint_trq_cmd_; + std::unordered_map gripper_pos_cmd_; }; class DracoInterface : public Interface { diff --git a/controller/draco_controller/draco_state_machines/teleop_manipulation.cpp b/controller/draco_controller/draco_state_machines/teleop_manipulation.cpp index e35ba9d3..7b8f7172 100644 --- a/controller/draco_controller/draco_state_machines/teleop_manipulation.cpp +++ b/controller/draco_controller/draco_state_machines/teleop_manipulation.cpp @@ -120,6 +120,20 @@ void TeleopManipulation::OneStep() { } else if (teleop_handler_->IsReady() && b_teleop_mode_) { // execute the desired trajectory ctrl_arch_->rh_SE3_tm_->UpdateHandPose(sp_->current_time_); + + // gripper command + sp_->b_recv_gripper_cmd_ = false; + if (rs_commands_->b_grasp_ != b_prev_grasp_) { + sp_->b_recv_gripper_cmd_ = true; + + // update gripper target pos + if (rs_commands_->b_grasp_) + sp_->gripper_pos_cmd_["right"] = right_gripper_target_pos_["grasp"]; + else + sp_->gripper_pos_cmd_["right"] = right_gripper_target_pos_["release"]; + + b_prev_grasp_ = rs_commands_->b_grasp_; + } } } } @@ -147,6 +161,24 @@ void TeleopManipulation::SetParameters(const YAML::Node &node) { "transition_duration", transition_duration_); util::ReadParameter(node["state_machine"]["teleop_manipulation"], "moving_duration", moving_duration_); + double tmp; + util::ReadParameter( + node["state_machine"]["teleop_manipulation"]["gripper_control"]["left"], + "grasp", tmp); + left_gripper_target_pos_["grasp"] = tmp; + util::ReadParameter( + node["state_machine"]["teleop_manipulation"]["gripper_control"]["left"], + "release", tmp); + left_gripper_target_pos_["release"] = tmp; + util::ReadParameter(node["state_machine"]["teleop_manipulation"] + ["gripper_control"]["right"], + "grasp", tmp); + right_gripper_target_pos_["grasp"] = tmp; + util::ReadParameter(node["state_machine"]["teleop_manipulation"] + ["gripper_control"]["right"], + "release", tmp); + right_gripper_target_pos_["release"] = tmp; + } catch (const std::runtime_error &ex) { std::cerr << "Error Reading Parameter [" << ex.what() << "] at file: [" << __FILE__ << "]" << std::endl; diff --git a/controller/draco_controller/draco_state_machines/teleop_manipulation.hpp b/controller/draco_controller/draco_state_machines/teleop_manipulation.hpp index a4ff58ae..8c672c37 100644 --- a/controller/draco_controller/draco_state_machines/teleop_manipulation.hpp +++ b/controller/draco_controller/draco_state_machines/teleop_manipulation.hpp @@ -43,4 +43,8 @@ class TeleopManipulation : public StateMachine { double moving_duration_; double transition_duration_; + + std::unordered_map left_gripper_target_pos_; + std::unordered_map right_gripper_target_pos_; + bool b_prev_grasp_ = false; }; diff --git a/controller/draco_controller/draco_state_provider.cpp b/controller/draco_controller/draco_state_provider.cpp index 906bb589..f8bc7d15 100644 --- a/controller/draco_controller/draco_state_provider.cpp +++ b/controller/draco_controller/draco_state_provider.cpp @@ -52,4 +52,9 @@ DracoStateProvider::DracoStateProvider() { wbo_ypr_ = Eigen::Vector3d::Zero(); wbo_ang_vel_ = Eigen::Vector3d::Zero(); wbo_des_ = Eigen::VectorXd::Zero(4); + + // gripper + b_recv_gripper_cmd_ = false; + gripper_pos_cmd_["left"] = 0.0; + gripper_pos_cmd_["right"] = 0.0; } diff --git a/controller/draco_controller/draco_state_provider.hpp b/controller/draco_controller/draco_state_provider.hpp index 577257ef..6ba1625a 100644 --- a/controller/draco_controller/draco_state_provider.hpp +++ b/controller/draco_controller/draco_state_provider.hpp @@ -53,6 +53,9 @@ class DracoStateProvider { Eigen::Vector3d wbo_ang_vel_; Eigen::VectorXd wbo_des_; + bool b_recv_gripper_cmd_; + std::unordered_map gripper_pos_cmd_; + private: DracoStateProvider(); }; diff --git a/scripts/real_teleop.py b/scripts/real_teleop.py index 50f7899c..efb53d0e 100644 --- a/scripts/real_teleop.py +++ b/scripts/real_teleop.py @@ -99,7 +99,7 @@ def _on_release(self, key): self._reset_state = 1 self._enabled = True - elif key_char == 'c': + elif key_char == 'p': self._reset_state = 1 self._grasp = True diff --git a/simulator/mujoco/draco/main_manipulation.cc b/simulator/mujoco/draco/main_manipulation.cc index 66ed2cab..d3469572 100644 --- a/simulator/mujoco/draco/main_manipulation.cc +++ b/simulator/mujoco/draco/main_manipulation.cc @@ -608,16 +608,18 @@ void CopyCommand() { // int pin_act_idx = pin_act_map_.at(mj_act_name); int mj_gripper_qpos_idx = mj_gripper_qpos_map_.at(mj_act_name); int mj_gripper_qvel_idx = mj_gripper_qvel_map_.at(mj_act_name); - - // d->ctrl[mj_act_idx] = - // draco_command->joint_trq_cmd_[pin_act_idx] + - // kp_[mj_act_idx] * (draco_command->joint_pos_cmd_[pin_act_idx] - - // d->qpos[mj_qpos_idx]) + - // kd_[mj_act_idx] * - //(draco_command->joint_vel_cmd_[pin_act_idx] - d->qvel[mj_qvel_idx]); - d->ctrl[mj_act_idx] = - 0. + kp_[mj_act_idx] * (0. - d->qpos[mj_gripper_qpos_idx]) + - kd_[mj_act_idx] * (0. - d->qvel[mj_gripper_qvel_idx]); + if (mj_act_name == "left_ezgripper_knuckle_palm_L1_1") + d->ctrl[mj_act_idx] = + 0. + + kp_[mj_act_idx] * (draco_command->gripper_pos_cmd_["left"] - + d->qpos[mj_gripper_qpos_idx]) + + kd_[mj_act_idx] * (-d->qvel[mj_gripper_qvel_idx]); + else if (mj_act_name == "right_ezgripper_knuckle_palm_L1_1") + d->ctrl[mj_act_idx] = + 0. + + kp_[mj_act_idx] * (draco_command->gripper_pos_cmd_["right"] - + d->qpos[mj_gripper_qpos_idx]) + + kd_[mj_act_idx] * (-d->qvel[mj_gripper_qvel_idx]); } }