How to change Damping and inertia parameters present in the robot’s admittance model in ROS?

[yuweiDu]

Hi! How can I change Damping and inertia parameters present in the robot’s admittance model in ROS? You've mentioned that Damping and inertia parameters present in the robot’s admittance model are also configurable in gen2 user guide and there is SetTorqueControlParameters service in ROS. I am a beginner and quite confused about how to use it.Could you please give me advice. Thank you!

[felixmaisonneuve]

Hi @yuweiDu,

1. How to use SetTorqueControlParameters : In kinova_bringup/launch/config/robot_parameters.yaml, you will find a section torque_parameters. You will need to uncomment these parameters (torque_min, torque_max, safety_factor, com_parameters) and set values. You can also set these with rosparam set torque_parameters/safety_factor 1 (or something like that, I didn't check the syntax). When you call SetTorqueControlParameters with the service in/set_torque_control_parameters, it will use those parameters.

2. To change damping and inertia parameters There is two API calls : SetCartesianInertiaDamping and SetAngularInertiaDamping. Currently, there is not any ROS service that allows you to call these, so you will need to edit the driver.

In kinova_driver/src/kinova_arm.cpp, in KinovaArm() add something like

    cartesian_damping_service_ = node_handle_.advertiseService("in/cartesian_damping", &KinovaArm::ChangeCartesianDamping, this);

In that same file, you will need to create the function KinovaArm::ChangeCartesianDamping that calls SetCartesianInertiaDamping

bool KinovaArm::ChangeCartesianDamping(kinova_msgs::ChangeCartesianDamping::Request &req, kinova_msgs::ChangeCartesianDamping::Response &res)
{
        AngularInfo inertia_info,damping_info;

        inertia_info.Actuator1 = req.inertia1
        inertia_info.Actuator2 = req.inertia2
        inertia_info.Actuator3 = req.inertia3
        inertia_info.Actuator4 = req.inertia4
        inertia_info.Actuator5 = req.inertia5
        inertia_info.Actuator6 = req.inertia6

        damping_info.Actuator1 = req.damping1
        damping_info.Actuator2 = req.damping2
        damping_info.Actuator3 = req.damping3
        damping_info.Actuator4 = req.damping4
        damping_info.Actuator5 = req.damping5
        damping_info.Actuator6 = req.damping6

        kinova_comm_.SetCartesianInertiaDamping(inertia_info, damping_info);
        return true;
}

We are using a kinova_msgs that doesn't exist (kinova_msgs::ChangeCartesianDamping), so we need to create this as well. In kinova_msgs/msg/srv/, add a file ChangeCartesianDamping with this :

float32 inertia1
float32 inertia2
float32 inertia3
float32 inertia4
float32 inertia5
float32 inertia6
float32 damping1
float32 damping2
float32 damping3
float32 damping4
float32 damping5
float32 damping6
---

After all of this, you will need to do a catkin_make and, after launching the driver, you will have access to a new service in/cartesian_damping. This is not very pretty, but it should be able to get you going. Also, I did not test any of this, so it might not work out of the box, but it should give you an idea on how to add your own service to use a particular call in the API. All the API calls are found in kinova_driver/include/kinova/Kinova.API.UsbCommandLayerUbuntu.h (if you are using a USB connection on Ubuntu)

Regards, Felix

[yuweiDu]

Hi @felixmaisonneuve Thank you for your reply! I'll try it.

[Tzxtoiny]

SO I want to know if this parameter is the parameter when I start Cartesian Admittance mode（rosservice call /'${kinova_robotType}_driver'/in/start_force_control）? What is the default parameter?

[yuweiDu]

SO I want to know if this parameter is the parameter when I start Cartesian Admittance mode（rosservice call /'${kinova_robotType}_driver'/in/start_force_control）? What is the default parameter?

Hi, @Tzxtoiny You can refer to page 36 of KINOVA® Gen2 Ultra lightweight robot User Guide

[felixmaisonneuve]

Hi @yuweiDu,

I just realised I mixed SetCartesianInertiaDamping and SetAngularInertiaDamping. I am using AngularInfo for SetCartesianInertiaDamping instead of CartesianInfo.

Also, I forgot to mention you need to add a function in kinova_comm.cpp.

The result should be more like this :

In kinova_driver/src/kinova_arm.cpp

cartesian_damping_service_ = node_handle_.advertiseService("in/cartesian_damping", &KinovaArm::ChangeCartesianDamping, this);

bool KinovaArm::ChangeCartesianDamping(kinova_msgs::ChangeCartesianDamping::Request &req, kinova_msgs::ChangeCartesianDamping::Response &res)
{
        CartesianInfo info;

        info.X= req.X
        info.Y= req.Y
        info.Z= req.Z

        info.ThetaX= req.ThetaX
        info.ThetaY= req.ThetaY
        info.ThetaZ= req.ThetaZ

        kinova_comm_.SetCartesianInertiaDamping(info);
        return true;
}

Your kinova_msgs/msg/srv/ChangeCartesianDamping file should look like this :

float32 X
float32 Y
float32 Z
float32 ThetaX
float32 ThetaY
float32 ThetaZ
---

We use

kinova_comm_.SetCartesianInertiaDamping(info);

So we need to create this funciton in kinova_comm.cpp, it will look something like this

void KinovaComm::SetCartesianInertiaDamping(CartesianInfo &info)
{
    boost::recursive_mutex::scoped_lock lock(api_mutex_);
    int result = kinova_api_.setCartesianInertiaDamping(info);
    if (result != NO_ERROR_KINOVA)
    {
        throw KinovaCommException("Could not set the cartesian damping and inertia", result);
    }
}

[yuweiDu]

Hi @felixmaisonneuve Thank you for reminding me of this issue. I have noticed it and thanks to the fantastic work you and your team have done, I finish the function by referring to the exisiting code. However, I have another question that why there is no similar function to modify the stiffness parameter of admittance controller but only damping and inertia parameters. Thank you!

[felixmaisonneuve]

Hi @yuweiDu,

I do not know how easy/hard it is to implement admittance control, and I don't think changing the stiffness of it is as simple as changing one variable. Also, the arm was not originaly conceived with this feature in mind. Admittance Control was added afterwards, so it is possible some options could not be implemented (or they were simply not implemented at all). Then again, I don't know how such feature is implemented, so I have no idea how this works behind the scene. What I know is that the current implementation was probably made in a "generic" way, because the same code needs to support 4,6 and 7 degrees of freedom, it needs to allow cartesian control, angular control, admittance control, it needs to have a certain degre of precision... For sure, some options/tweakings had to be sacrificed.

If you know what you are doing, the API contains a couple function that you could use to tweak your own admittance control. I need to warn you some of these calls could damage your arm (e.g. changing PID filters), so you need to be extra carefull if you want to play with these (this is from kinova_api.h)

    // %Tag(experimental)%
    // force/torque control are not tested in ROS and may lead to unexpect motion.
    // do not use these functions unless you are know exactly what you are doing.

    int (*setTorqueControlType)(TORQUECONTROL_TYPE);

    int (*setActuatorPIDFilter)(int, float, float, float);
    int (*setAngularInertiaDamping)(AngularInfo, AngularInfo);
    int (*getAngularForceGravityFree)(AngularPosition &);    
    int (*getAngularTorqueCommand)(float[COMMAND_SIZE]);
    int (*getAngularTorqueGravityEstimation)(float[COMMAND_SIZE]);
    int (*setTorqueActuatorGain)(float[COMMAND_SIZE]);
    int (*setTorqueActuatorDamping)(float[COMMAND_SIZE]);
    int (*setTorqueCommandMax)(float[COMMAND_SIZE]);    
    int (*setTorqueRateLimiter)(float[COMMAND_SIZE]);
    int (*setTorqueFeedCurrent)(float[COMMAND_SIZE]);
    int (*setTorqueFeedVelocity)(float[COMMAND_SIZE]);
    int (*setTorquePositionLimitDampingGain)(float[COMMAND_SIZE]);
    int (*setTorquePositionLimitDampingMax)(float[COMMAND_SIZE]);
    int (*setTorquePositionLimitRepulsGain)(float[COMMAND_SIZE]);
    int (*setTorquePositionLimitRepulsMax)(float[COMMAND_SIZE]);
    int (*setTorqueFilterVelocity)(float[COMMAND_SIZE]);
    int (*setTorqueFilterMeasuredTorque)(float[COMMAND_SIZE]);
    int (*setTorqueFilterError)(float[COMMAND_SIZE]);
    int (*setTorqueFilterControlEffort)(float[COMMAND_SIZE]);

    int (*setGravityVector)(float[GRAVITY_VECTOR_SIZE]);
    int (*setGravityType)(GRAVITY_TYPE);
    int (*setSwitchThreshold)(float[COMMAND_SIZE]);
    int (*setPositionLimitDistance)(float[COMMAND_SIZE]);
    int (*setGravityPayload)(float[GRAVITY_PAYLOAD_SIZE]);
    int (*setTorqueVibrationController)(float);
    int (*setTorqueRobotProtection)(int);

    int (*getTrajectoryTorqueMode)(int &);
    int (*setTorqueInactivityType)(int);

    // %EndTag(experimental)%

If you want more specific informations, I can ask coworkers that know more than I do about this topic.

Hope this helps, Felix

[felixmaisonneuve]

Hi @yuweiDu,

Do you still need support on this topic or can we close this issue? I will close it in a couple of weeks if I don't get any answer from you.

Best, Felix

[yuweiDu]

Hi @felixmaisonneuve , Thank you for your help! Feel free to close this issue. Sincerely, Yuwei