feat: add grpc debug log

cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h

@@ -8,6 +8,7 @@
 #include <functional>
 #include <memory>
 #include <mutex>
+#include <optional>
 #include <thread>
 #include <vector>
 
@@ -42,7 +43,7 @@ public:
                   double acceleration,
                   double duration,
                   FrameType frame);
-    Result stop(double acceleration);
+    Result stop(std::optional<double> acceleration = std::nullopt);
     void shutdown();
 
     bool busy() const { return busy_.load(); }

cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp

@@ -95,9 +95,8 @@ Result CartesianVelocityController::speedL(const CartesianVelocity& velocity,
     return Result::success();
 }
 
-Result CartesianVelocityController::stop(const double acceleration)
+Result CartesianVelocityController::stop(const std::optional<double> acceleration)
 {
-    (void)acceleration;
     if (!worker_ || !worker_->joinable()) {
         return Result::success();
     }
@@ -105,6 +104,9 @@ Result CartesianVelocityController::stop(const double acceleration)
         std::lock_guard<std::mutex> lock(mutex_);
         target_twist_ = {};
         target_frame_ = FrameType::Base;
+        if (acceleration.has_value()) {
+            target_acceleration_ = *acceleration;
+        }
         command_active_ = true;
         ++command_version_;
     }

cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_dls_ik_solver.cpp

@@ -101,16 +101,16 @@ bool PinocchioDlsIKSolver::refreshJointLimits_(const config::PinocchioDlsIKConfi
             return false;
         }
         const auto& limit = it->second;
-        if (!std::isfinite(limit.lower()) || !std::isfinite(limit.upper()) ||
+        if (!std::isfinite(limit.q_lb()) || !std::isfinite(limit.q_ub()) ||
-            !std::isfinite(limit.velocity()) || limit.upper() <= limit.lower() ||
+            !std::isfinite(limit.qd()) || limit.q_ub() <= limit.q_lb() ||
-            limit.velocity() <= 0.0) {
+            limit.qd() <= 0.0) {
             CMVR_LOG(ERROR) << "[PinocchioDlsIKSolver] invalid custom joint limit for "
                             << limit.joint_name();
             return false;
         }
-        joint_pos_lower_limits_[i] = limit.lower();
+        joint_pos_lower_limits_[i] = limit.q_lb();
-        joint_pos_upper_limits_[i] = limit.upper();
+        joint_pos_upper_limits_[i] = limit.q_ub();
-        joint_vel_limits_[i] = std::abs(limit.velocity());
+        joint_vel_limits_[i] = std::abs(limit.qd());
     }
     return true;
 }

cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio_qp/src/pinocchio_qp_cartesian_motion_planner.cpp

@@ -94,16 +94,16 @@ bool PinocchioQpCartesianMotionPlanner::refreshJointLimits_(
                 return false;
             }
             const auto& limit = it->second;
-            if (!std::isfinite(limit.lower()) || !std::isfinite(limit.upper()) ||
+            if (!std::isfinite(limit.q_lb()) || !std::isfinite(limit.q_ub()) ||
-                !std::isfinite(limit.velocity()) || limit.upper() <= limit.lower() ||
+                !std::isfinite(limit.qd()) || limit.q_ub() <= limit.q_lb() ||
-                limit.velocity() <= 0.0) {
+                limit.qd() <= 0.0) {
                 CMVR_LOG(ERROR) << "[PinocchioQpCartesianMotionPlanner] invalid custom joint limit for "
                                 << limit.joint_name();
                 return false;
             }
-            joint_lower_limits_[i] = limit.lower();
+            joint_lower_limits_[i] = limit.q_lb();
-            joint_upper_limits_[i] = limit.upper();
+            joint_upper_limits_[i] = limit.q_ub();
-            joint_velocity_limits_[i] = std::abs(limit.velocity());
+            joint_velocity_limits_[i] = std::abs(limit.qd());
         }
         return true;
     }

cmvr-es/config/devices/camera/camera.pb.txt

@@ -63,16 +63,19 @@ camera {
     }
   }
 
