Added teleop grasping for Draco3 in MuJoCo

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.

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.

controller/draco_controller/draco_interface.cpp

@@ -30,9 +30,9 @@ DracoInterface::DracoInterface() : Interface() {
   std::vector<std::string> 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) {

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<std::string, double> gripper_pos_cmd_;
 };
 
 class DracoInterface : public Interface {

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;

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<std::string, double> left_gripper_target_pos_;
+  std::unordered_map<std::string, double> right_gripper_target_pos_;
+  bool b_prev_grasp_ = false;
 };

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;
 }

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<std::string, double> gripper_pos_cmd_;
+
 private:
   DracoStateProvider();
 };

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
 

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]);
   }
 }