Error or exeption caught::Trying to initialize a DQ with a size of 0 which is not allowed.

[AkaneUnosawa]

Hello, everyone.
I tried to do automation then I'm getting this error, is there a problem with the kinematics and DQ module settings?
Error or exeption caught::Trying to initialize a DQ with a size of 0 which is not allowed.

#include "KinematicsNode.h"

namespace psc
{

DQ __get_next_trajectory_step(const DQ& x, const DQ& xf, const double& sampling_time, const double& speed);

KinematicsNode::KinematicsNode(ros::NodeHandle& nodehandle_publisher,
                                                ros::NodeHandle& nodehandle_subsciber,
                                                ros::NodeHandle& nodehandle_datalogger,
                                                const KinematicsNodeConfiguration& configuration,
                                                std::atomic_bool* kill_this_node):
kill_this_node_(kill_this_node),
enabled_(false),
cfg_(configuration),
vrep_interface_(kill_this_node),
clock_(configuration.thread_sampling_time_nsec)
{
    nodehandle_publisher.setCallbackQueue(&publisher_callback_queue_);
    nodehandle_subsciber.setCallbackQueue(&subscriber_callback_queue_);
    nodehandle_datalogger.setCallbackQueue(&datalogger_callback_queue_);
    robot1_kinematics_provider_.reset(new sas::RobotKinematicsProvider(nodehandle_publisher, nodehandle_subsciber, configuration.robot1_kinematics_provider_prefix));
    robot2_kinematics_provider_.reset(new sas::RobotKinematicsProvider(nodehandle_publisher, nodehandle_subsciber, configuration.robot2_kinematics_provider_prefix));
    robot1_driver_interface_.reset(new sas::RobotDriverInterface(nodehandle_publisher, nodehandle_subsciber, configuration.robot1_driver_interface_prefix));
    robot2_driver_interface_.reset(new sas::RobotDriverInterface(nodehandle_publisher, nodehandle_subsciber, configuration.robot2_driver_interface_prefix));
    datalogger_.reset(new sas::DataloggerInterface(nodehandle_datalogger, 20));
}

void KinematicsNode::initialize()
{
    clock_.init();
    ROS_INFO_STREAM(ros::this_node::getName() + "::Connecting to Vrep...");
    if(!vrep_interface_.connect(cfg_.vrep_ip, cfg_.vrep_port, 100, 10))
    {
        throw std::runtime_error("Unable to connect to Vrep \"" + cfg_.vrep_ip + "\" port: " + std::to_string(cfg_.vrep_port));
    }
    ROS_INFO_STREAM(ros::this_node::getName() + "::Connected to Vrep.");
    ROS_INFO_STREAM(ros::this_node::getName() + "::Waiting to connect to robot...");
    while(!robot1_driver_interface_->is_enabled() || !robot2_driver_interface_->is_enabled())
    {
        clock_.update_and_sleep();
        subscriber_callback_queue_.callAvailable();
        if (_should_shutdown())
            throw std::runtime_error("Shutdown while trying to connect to robot.");
    }
    ROS_INFO_STREAM(ros::this_node::getName() + "::Connected to robot.");
    ROS_INFO_STREAM(ros::this_node::getName() + "::Obtained argument DOF = " + std::to_string(cfg_.DOF));
    ROS_INFO_STREAM(ros::this_node::getName() + "::Getting robot1 information from: " + cfg_.robot1_parameter_file_path);
    ROS_INFO_STREAM(ros::this_node::getName() + "::Getting robot2 information from: " + cfg_.robot2_parameter_file_path);

    if (cfg_.DOF == 6)
    {
        arm1_.reset(new SmartArm());
        arm2_.reset(new SmartArm());
    }
    else if (cfg_.DOF == 7)
    {
        arm1_.reset(new SmartArm());
        arm2_.reset(new SmartArm());
    }
    else if (cfg_.DOF == 9)
    {
        arm1_.reset(new FlexibleSmartArm());
        arm2_.reset(new FlexibleSmartArm());
    }
    else
    {
        throw std::runtime_error("Wrong Argument \"DOF\": \"" + std::to_string(cfg_.DOF) + "\", can only be \"6\", \"7\" or \"9\".");
    }
}

/**************************************************************************
 * @brief Runs the control loop until a shutdown signal is not received.
***************************************************************************/
void KinematicsNode::control_loop()
{
    try{

    initialize();
    ROS_INFO_STREAM(ros::this_node::getName() + "::Initializing pre-loop variables...");
    VectorXd q1 = robot1_driver_interface_->get_joint_positions();
    VectorXd q2 = robot2_driver_interface_->get_joint_positions();
    robot1_driver_interface_->send_target_joint_positions(q1);
    robot2_driver_interface_->send_target_joint_positions(q2);

    arm1_->initialize(cfg_.robot1_parameter_file_path, q1, &vrep_interface_);
    arm2_->initialize(cfg_.robot2_parameter_file_path, q2, &vrep_interface_);
    arm1_->set_partner(arm2_.get());
    arm2_->set_partner(arm1_.get());

    // if (cfg_.use_vrep_rcm)
    // {
    //     arm1_->set_rcm(vrep_interface_.get_object_translation("entry1"));
    //     arm2_->set_rcm(vrep_interface_.get_object_translation("entry4"));
    // }

    robot1_kinematics_provider_->send_reference_frame(arm1_->get_reference_frame());
    robot2_kinematics_provider_->send_reference_frame(arm2_->get_reference_frame());
    vrep_interface_.set_object_translation("rcm1", arm1_->rcm_translation());
    vrep_interface_.set_object_translation("rcm2", arm2_->rcm_translation());

    ROS_INFO_STREAM(ros::this_node::getName() + "::Pre-loop variables initialized.");
    ROS_INFO_STREAM(ros::this_node::getName() + "::Starting control loop...");
    while(not _should_shutdown())
    {
        clock_.update_and_sleep();
        subscriber_callback_queue_.callAvailable();
        try
        {
            datalogger_->log(ros::this_node::getName().substr(1)+"_computational_time",clock_.get_computation_time());
            datalogger_->log(ros::this_node::getName().substr(1)+"_elapsed_time", clock_.get_elapsed_time_sec());
            datalogger_->log(ros::this_node::getName().substr(1)+"_desired_sampling_time",clock_.get_desired_thread_sampling_time_sec());
            datalogger_->log(ros::this_node::getName().substr(1)+"_robot1_frequency",arm1_->get_frequency());
            datalogger_->log(ros::this_node::getName().substr(1)+"_robot2_frequency",arm2_->get_frequency());
            datalogger_->log(ros::this_node::getName().substr(1)+"_q1", arm1_->joint_positions());
            datalogger_->log(ros::this_node::getName().substr(1)+"_q2", arm2_->joint_positions());
            datalogger_->log(ros::this_node::getName().substr(1)+"_x1", vec8(arm1_->end_pose()));
            datalogger_->log(ros::this_node::getName().substr(1)+"_x2", vec8(arm2_->end_pose()));
        }
        catch(...)
        {
            ROS_WARN_STREAM(ros::this_node::getName() + "::Unknown error while logging.");
        }
//Confirm the validity of robotkinematics
        if (robot1_kinematics_provider_->is_enabled() && robot2_kinematics_provider_->is_enabled())
        {
        	const DQ& xf1 = robot1_kinematics_provider_->get_desired_pose();
        	const DQ& xf2 = robot2_kinematics_provider_->get_desired_pose();
// calculate the tragectory restep
            const DQ& xd1 = __get_next_trajectory_step(arm1_->end_pose(), xf1, clock_.get_desired_thread_sampling_time_sec(), robot1_kinematics_provider_->get_desired_interpolator_speed());
            const DQ& xd2 = __get_next_trajectory_step(arm2_->end_pose(), xf2, clock_.get_desired_thread_sampling_time_sec(), robot2_kinematics_provider_->get_desired_interpolator_speed());
//How to solve quadratic programming problems
            arm1_->solve_quadratic_programming(xd1, clock_.get_desired_thread_sampling_time_sec());
            arm2_->solve_quadratic_programming(xd2, clock_.get_desired_thread_sampling_time_sec());

            robot1_driver_interface_->send_target_joint_positions(arm1_->joint_positions());
            robot2_driver_interface_->send_target_joint_positions(arm2_->joint_positions());

            /// @brief  log information
            try
            {
                datalogger_->log(ros::this_node::getName().substr(1)+"_xf1", vec8(xf1));
                datalogger_->log(ros::this_node::getName().substr(1)+"_xf2", vec8(xf2));
                datalogger_->log(ros::this_node::getName().substr(1)+"_desired_interpolator_speed1",robot1_kinematics_provider_->get_desired_interpolator_speed());
                datalogger_->log(ros::this_node::getName().substr(1)+"_desired_interpolator_speed2",robot2_kinematics_provider_->get_desired_interpolator_speed());
                datalogger_->log(ros::this_node::getName().substr(1)+"_xd1", vec8(xd1));
                datalogger_->log(ros::this_node::getName().substr(1)+"_xd2", vec8(xd2));
            }
            catch(...)
            {
                ROS_WARN_STREAM(ros::this_node::getName() + "::Unknown error while logging.");
            }
        }
        else
        {
            robot1_kinematics_provider_->send_reference_frame(arm1_->get_reference_frame());
            robot2_kinematics_provider_->send_reference_frame(arm2_->get_reference_frame());
        }
        robot1_kinematics_provider_->send_pose(arm1_->end_pose());
        robot2_kinematics_provider_->send_pose(arm2_->end_pose());
        vrep_interface_.set_object_pose("x1", arm1_->end_pose());
        vrep_interface_.set_object_pose("x2", arm2_->end_pose());

        datalogger_callback_queue_.callAvailable();
        publisher_callback_queue_.callAvailable();
    }

    }catch(const std::exception& e)
    {
        ROS_WARN_STREAM(ros::this_node::getName()+"::Error or exeption caught::" + e.what());
    }catch(...)
    {
        ROS_WARN_STREAM(ros::this_node::getName()+"::Unknown error or exeption in control_loop().");
    }

}

/**
 * @brief Checks if the node should be shut down or not.
 * @return true is should be shut down, false otherwise.
 */
bool KinematicsNode::_should_shutdown()
{
    return (*kill_this_node_);
}

/**
 * @brief Obtains the next step using dual quaternion interpolation between the current pose
 * x and the final pose xf with a given speed.
 * @param x a DQ presenting the current pose.
 * @param xf a DQ representing the final pose for the interpolation.
 * @param sampling_time a double representing the sampling time in seconds.
 * @param speed a double representing the trajectory speed.
 * @return a DQ representing the next pose in the trajectory to reach xf with a given speed.
 */
DQ __get_next_trajectory_step(const DQ& x, const DQ& xf, const double& sampling_time, const double& speed)
{
    const double error_1 = vec8(conj(x)*xf-1).norm()/speed;
    const double error_2 = `vec8(conj(x)*xf+1).norm()/speed;`

    const double t_max = error_1 < error_2 ? error_1 : error_2;

    if(t_max < sampling_time)
        return xf;

    return error_1 < error_2 ? x*exp((sampling_time/t_max) * log(conj(x)*(xf))) : x*exp((sampling_time/t_max) * log(conj(x)*(-xf)));
}

}
`

arm1_.reset(new FlexibleSmartArm());
        arm2_.reset(new FlexibleSmartArm());

There seems to be a problem with the FlexibleSmartArm part of

[mmmarinho]

Hi, @AkaneUnosawa.

From my understanding of this, this is not a DQ Robotics problem.

One of these statements is not initializing a DQ correctly. Which one, in particular, I am not able to answer.

    arm1_->initialize(cfg_.robot1_parameter_file_path, q1, &vrep_interface_);
    arm2_->initialize(cfg_.robot2_parameter_file_path, q2, &vrep_interface_);
    arm1_->set_partner(arm2_.get());
    arm2_->set_partner(arm1_.get());

    robot1_kinematics_provider_->send_reference_frame(arm1_->get_reference_frame());
    robot2_kinematics_provider_->send_reference_frame(arm2_->get_reference_frame());
    vrep_interface_.set_object_translation("rcm1", arm1_->rcm_translation());
    vrep_interface_.set_object_translation("rcm2", arm2_->rcm_translation());

Please check if all these statements are returning a proper object. This would be how I'd start the search for the problem.

arm1_->get_reference_frame()
arm2_->get_reference_frame()
arm1_->rcm_translation()
arm2_->rcm_translation()

Kind regards,
Murilo

[bvadorno]

Thank you both for the discussion. @AkaneUnosawa, I suggest always creating a minimum example to illustrate your problem. Since this problem seems to be unrelated to DQ Robotics, I'll close this discussion.

Kind regards,
Bruno