Open sp-sophia-labs opened 6 months ago
void CartesianImpedanceController::equilibriumPoseCallback(const geometry_msgs::msg::PoseStamped::SharedPtr msg) { std::lock_guard<std::mutex> 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(); } }
adapt publisher to be in standard marker