+
     cameras {
       id: "cam5"
       realsense {
-      serialNumber: "243122075389"
+        serialNumber: "243122070435"
-      width: 640
+        width: 1280
-      height: 480
+        height: 720
+        encode_width: 640
+        encode_height: 360
         fps: 30
-      codec: "H265"
+        codec: "H264"
         camera_mode: CAMERA_MODE_VIDEO
-      stream_mode: STREAM_MODE_RGBD
+        stream_mode: STREAM_MODE_RGB
         align_mode: ALIGN_MODE_COLOR
         buffer_size: 30
         sync: false

cmvr-es/config/devices/motor/ti5_motors.pb.txt

@@ -10,8 +10,8 @@ motor {
     }
     joint_limits {
       source: JOINT_LIMIT_SOURCE_URDF
+      urdf_path: "model/xiaoyan_description/dual_arm.urdf"
     }
-    joint_limits_urdf_path: "model/xiaoyan_description/dual_arm.urdf"
     motors { id: 23 joint_name: "L_SHOULDER_P" }
     motors { id: 24 joint_name: "L_SHOULDER_R" }
     motors { id: 25 joint_name: "L_SHOULDER_Y" }
@@ -30,13 +30,13 @@ motor {
     }
     joint_limits {
       source: JOINT_LIMIT_SOURCE_CUSTOM
-      joints { joint_name: "R_SHOULDER_P" lower: -3.14 upper: 3.14 velocity: 5.0 }
+      joints { joint_name: "R_SHOULDER_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
-      joints { joint_name: "R_SHOULDER_R" lower: -0.78 upper: 1.57 velocity: 5.0 }
+      joints { joint_name: "R_SHOULDER_R" q_lb: -0.78 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
-      joints { joint_name: "R_SHOULDER_Y" lower: -3.14 upper: 3.14 velocity: 5.0 }
+      joints { joint_name: "R_SHOULDER_Y" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
-      joints { joint_name: "R_ELBOW_R" lower: 0 upper: 2.05 velocity: 5.0 }
+      joints { joint_name: "R_ELBOW_R" q_lb: 0 q_ub: 2.05 qd: 5.0 qdd: 10.0 }
-      joints { joint_name: "R_WRIST_P" lower: -3.14 upper: 3.14 velocity: 5.0 }
+      joints { joint_name: "R_WRIST_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
-      joints { joint_name: "R_WRIST_Y" lower: -0.78 upper: 0.78 velocity: 5.0 }
+      joints { joint_name: "R_WRIST_Y" q_lb: -0.78 q_ub: 0.78 qd: 5.0 qdd: 10.0 }
-      joints { joint_name: "R_WRIST_R" lower: -0.57 upper: 1.57 velocity: 5.0 }
+      joints { joint_name: "R_WRIST_R" q_lb: -0.57 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
     }
     motors { id: 16 joint_name: "R_SHOULDER_P" }
     motors { id: 17 joint_name: "R_SHOULDER_R" }
@@ -55,9 +55,9 @@ motor {
     }
     joint_limits {
       source: JOINT_LIMIT_SOURCE_CUSTOM
-      joints { joint_name: "HEAD_Y" lower: -3.14 upper: 3.14 velocity: 3.0 }
+      joints { joint_name: "HEAD_Y" q_lb: -3.14 q_ub: 3.14 qd: 3.0 }
-      joints { joint_name: "HEAD_P" lower: -3.14 upper: 3.14 velocity: 3.0 }
+      joints { joint_name: "HEAD_P" q_lb: -3.14 q_ub: 3.14 qd: 3.0 }
-      joints { joint_name: "HEAD_R" lower: -3.14 upper: 3.14 velocity: 3.0 }
+      joints { joint_name: "HEAD_R" q_lb: -3.14 q_ub: 3.14 qd: 3.0 }
     }
     motors { id: 32 joint_name: "HEAD_Y" }
     motors { id: 30 joint_name: "HEAD_P" }
@@ -72,8 +72,8 @@ motor {
     }
     joint_limits {
       source: JOINT_LIMIT_SOURCE_CUSTOM
-      joints { joint_name: "WAIST_Y" lower: -3.14 upper: 3.14 velocity: 3.0 }
+      joints { joint_name: "WAIST_Y" q_lb: -3.14 q_ub: 3.14 qd: 3.0 }
-      joints { joint_name: "WAIST_P" lower: -3.14 upper: 3.14 velocity: 3.0 }
+      joints { joint_name: "WAIST_P" q_lb: -3.14 q_ub: 3.14 qd: 3.0 }
     }
     motors { id: 4 joint_name: "WAIST_Y" }
     motors { id: 15 joint_name: "WAIST_P" }

cmvr-es/config/logger/logger.pb.txt

@@ -3,7 +3,7 @@ logger {
 
   routes {
     level: LOG_LEVEL_DEBUG
-    file: true
+    file: false
     terminal: true
   }
   routes {
@@ -13,17 +13,17 @@ logger {
   routes {
     level: LOG_LEVEL_WARNING
     terminal: true
-    file: true
+    file: false
   }
   routes {
     level: LOG_LEVEL_ERROR
     terminal: true
-    file: true
+    file: false
   }
   routes {
     level: LOG_LEVEL_FATAL
     terminal: true
-    file: true
+    file: false
   }
 
   directory: "../log"

cmvr-es/config/manager/device_manager.pb.txt

@@ -7,14 +7,22 @@ device_manager {
     id: "right_hand_cam"
     type: DEVICE_TYPE_CAMERA
     config_file: "devices/camera/camera.pb.txt"
+    enable: false
+  }
+
+    devices {
+      id: "cam5"
+      type: DEVICE_TYPE_CAMERA
+      config_file: "devices/camera/camera.pb.txt"
       enable: true
     }
 
+
   devices {
     id: "hand2"
     type: DEVICE_TYPE_DEXHAND
     config_file: "devices/dexhand/dexhand.pb.txt"
-    enable: true
+    enable: false
   }
 
   devices {
@@ -28,14 +36,14 @@ device_manager {
     id: "ti5_motors"
     type: DEVICE_TYPE_MOTOR_SYSTEM
     config_file: "devices/motor/ti5_motors.pb.txt"
-    enable: true
+    enable: false
   }
 
   devices {
     id: "right_arm"
     type: DEVICE_TYPE_ROBOT_ARM
     config_file: "devices/arm/arm.pb.txt"
-    enable: true
+    enable: false
   }
 
   devices {

cmvr-es/config/manager/task_manager.pb.txt

@@ -5,7 +5,7 @@ task_manager {
     run_mode: TASK_RUN_MODE_PERIODIC_STEP
     control_period_s: 0.001
     config_file: "tasks/touch_screen_task/touch_screen_task.pb.txt"
-    enable: true
+    enable: false
   }
   tasks {
     id: "grpc_server"

cmvr-es/devices/arm/aubo_arm/include/aubo_arm.h

@@ -4,6 +4,7 @@
 #include <atomic>
 #include <memory>
 #include <mutex>
+#include <optional>
 #include <string>
 #include <vector>
 
@@ -46,7 +47,7 @@ public:
     Result stopJ(double acceleration) override;
     Result moveL(const CartesianPose& target, const MotionOptions& options, FrameType frame = FrameType::Base) override;
     Result speedL(const CartesianVelocity& velocity, double acceleration, double duration, FrameType frame = FrameType::Base) override;
-    Result stopL(double acceleration) override;
+    Result stopL(std::optional<double> acceleration = std::nullopt) override;
     Result stopMotion() override;
 
     Result startServoMode(const ServoOptions& options) override;

cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp

@@ -366,7 +366,7 @@ Result AuboArm::speedL(const CartesianVelocity& velocity, double acceleration, d
     return unsupported_("speedL");
 }
 
-Result AuboArm::stopL(double acceleration)
+Result AuboArm::stopL(std::optional<double> acceleration)
 {
     (void)acceleration;
     return stopMotion();

cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h

@@ -4,6 +4,7 @@
 #include <atomic>
 #include <memory>
 #include <mutex>
+#include <optional>
 #include <string>
 #include <unordered_set>
 #include <vector>
@@ -57,7 +58,7 @@ public:
                   double acceleration,
                   double duration,
                   FrameType frame = FrameType::Base) override;
-    Result stopL(double acceleration) override;
+    Result stopL(std::optional<double> acceleration = std::nullopt) override;
     Result stopMotion() override;
 
     Result startServoMode(const ServoOptions& options) override;

cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp

@@ -421,7 +421,7 @@ Result MotorRobotArm::speedL(const CartesianVelocity& velocity,
     return cartesian_velocity_controller_->speedL(velocity, acceleration, duration, frame);
 }
 
-Result MotorRobotArm::stopL(const double acceleration)
+Result MotorRobotArm::stopL(const std::optional<double> acceleration)
 {
     if (!cartesian_velocity_controller_) {
         return Result::success();

cmvr-es/devices/arm/robot_arm.h

@@ -3,6 +3,7 @@
 
 #include <cstddef>
 #include <memory>
+#include <optional>
 #include <string>
 #include <vector>
 
@@ -53,7 +54,7 @@ public:
                           double acceleration,
                           double duration,
                           FrameType frame = FrameType::Base) = 0;
-    virtual Result stopL(double acceleration) = 0;
+    virtual Result stopL(std::optional<double> acceleration = std::nullopt) = 0;
     virtual Result stopMotion() = 0;
 
     virtual Result moveP(const CartesianPose& target,

cmvr-es/devices/motor/abstract_motor.h

@@ -27,6 +27,7 @@ namespace cmvr::device{
             double limit_q_lb;
             double limit_q_ub;
             double limit_qd;
+            double limit_qdd;
         };
 
         typedef struct {

cmvr-es/devices/motor/motor_system/src/motor_system.cpp

@@ -253,9 +253,9 @@ bool MotorSystem::applyConfiguredJointLimits_(const config::MotorGroupConfig& gr
         }
     } else if (source == config::JOINT_LIMIT_SOURCE_URDF) {
         const auto urdf_path =
-            cmvr::ConfigHelper::resolveResourceFile(group_cfg.joint_limits_urdf_path());
+            cmvr::ConfigHelper::resolveResourceFile(limits_cfg.urdf_path());
         if (urdf_path.empty()) {
-            CMVR_LOG(ERROR) << "[MotorSystem] joint_limits_urdf_path is empty for motor group: "
+            CMVR_LOG(ERROR) << "[MotorSystem] joint_limits.urdf_path is empty for motor group: "
                             << group_cfg.id();
             return false;
         }
@@ -273,9 +273,9 @@ bool MotorSystem::applyConfiguredJointLimits_(const config::MotorGroupConfig& gr
             }
             config::JointLimitConfig limit;
             limit.set_joint_name(model.names[joint_id]);
-            limit.set_lower(model.lowerPositionLimit[model.idx_qs[joint_id]]);
+            limit.set_q_lb(model.lowerPositionLimit[model.idx_qs[joint_id]]);
-            limit.set_upper(model.upperPositionLimit[model.idx_qs[joint_id]]);
+            limit.set_q_ub(model.upperPositionLimit[model.idx_qs[joint_id]]);
-            limit.set_velocity(model.velocityLimit[model.idx_vs[joint_id]]);
+            limit.set_qd(model.velocityLimit[model.idx_vs[joint_id]]);
             limits_by_joint[limit.joint_name()] = limit;
         }
     } else {
@@ -291,15 +291,19 @@ bool MotorSystem::applyConfiguredJointLimits_(const config::MotorGroupConfig& gr
             return false;
         }
         const auto& limit = it->second;
-        if (!std::isfinite(limit.lower()) || !std::isfinite(limit.upper()) ||
+        if (!std::isfinite(limit.q_lb()) || !std::isfinite(limit.q_ub()) ||
-            !std::isfinite(limit.velocity()) || limit.upper() <= limit.lower() ||
+            !std::isfinite(limit.qd()) || limit.q_ub() <= limit.q_lb() ||
-            limit.velocity() <= 0.0) {
+            limit.qd() <= 0.0 || !std::isfinite(limit.qdd()) ||
+            limit.qdd() < 0.0) {
             CMVR_LOG(ERROR) << "[MotorSystem] invalid joint limit for " << motor_cfg.joint_name();
             return false;
         }
-        motor_cfg.set_limit_q_lb(limit.lower());
+        motor_cfg.set_limit_q_lb(limit.q_lb());
-        motor_cfg.set_limit_q_ub(limit.upper());
+        motor_cfg.set_limit_q_ub(limit.q_ub());
-        motor_cfg.set_limit_qd(std::abs(limit.velocity()));
+        motor_cfg.set_limit_qd(std::abs(limit.qd()));
+        if (limit.qdd() > 0.0) {
+            motor_cfg.set_limit_qdd(std::abs(limit.qdd()));
+        }
     }
     return true;
 }

cmvr-es/devices/motor/ti5_motor/ti5_motor.h

@@ -21,6 +21,7 @@ namespace  cmvr {
                 info_.limit_q_lb = config.limit_q_lb();
                 info_.limit_q_ub = config.limit_q_ub();
                 info_.limit_qd = config.limit_qd() > 0.0 ? config.limit_qd() : 0.5;
+                info_.limit_qdd = config.limit_qdd() > 0.0 ? config.limit_qdd() : 10.0;
                 node_id_ = info_.id;
             }
 
@@ -48,7 +49,7 @@ namespace  cmvr {
                     // canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x0F,15);
                     canopen_protocol->setLimitQ(node_id_, info_.limit_q_ub, info_.limit_q_lb);
                     canopen_protocol->setLimitQd(node_id_, info_.limit_qd);
-                    canopen_protocol->setLimitQdd(node_id_,10.0,-10.0);
+                    canopen_protocol->setLimitQdd(node_id_, info_.limit_qdd, -info_.limit_qdd);
 
                     // canopen_protocol->torqueOff(node_id_);
                 }

cmvr-es/service/grpc/src/grpc_arm_service.cpp

@@ -2,6 +2,8 @@
 
 #include <google/protobuf/util/time_util.h>
 
+#include "common/base/logging/logger.h"
+
 using google::protobuf::util::TimeUtil;
 
 namespace cmvr::service {
@@ -25,6 +27,12 @@ grpc::Status resultToStatus(const device::Result& result)
     return grpc::Status(grpc::StatusCode::INTERNAL, result.message);
 }
 
+void logRpcSuccess(const char* rpc_name, const std::string& device_id)
+{
+    CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (" << rpc_name
+                    << "): success, id=" << device_id;
+}
+
 device::FrameType toFrameType(const api::ArmFrameType frame)
 {
     switch (frame) {
@@ -130,6 +138,9 @@ grpc::Status gRPCArmServiceImpl::torqueOff(grpc::ServerContext*,
         }
         const auto result = arm->torqueOff();
         fillFeedback(response, result.ok(), result.ok() ? "" : result.message);
+        if (result.ok()) {
+            logRpcSuccess("torqueOff", device_id);
+        }
         return resultToStatus(result);
     } catch (const std::exception& e) {
         fillFeedback(response, false, e.what());
@@ -149,6 +160,9 @@ grpc::Status gRPCArmServiceImpl::torqueOn(grpc::ServerContext*,
         }
         const auto result = arm->torqueOn();
         fillFeedback(response, result.ok(), result.ok() ? "" : result.message);
+        if (result.ok()) {
+            logRpcSuccess("torqueOn", device_id);
+        }
         return resultToStatus(result);
     } catch (const std::exception& e) {
         fillFeedback(response, false, e.what());
@@ -166,8 +180,13 @@ grpc::Status gRPCArmServiceImpl::moveJ(grpc::ServerContext*,
         if (!arm) {
             return setDeviceNotFound(response, device_id);
         }
-        return setResponseResult(response, arm->moveJ(toJointPositionCommand(request->target()),
+        const auto result = arm->moveJ(toJointPositionCommand(request->target()),
-                                                      toMotionOptions(request->options())));
+                                       toMotionOptions(request->options()));
+        if (result.ok()) {
+            CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (moveJ): success, id=" << device_id
+                            << ", positions=" << request->target().position_size();
+        }
+        return setResponseResult(response, result);
     } catch (const std::exception& e) {
         fillFeedback(response->mutable_header(), false, e.what());
         return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
@@ -184,9 +203,14 @@ grpc::Status gRPCArmServiceImpl::moveL(grpc::ServerContext*,
         if (!arm) {
             return setDeviceNotFound(response, device_id);
         }
-        return setResponseResult(response, arm->moveL(toCartesianPose(request->target()),
+        const auto result = arm->moveL(toCartesianPose(request->target()),
                                        toMotionOptions(request->options()),
-                                                      toFrameType(request->frame())));
+                                       toFrameType(request->frame()));
+        if (result.ok()) {
+            CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (moveL): success, id=" << device_id
+                            << ", frame=" << request->frame();
+        }
+        return setResponseResult(response, result);
     } catch (const std::exception& e) {
         fillFeedback(response->mutable_header(), false, e.what());
         return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
@@ -203,9 +227,16 @@ grpc::Status gRPCArmServiceImpl::speedJ(grpc::ServerContext*,
         if (!arm) {
             return setDeviceNotFound(response, device_id);
         }
-        return setResponseResult(response, arm->speedJ(toJointVelocityCommand(request->velocity()),
+        const auto result = arm->speedJ(toJointVelocityCommand(request->velocity()),
                                         request->acceleration(),
-                                                       request->duration()));
+                                        request->duration());
+        if (result.ok()) {
+            CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (speedJ): success, id=" << device_id
+                            << ", velocities=" << request->velocity().velocity_size()
+                            << ", acceleration=" << request->acceleration()
+                            << ", duration=" << request->duration();
+        }
+        return setResponseResult(response, result);
     } catch (const std::exception& e) {
         fillFeedback(response->mutable_header(), false, e.what());
         return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
@@ -222,10 +253,17 @@ grpc::Status gRPCArmServiceImpl::speedL(grpc::ServerContext*,
         if (!arm) {
             return setDeviceNotFound(response, device_id);
         }
-        return setResponseResult(response, arm->speedL(toCartesianVelocity(request->velocity()),
+        const auto result = arm->speedL(toCartesianVelocity(request->velocity()),
                                         request->acceleration(),
                                         request->duration(),
-                                                       toFrameType(request->frame())));
+                                        toFrameType(request->frame()));
+        if (result.ok()) {
+            CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (speedL): success, id=" << device_id
+                            << ", acceleration=" << request->acceleration()
+                            << ", duration=" << request->duration()
+                            << ", frame=" << request->frame();
+        }
+        return setResponseResult(response, result);
     } catch (const std::exception& e) {
         fillFeedback(response->mutable_header(), false, e.what());
         return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
@@ -242,7 +280,12 @@ grpc::Status gRPCArmServiceImpl::servoJ(grpc::ServerContext*,
         if (!arm) {
             return setDeviceNotFound(response, device_id);
         }
-        return setResponseResult(response, arm->servoJ(toJointPositionCommand(request->target())));
+        const auto result = arm->servoJ(toJointPositionCommand(request->target()));
+        if (result.ok()) {
+            CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (servoJ): success, id=" << device_id
+                            << ", positions=" << request->target().position_size();
+        }
+        return setResponseResult(response, result);
     } catch (const std::exception& e) {
         fillFeedback(response->mutable_header(), false, e.what());
         return grpc::Status(grpc::StatusCode::INTERNAL, e.what());
@@ -261,6 +304,9 @@ grpc::Status gRPCArmServiceImpl::stopMotion(grpc::ServerContext*,
         }
         const auto result = arm->stopMotion();
         fillFeedback(response, result.ok(), result.ok() ? "" : result.message);
+        if (result.ok()) {
+            logRpcSuccess("stopMotion", device_id);
+        }
         return resultToStatus(result);
     } catch (const std::exception& e) {
         fillFeedback(response, false, e.what());
@@ -288,6 +334,9 @@ grpc::Status gRPCArmServiceImpl::getJointState(grpc::ServerContext*,
         for (double v : state.velocity) msg->add_velocity(v);
         for (double v : state.effort) msg->add_effort(v);
         fillFeedback(response->mutable_header(), true);
+        CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (getJointState): success, id=" << device_id
+                        << ", joints=" << msg->name_size()
+                        << ", positions=" << msg->position_size();
         return grpc::Status::OK;
     } catch (const std::exception& e) {
         fillFeedback(response->mutable_header(), false, e.what());
@@ -310,6 +359,9 @@ grpc::Status gRPCArmServiceImpl::getPose(grpc::ServerContext*,
             : arm->fk(request->base_link(), request->ee_link());
         *response->mutable_pose() = toApiCartesianPose(pose);
         fillFeedback(response->mutable_header(), true);
+        CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (getPose): success, id=" << device_id
+                        << ", pose=(" << pose.x << ", " << pose.y << ", " << pose.z
+                        << ", " << pose.rx << ", " << pose.ry << ", " << pose.rz << ")";
         return grpc::Status::OK;
     } catch (const std::exception& e) {
         fillFeedback(response->mutable_header(), false, e.what());
@@ -327,7 +379,12 @@ grpc::Status gRPCArmServiceImpl::calibrateZeroQ(grpc::ServerContext*,
         if (!arm) {
             return setDeviceNotFound(response, device_id);
         }
-        return setResponseResult(response, arm->calibrateZeroQ(request->joint_name()));
+        const auto result = arm->calibrateZeroQ(request->joint_name());
+        if (result.ok()) {
+            CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (calibrateZeroQ): success, id=" << device_id
+                            << ", joint=" << request->joint_name();
+        }
+        return setResponseResult(response, result);
     } catch (const std::exception& e) {
         fillFeedback(response->mutable_header(), false, e.what());
         return grpc::Status(grpc::StatusCode::INTERNAL, e.what());

cmvr-es/service/grpc/src/grpc_camera_service.cpp

@@ -47,6 +47,14 @@ grpc::Status gRPCCameraServiceImpl::GetStatus(grpc::ServerContext* context,
         response->mutable_state()->set_fps(state.fps);
         response->mutable_state()->set_width(state.width);
         response->mutable_state()->set_height(state.height);
+        CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (GetStatus): success, id=" << dev_id
+                        << ", initialized=" << state.is_initialized
+                        << ", opened=" << state.is_opened
+                        << ", streaming=" << state.is_streaming
+                        << ", recording=" << state.is_recording
+                        << ", error=" << state.is_error
+                        << ", size=" << state.width << "x" << state.height
+                        << ", fps=" << state.fps;
         return grpc::Status::OK;
     }
     catch(const exception &e) {
@@ -72,6 +80,7 @@ grpc::Status gRPCCameraServiceImpl::StartCamera(grpc::ServerContext* context,
         }
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (StartCamera): success, id=" << dev_id;
         return grpc::Status::OK;
     }
     catch (const exception &e) {
@@ -97,6 +106,7 @@ grpc::Status gRPCCameraServiceImpl::StopCamera(grpc::ServerContext* context,
         }
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (StopCamera): success, id=" << dev_id;
         return grpc::Status::OK;
     }
     catch (exception &e) {
@@ -150,6 +160,10 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImage(grpc::ServerContext* context,
         response->mutable_color_frame()->set_height(image.rows);
         response->mutable_color_frame()->set_width(image.cols);
         response->mutable_color_frame()->set_codec("none");
+        CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (GetRGBImage): success, id=" << dev_id
+                        << ", size=" << image.cols << "x" << image.rows
+                        << ", cv_type=" << image.type()
+                        << ", bytes=" << image.total() * image.elemSize();
         return grpc::Status::OK;
     }
     catch (exception &e) {
@@ -206,6 +220,10 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImage(grpc::ServerContext* context,
         response->mutable_depth_frame()->set_height(image.rows);
         response->mutable_depth_frame()->set_width(image.cols);
         response->mutable_depth_frame()->set_codec("none");
+        CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (GetDepthImage): success, id=" << dev_id
+                        << ", size=" << image.cols << "x" << image.rows
+                        << ", cv_type=" << image.type()
+                        << ", bytes=" << image.total() * image.elemSize();
         return grpc::Status::OK;
     }
     catch (exception &e) {
@@ -275,6 +293,11 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImages(grpc::ServerContext* context,
         response->mutable_depth_frame()->set_height(depth_image.rows);
         response->mutable_depth_frame()->set_width(depth_image.cols);
         response->mutable_depth_frame()->set_codec("none");
+        CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (GetRGBDImages): success, id=" << dev_id
+                        << ", color_size=" << color_image.cols << "x" << color_image.rows
+                        << ", color_type=" << color_image.type()
+                        << ", depth_size=" << depth_image.cols << "x" << depth_image.rows
+                        << ", depth_type=" << depth_image.type();
         return grpc::Status::OK;
     }
     catch (exception &e) {
@@ -298,6 +321,8 @@ grpc::Status gRPCCameraServiceImpl::StartRecording(grpc::ServerContext* context,
         dev->startRecording(request->video_path());
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (StartRecording): success, id=" << dev_id
+                        << ", path=" << request->video_path();
         return grpc::Status::OK;
     }
     catch (exception &e) {
@@ -321,6 +346,7 @@ grpc::Status gRPCCameraServiceImpl::StopRecording(grpc::ServerContext* context,
         dev->stopRecording();
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (StopRecording): success, id=" << dev_id;
         return grpc::Status::OK;
     }
     catch (exception &e) {
@@ -351,6 +377,7 @@ grpc::Status gRPCCameraServiceImpl::GetDepthImageStream(grpc::ServerContext* con
         }
         int nFrameCount = 0;
         dev->startStreaming();
+        CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (GetDepthImageStream): start streaming success, id=" << dev_id;
         size_t index = 0;
         while (true)
         {
@@ -420,6 +447,7 @@ grpc::Status gRPCCameraServiceImpl::GetRGBDImagesStream(grpc::ServerContext* con
         }
         int nFrameCount = 0;
         dev->startStreaming();
+        CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (GetRGBDImagesStream): start streaming success, id=" << dev_id;
         size_t index = 0;
         while (true)
         {
@@ -494,6 +522,7 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImageStream(grpc::ServerContext* conte
         }
         int nFrameCount = 0;
         dev->startStreaming();
+        CMVR_LOG(DEBUG) << "[gRPCCameraServiceImpl] (GetRGBImageStream): start streaming success, id=" << dev_id;
         size_t last_sent_index = std::numeric_limits<size_t>::max();
         while (true)
         {

cmvr-es/service/grpc/src/grpc_dexhand_service.cpp

@@ -211,6 +211,9 @@ grpc::Status gRPCDexHandServiceImpl::GetStatus(grpc::ServerContext* context,
         }
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (GetStatus): success, id=" << dev_id
+                        << ", initialized=" << state.is_initialized
+                        << ", dof=" << response->state().hands_size();
         return grpc::Status::OK;
     }
     catch (const std::exception& e) {
@@ -246,6 +249,8 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandPos(grpc::ServerContext* context
         dev->setPositions(finger_joint_targets);
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (SetDexHandPos): success, id=" << dev_id
+                        << ", values=" << request->values_size();
         return grpc::Status::OK;
     }
     catch (const std::exception& e) {
@@ -285,6 +290,8 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandAngle(grpc::ServerContext* contex
 
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (SetDexHandAngle): success, id=" << dev_id
+                        << ", values=" << request->values_size();
         return grpc::Status::OK;
     }
     catch (const std::exception& e) {
@@ -320,6 +327,8 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandForce(grpc::ServerContext* contex
         dev->setForce(finger_joint_targets);
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (SetDexHandForce): success, id=" << dev_id
+                        << ", values=" << request->values_size();
         return grpc::Status::OK;
     }
     catch (const std::exception& e) {
@@ -355,6 +364,8 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandSpeed(grpc::ServerContext* contex
         dev->setVelocities(finger_joint_targets);
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (SetDexHandSpeed): success, id=" << dev_id
+                        << ", values=" << request->values_size();
         return grpc::Status::OK;
     }
     catch (const std::exception& e) {
@@ -386,6 +397,8 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandPresetAct(grpc::ServerContext* co
         dev->setPresetAct(presetActId);
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (SetDexHandPresetAct): success, id=" << dev_id
+                        << ", preset_act_id=" << presetActId;
         return grpc::Status::OK;
     }
     catch (const std::exception& e) {
@@ -411,6 +424,8 @@ grpc::Status gRPCDexHandServiceImpl::GetSensorData(grpc::ServerContext* context
         appendSensorData(dev->getSensorData(), response);
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (GetSensorData): success, id=" << dev_id
+                        << ", sensors=" << response->sensor_size();
         return grpc::Status::OK;
     }
     catch (const std::exception& e) {
@@ -442,6 +457,7 @@ grpc::Status gRPCDexHandServiceImpl::GetSensorDataStream(grpc::ServerContext* co
             return grpc::Status::OK;
         }
         maybeConfigureRh56FullTactilePolling(dev);
+        CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (GetSensorDataStream): streaming success, id=" << dev_id;
 
         while (!context->IsCancelled())
         {
@@ -457,6 +473,7 @@ grpc::Status gRPCDexHandServiceImpl::GetSensorDataStream(grpc::ServerContext* co
 
             std::this_thread::sleep_for(std::chrono::milliseconds(33));
         }
+        CMVR_LOG(DEBUG) << "[gRPCDexHandServiceImpl] (GetSensorDataStream): finished, id=" << dev_id;
         return grpc::Status::OK;
     }
     catch (const std::exception& e) {

cmvr-es/service/grpc/src/grpc_head_service.cpp

@@ -23,6 +23,11 @@ grpc::Status failResponse(ResponseT* response, const std::string& message) {
     setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
     return grpc::Status::OK;
 }
+
+void logSuccess(const char* rpc_name, const std::string& device_id) {
+    CMVR_LOG(DEBUG) << "[gRPCMBioHeadServiceImpl] (" << rpc_name
+                    << "): success, id=" << device_id;
+}
 }
 
 gRPCMBioHeadServiceImpl::gRPCMBioHeadServiceImpl()
@@ -69,6 +74,7 @@ grpc::Status gRPCMBioHeadServiceImpl::SetExpression(
 
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        logSuccess("SetExpression", dev_id);
         return grpc::Status::OK;
     } catch (const std::exception& e) {
         response->mutable_header()->set_success(false);
@@ -123,6 +129,7 @@ grpc::Status gRPCMBioHeadServiceImpl::StreamExpression(
                 robot->emergency_stop_requested = false;
 
                 first_message = false;
+                CMVR_LOG(DEBUG) << "[gRPCMBioHeadServiceImpl] (StreamExpression): streaming success, id=" << dev_id;
             }
 
             // ✅ 如果紧急停止触发，直接退出
@@ -184,6 +191,7 @@ grpc::Status gRPCMBioHeadServiceImpl::StreamExpression(
         }
 
         CMVR_LOG(INFO) << "StreamExpression finished for device: " << dev_id;
+        CMVR_LOG(DEBUG) << "[gRPCMBioHeadServiceImpl] (StreamExpression): finished, id=" << dev_id;
         return grpc::Status::OK;
     } catch (const std::exception& e) {
         CMVR_LOG(ERROR) << "StreamExpression error: " << e.what();
@@ -211,6 +219,7 @@ grpc::Status gRPCMBioHeadServiceImpl::GetSystemStatus(
 
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        logSuccess("GetSystemStatus", dev_id);
         return grpc::Status::OK;
     }
     catch (const exception& e) {
@@ -245,6 +254,7 @@ grpc::Status gRPCMBioHeadServiceImpl::EmergencyStop(
 
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        logSuccess("EmergencyStop", dev_id);
         return grpc::Status::OK;
     }
     catch (const exception& e) {
@@ -274,6 +284,7 @@ grpc::Status gRPCMBioHeadServiceImpl::SpeakStart(grpc::ServerContext* context, c
 
             response->mutable_header()->set_success(true);
             setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+            logSuccess("SpeakStart", dev_id);
             return grpc::Status::OK;
         }
         catch (const exception& e) {
@@ -300,6 +311,7 @@ grpc::Status gRPCMBioHeadServiceImpl::SpeakStop(grpc::ServerContext* context, co
 
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        logSuccess("SpeakStop", dev_id);
         return grpc::Status::OK;
     }
     catch (const exception& e) {
@@ -325,6 +337,7 @@ grpc::Status gRPCMBioHeadServiceImpl::Happy(grpc::ServerContext* context, const
 
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        logSuccess("Happy", dev_id);
         return grpc::Status::OK;
     }
     catch (const exception& e) {
@@ -348,6 +361,7 @@ grpc::Status gRPCMBioHeadServiceImpl::Surprise(grpc::ServerContext* context, con
 
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        logSuccess("Surprise", dev_id);
         return grpc::Status::OK;
     }
     catch (const exception& e) {
@@ -372,6 +386,7 @@ grpc::Status gRPCMBioHeadServiceImpl::ExpressionTired(grpc::ServerContext* conte
 
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        logSuccess("ExpressionTired", dev_id);
         return grpc::Status::OK;
     }
     catch (const exception& e) {
@@ -397,6 +412,7 @@ grpc::Status gRPCMBioHeadServiceImpl::ExpressionAngry(grpc::ServerContext* conte
 
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        logSuccess("ExpressionAngry", dev_id);
         return grpc::Status::OK;
     }
     catch (const exception& e) {
@@ -422,6 +438,7 @@ grpc::Status gRPCMBioHeadServiceImpl::ExpressionSadness(grpc::ServerContext* con
 
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        logSuccess("ExpressionSadness", dev_id);
         return grpc::Status::OK;
     }
     catch (const exception& e) {
@@ -446,6 +463,7 @@ grpc::Status gRPCMBioHeadServiceImpl::ExpressionYawn(grpc::ServerContext* contex
 
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        logSuccess("ExpressionYawn", dev_id);
         return grpc::Status::OK;
     }
     catch (const exception& e) {

cmvr-es/service/grpc/src/grpc_hlc_service.cpp

@@ -11,6 +11,7 @@
 
 #include <google/protobuf/util/time_util.h>
 
+#include "common/base/logging/logger.h"
 #include "manager/task_manager/include/task_manager.h"
 #include "task/touch_screen_task/include/touch_screen_task.h"
 
@@ -74,6 +75,10 @@ grpc::Status gRPCHlcServiceImpl::touch(grpc::ServerContext *context, const cmvr:
         }
 
         fillTouchResponse(response, true, "");
+        CMVR_LOG(DEBUG) << "[gRPCHlcServiceImpl] (touch): success, u=" << request->u()
+                        << ", v=" << request->v()
+                        << ", phase=" << cmvr::task::TouchScreenTask::phaseToString(touch_task->phase())
+                        << ", status=" << cmvr::task::TouchScreenTask::statusToString(touch_task->lastStatus());
         return grpc::Status::OK;
     } catch (const std::exception& e) {
         fillTouchResponse(response, false, e.what());

cmvr-es/service/grpc/src/grpc_microphone_service.cpp

@@ -40,6 +40,11 @@ grpc::Status gRPCMicroPhoneServiceImpl::GetStatus(grpc::ServerContext* context,
         response->mutable_state()->set_error_message(state.error_message);
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        CMVR_LOG(DEBUG) << "[gRPCMicroPhoneServiceImpl] (GetStatus): success, id=" << dev_id
+                        << ", initialized=" << state.is_initialized
+                        << ", running=" << state.is_running
+                        << ", recording=" << state.is_recording
+                        << ", volume=" << state.volume;
         return grpc::Status::OK;
     }
     catch (const std::exception& e) {
@@ -65,6 +70,8 @@ grpc::Status gRPCMicroPhoneServiceImpl::StartRecord(grpc::ServerContext* context
         dev->startRecording(request->file_path());
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        CMVR_LOG(DEBUG) << "[gRPCMicroPhoneServiceImpl] (StartRecord): success, id=" << dev_id
+                        << ", path=" << request->file_path();
         return grpc::Status::OK;
     }
     catch (const std::exception& e) {
@@ -87,6 +94,7 @@ grpc::Status gRPCMicroPhoneServiceImpl::StopRecord(grpc::ServerContext* context,
         dev->stopRecording();
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        CMVR_LOG(DEBUG) << "[gRPCMicroPhoneServiceImpl] (StopRecord): success, id=" << dev_id;
         return grpc::Status::OK;
     }
     catch (const std::exception& e) {
@@ -109,6 +117,7 @@ grpc::Status gRPCMicroPhoneServiceImpl::PauseRecord(grpc::ServerContext* context
         dev->pause();
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        CMVR_LOG(DEBUG) << "[gRPCMicroPhoneServiceImpl] (PauseRecord): success, id=" << dev_id;
         return grpc::Status::OK;
     }
     catch (const std::exception& e) {
@@ -131,6 +140,7 @@ grpc::Status gRPCMicroPhoneServiceImpl::ResumeRecord(grpc::ServerContext* contex
         dev->resume();
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        CMVR_LOG(DEBUG) << "[gRPCMicroPhoneServiceImpl] (ResumeRecord): success, id=" << dev_id;
         return grpc::Status::OK;
     }
     catch (const std::exception& e) {
@@ -153,6 +163,8 @@ grpc::Status gRPCMicroPhoneServiceImpl::SetVolume(grpc::ServerContext* context,
         dev->setVolume(request->volume());
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        CMVR_LOG(DEBUG) << "[gRPCMicroPhoneServiceImpl] (SetVolume): success, id=" << dev_id
+                        << ", volume=" << request->volume();
         return grpc::Status::OK;
     }
     catch (const std::exception& e) {
@@ -175,6 +187,8 @@ grpc::Status gRPCMicroPhoneServiceImpl::GetVolume(grpc::ServerContext* context,
         response->set_volume(dev->getVolume());
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        CMVR_LOG(DEBUG) << "[gRPCMicroPhoneServiceImpl] (GetVolume): success, id=" << dev_id
+                        << ", volume=" << response->volume();
         return grpc::Status::OK;
     }
     catch (const std::exception& e) {

cmvr-es/service/grpc/src/grpc_speaker_service.cpp

@@ -40,6 +40,12 @@ grpc::Status gRPCSpeakerServiceImpl::GetStatus(grpc::ServerContext* context,
         response->mutable_state()->set_volume(state.volume);
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        CMVR_LOG(DEBUG) << "[gRPCSpeakerServiceImpl] (GetStatus): success, id=" << dev_id
+                        << ", initialized=" << state.is_initialized
+                        << ", running=" << state.is_running
+                        << ", decoding=" << state.is_decoding
+                        << ", paused=" << state.is_paused
+                        << ", volume=" << state.volume;
         return grpc::Status::OK;
     }
     catch (const std::exception& e) {
@@ -63,6 +69,8 @@ grpc::Status gRPCSpeakerServiceImpl::PlayAudio(grpc::ServerContext* context,
         dev->play(request->audio_path());
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        CMVR_LOG(DEBUG) << "[gRPCSpeakerServiceImpl] (PlayAudio): success, id=" << dev_id
+                        << ", path=" << request->audio_path();
         return grpc::Status::OK;
     }
     catch (const std::exception& e) {
@@ -87,6 +95,7 @@ grpc::Status gRPCSpeakerServiceImpl::StopPlayback(grpc::ServerContext* context,
         }
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        CMVR_LOG(DEBUG) << "[gRPCSpeakerServiceImpl] (StopPlayback): success, id=" << dev_id;
         return grpc::Status::OK;
     }
     catch (const std::exception& e) {
@@ -109,6 +118,7 @@ grpc::Status gRPCSpeakerServiceImpl::PausePlayback(grpc::ServerContext* context,
         dev->pause();
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        CMVR_LOG(DEBUG) << "[gRPCSpeakerServiceImpl] (PausePlayback): success, id=" << dev_id;
         return grpc::Status::OK;
     }
     catch (const std::exception& e) {
@@ -131,6 +141,7 @@ grpc::Status gRPCSpeakerServiceImpl::ResumePlayback(grpc::ServerContext* context
         dev->resume();
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        CMVR_LOG(DEBUG) << "[gRPCSpeakerServiceImpl] (ResumePlayback): success, id=" << dev_id;
         return grpc::Status::OK;
     }
     catch (const std::exception& e) {
@@ -153,6 +164,8 @@ grpc::Status gRPCSpeakerServiceImpl::SetVolume(grpc::ServerContext* context,
         dev->setVolume(request->volume());
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        CMVR_LOG(DEBUG) << "[gRPCSpeakerServiceImpl] (SetVolume): success, id=" << dev_id
+                        << ", volume=" << request->volume();
         return grpc::Status::OK;
     }
     catch (const std::exception& e) {
@@ -175,6 +188,8 @@ grpc::Status gRPCSpeakerServiceImpl::GetVolume(grpc::ServerContext* context,
         response->set_volume(dev->getVolume());
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        CMVR_LOG(DEBUG) << "[gRPCSpeakerServiceImpl] (GetVolume): success, id=" << dev_id
+                        << ", volume=" << response->volume();
         return grpc::Status::OK;
     }
     catch (const std::exception& e) {

cmvr-es/service/grpc/src/grpc_system_service.cpp

@@ -4,6 +4,8 @@
 
 #include "../include/grpc_system_service.h"
 
+#include "common/base/logging/logger.h"
+
 using namespace cmvr::device;
 using namespace cmvr::device;
 using namespace cmvr::service;
@@ -18,6 +20,8 @@ grpc::Status gRPCSystemServiceImpl::GetSystemInfo(grpc::ServerContext* context,
         response->set_system_name(dmgr_.name());
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        CMVR_LOG(DEBUG) << "[gRPCSystemServiceImpl] (GetSystemInfo): success, name="
+                        << response->system_name() << ", version=" << response->version();
         return grpc::Status::OK;
     }
     catch (std::exception& e) {
@@ -67,6 +71,8 @@ grpc::Status gRPCSystemServiceImpl::GetSystemStatus(grpc::ServerContext* context
         }
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        CMVR_LOG(DEBUG) << "[gRPCSystemServiceImpl] (GetSystemStatus): success, devices="
+                        << response->device_list_size();
         return grpc::Status::OK;
     }
     catch (const std::exception &e) {
@@ -92,6 +98,7 @@ grpc::Status gRPCSystemServiceImpl::StopAll(grpc::ServerContext* context,
         dmgr_.stop();
         response->mutable_header()->set_success(true);
         setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
+        CMVR_LOG(DEBUG) << "[gRPCSystemServiceImpl] (StopAll): success";
         return grpc::Status::OK;
     }
     catch (std::exception& e) {

cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp

@@ -506,7 +506,7 @@ void TouchScreenTask::stop() {
 void TouchScreenTask::stopUnlocked() {
     if (arm_) {
         try {
-            arm_->stopL(0.0);
+            arm_->stopL();
         } catch (...) {
         }
     }
@@ -1337,7 +1337,7 @@ bool TouchScreenTask::stepRetracting() {
     }
 
     try {
-        arm_->stopL(0.0);
+        arm_->stopL();
     } catch (...) {
         enterFailed(Status::ROBOT_COMMAND_FAILED);
         return false;
@@ -1538,7 +1538,7 @@ bool TouchScreenTask::moveToInitPositionIfEnabled() const {
 
 bool TouchScreenTask::handleTouchTriggered(const bool stop_forward_motion) {
     if (stop_forward_motion) {
-        const auto result = arm_->stopL(0.0);
+        const auto result = arm_->stopL();
         if (!result.ok()) {
             return false;
         }
@@ -1675,7 +1675,7 @@ bool TouchScreenTask::startRetractPhase(const Phase next_phase_after_retract,
 void TouchScreenTask::enterFailed(const Status status) {
     try {
         if (arm_) {
-            arm_->stopL(0.0);
+            arm_->stopL();
         }
     } catch (...) {
     }

protos/cmvr/config/joint_limits_config.proto

@@ -9,12 +9,14 @@ enum JointLimitSource {
 
 message JointLimitConfig {
   string joint_name = 1;
-  double lower = 2;
+  double q_lb = 2;
-  double upper = 3;
+  double q_ub = 3;
-  double velocity = 4;
+  double qd = 4;
+  double qdd = 5;
 }
 
 message JointLimitsConfig {
   JointLimitSource source = 1;
   repeated JointLimitConfig joints = 2;
+  string urdf_path = 3;
 }

protos/cmvr/config/motor_config/motor_config.proto

@@ -9,6 +9,7 @@ message Ti5MotorConfig {
   double limit_q_lb = 3;
   double limit_q_ub = 4;
   double limit_qd = 5;
+  double limit_qdd = 6;
 }
 
 message SocketCanConfig {
@@ -40,7 +41,6 @@ message MotorGroupConfig {
 
   repeated Ti5MotorConfig motors = 20;
   JointLimitsConfig joint_limits = 30;
-  string joint_limits_urdf_path = 31;
 }
 
 message MotorConfig {