equilibrium pose

[sp-sophia-labs]

No description provided.

[sp-sophia-labs]

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

[sp-sophia-labs]

adapt publisher to be in standard marker