Our top priority

We are as proud of our service as we are of our robots.

Forgot your password?
Register

Splash Forums Development SDK Some questions about force control

This topic contains 29 replies, has 6 voices, and was last updated by  cortesao 1 year, 7 months ago.

Viewing 15 posts - 1 through 15 (of 30 total)
  • Author
    Posts
  • #1745

    Anonymous

    Hi, everyone.
    Kinova provide really good robot arm and I believe many people are using it. But why is the forum so quiet? Is there some other forums where we can discuss? haha!
    Our lab bought a Kinova jaco2 arm recently and I am really excited to work with it. My research topic is related to force control of manipulator. I found that in Kinova SDK provide many useful api concerned with the torque and force control of the arm. But some questions arise:
    1. How do you do the force estimation?
    I notice the sdk provide the api “GetCartesianForce” which can estimate the external force. I was wondering what kind of method is used during force estimation. Do the inertia and Coriolis/centrifugal torque is considered or just consider the gravity torque when estimate external force.

    2. Can you provide the dynamic model of the kinova jaco arm?
    Continue with last problem, if the force estimation consider the inertia and Coriolis/centrifugal torque, a dynamic model of robot arm is needed. If so, can user get access to the dynamic model of the robot arm?

    3. How is gravity estimation doing when choose “optimal”?
    Now the optimal mode is only available when the robot is standing. If I want to develop a program to estimate the gravity when arm is not standing, how should I start it.

    Any reply are appreciated and thank you in advance!
    Jin

    #1775

    Hugo Lamontagne
    Participant

    Hi Jin,

    I’m glad that you took some time to write a post here. Inertia parameters are considered during the force estimation along with the gravity. To have the dynamic model, send a mail to support@kinova.ca, they will give you the document without any problem. When you choose the optimal gravity compensation, you need to first run the function named RunGravityZEstimationSequence. This function will start a trajectory and the robot will move so make sure nothing is in its way. Once the trajectory is over, a new file will be created(ParametersOptimal_Z.txt) and the calibrated parameters will be stored in it. The parameters will also be stored in a double array returned by the function. Take those parameters and send them in the function SetGravityOptimalZParam.

    I hope this will help you,

    Hugo

    #2096

    gurusub90
    Participant

    Hi!
    I’ve been working with force control and have a question:

    What are the optimal settings for the inertia and damping parameters. Could someone provide some combinations that work well with the robot? I’ve been playing with these settings and can get the inertia parameters for translation to work like I would like them too, but for the rotational DOFs they are either too stiff or jumpy.

    Is it possible to keep the orientation of the robot end effector constant with respect to the world coordinate system but have the position under admittance control?

    I’d also like the default settings for the robot.

    Thanks
    Guru

    #2097

    gurusub90
    Participant

    Hi,
    I have another question, If I use a different end effector, ie, change the gripper to something else, how do I modify force control so it knows the new kinematics/dynamics of the robot? Would RunGravityZEstimationSequence function help estimate the new parameters?

    Thanks.
    Guru

    #2108

    Anonymous

    Hi Guru

    Here are my answers to your questions:

    Q: What are the optimal settings for the inertia and damping parameters. Could someone provide some combinations that work well with the robot? I’ve been playing with these settings and can get the inertia parameters for translation to work like I would like them too, but for the rotational DOFs they are either too stiff or jumpy.
    A: I can’t give you the perfect combination. Normally the default parameters work fine, but not in the case of your robot apparently. Here are some debugging steps before trying to change the inertia and damping parameters:
    1) Insure your torque sensors are well calibrated. To do so, follow the ‘Zero torque’ calibration procedure detailed in the ‘Advanced specification guide’.
    2) Use the optimal gravity mode. To do so, you will have to perform an automatic calibration trajectory with your robot in order to get some parameters (RunGravityZEstimationSequence). Then you will have to send these parameters to the robot and to activate the optimal gravity mode (SetOptimalZParam and SetGravityType)
    3) If both of these previous steps are not enough, you can try to tweak orientation admittance parameters, while specifying inertia and damping values close to the default values.

    Q:Is it possible to keep the orientation of the robot end effector constant with respect to the world coordinate system but have the position under admittance control?
    A: You could increase a lot the damping and inertia parameters in orientation while keeping those in translation to their default value. This way, the admittance orientation command will always be close to zero. Also, please insre your arm is set in Fixed mode (and not in Rotating mode) if you want to keep the orientation constant. By default, the arm is the Rotating mode.

    Q: I’d also like the default settings for the robot.
    A: Do you have a Jaco or a Mico arm?
    For Mico:
    Damping_X=20.8 N/(m/s)
    Damping_Y=20.8 N/(m/s)
    Damping_Z=20.8 N/(m/s)
    Damping_ThetaX=0.779 Nm/(rad/s)
    Damping_ThetaY=0.779 Nm/(rad/s)
    Damping_ThetaZ=0.779 Nm/(rad/s)

    Inertia_X=5.0 N/(m/s^2)
    Inertia_Y=5.0 N/(m/s^2)
    Inertia_Z=5.0 N/(m/s^2)
    Inertia_ThetaX=0.14 Nm/(rad/s^2)
    Inertia_ThetaY=0.14 Nm/(rad/s^2)
    Inertia_ThetaZ=0.14 Nm/(rad/s^2)

    For Jaco V2
    Damping_X=18.18 N/(m/s)
    Damping_Y=31.82 N/(m/s)
    Damping_Z=54.55 N/(m/s)
    Damping_ThetaX=0.85 Nm/(rad/s)
    Damping_ThetaY=0.85 Nm/(rad/s)
    Damping_ThetaZ=0.85 Nm/(rad/s)

    Inertia_X=6.36 N/(m/s^2)
    Inertia_Y=11.14 N/(m/s^2)
    Inertia_Z=19.10 N/(m/s^2)
    Inertia_ThetaX=0.21 Nm/(rad/s^2)
    Inertia_ThetaY=0.21 Nm/(rad/s^2)
    Inertia_ThetaZ=0.21 Nm/(rad/s^2)

    Q: If I use a different end effector, ie, change the gripper to something else, how do I modify force control so it knows the new kinematics/dynamics of the robot? Would RunGravityZEstimationSequence function help estimate the new parameters?
    A: Yes, you can run the automatic calibration trajectory using RunGravityZEstimationSequence, and this function will output you the right gravity parameters with your new gripper. Of course, run RunGravityZEstimationSequence with your new gripper. Another method would be to use the TorqueConsole and specify the mass and center of mass of your new gripper in the Payload section.
    TorqueConsole

      Hope this helps you 🙂
      Martine

    #2117

    gurusub90
    Participant

    Hi Martine,
    Thanks for the response! It was very helpful! Is there any way to have gravity compensation on with the robot in a different orientation? My robot is a Mico2.

    Do you have documents specifying the volume of usable space around the robot, locations of singularities etc, this would be very useful information.

    Thank you!
    Guru

    • This reply was modified 2 years ago by  gurusub90.
    • This reply was modified 2 years ago by  gurusub90.
    #2120

    gurusub90
    Participant

    Hi Martine,
    Thanks again for your help. I have another question:

    When setting the torque zero using the Torque zero GUI option, is this reflected in the Angular Monitoring screen? When I tried, it did not change.

    Thanks again for your help,
    Guru

    #2121

    Anonymous

    Hi Guru

    Yes it is supposed to show in the Angular Monitoring Center. With the Development Center, you have to reset the zero torque joint by joint (the ‘Apply to all’ command does not work properly). Use the ‘Apply’ command instead. The addresses are the following:

    16: actuator 1
    17: actuator 2
    18: actuator 3
    19: actuator 4
    20: actuator 5
    21: actuator 6

    I did not forget you for your other questions. I am working on some documentation

    Thanks

    Martine

    #2127

    gurusub90
    Participant

    Hi Martine,
    Thanks for your support, its been really helpful. I think I’ve come across a bug, but I might be using the API incorrectly.

    When I command a cartesian position and then enable force control, the robot shifts to force control but with impedance set at default values w.r.t the angular coordinates of the joints. I can kind of get around this by pressing the home button on the joy stick when its in force control. I tried many things including the function: SetCartesianControl, but this doesn’t seem to work. If this is a bug is there a work around? If it isn’t please let me know what I’m doing wrong.

    Thanks a lot!
    Guru

    #2128

    Anonymous

    Dear Guru

    I do not understand what you mean by “with impedance set at default values w.r.t the angular coordinates of the joints”. Do you mean th force control behaviour us different depending what is the current position of the robot? If so, this is a suspect behaviour. Could you also tell me how would you expect the robot to behave (i.e. tell me what is wrong with the actual behaviour?) Are you enabling the force control after having sent the robot in its home position at least once? Seems from what you tell me that you are still in angular control… What happens if you try controlling the robot when it has the impedence set as at default values w.r.t the angular coordinateur of the joint? For the SetCartesianControl function to work, the robot as to be in a valid Cartesian position (i.e. with the hand not too close from the base, not in singularity, etc.). The home position is a valid Cartesian position.

    Thanks

    Martine

    #2132

    gurusub90
    Participant

    Hi Martine,
    I’m sorry I wasn’t clear, maybe a code snippet will help:

    
    result = MyInitAPI();
    MyMoveHome();
    float OptimalParam[OPTIMAL_Z_PARAM_SIZE] = {0.86285,0.0180729,-0.00496937,-0.854213,
                                                0.00472874,0.482024,0.00124092,0.151529,-0.00096519,
                                                -0.00816589,-0.329973,-0.128842,0.238235,0.058439,-0.0345066,0.111202
                                               };
    MySetGravityOptimalZParam(OptimalParam);
    MySetGravityType(OPTIMAL);
    MyInitFingers();
    TrajectoryPoint pointToSend;
    pointToSend.InitStruct();
    pointToSend.Position.Type = CARTESIAN_POSITION;
    pointToSend.Position.CartesianPosition.X = -0.3;
    pointToSend.Position.CartesianPosition.Y = -0.2;
    pointToSend.Position.CartesianPosition.Z = 0.2;
    pointToSend.Position.CartesianPosition.ThetaX = PI;
    pointToSend.Position.CartesianPosition.ThetaY = 0;
    pointToSend.Position.CartesianPosition.ThetaZ = 0;
    
    pointToSend.Position.Fingers.Finger1 = 6400;
    pointToSend.Position.Fingers.Finger2 = 5500;
    pointToSend.Position.Fingers.Finger3 = 0;
    
    MySendBasicTrajectory(pointToSend);
    usleep(1000);
    
    pointToSend.Position.CartesianPosition.X = -0.3; 
    pointToSend.Position.CartesianPosition.Y = 0; 
    pointToSend.Position.CartesianPosition.Z = 0.1;
    
    MySendBasicTrajectory(pointToSend);
    usleep(1000);
    
    cin.ignore(); // Key press after robot reaches required location
    
    result = (*MyEraseAllTrajectories)();
    result = (*MySetCartesianControl)();
    result = (*MySetFrameType)(0);
    result = MyStartForceControl();
    for(int a =1; a != 0;)
        cin >> a;// Press any key except 1 to exit
    result = (*MyStopForceControl)();
    result = (*MyCloseAPI)();
    

    I believe the manipulator is in a valid position. When I activate forcecontrol in the home position and then move it to the desired location manually by physically moving it, it stays in forcecontrol. But when I ask the robot to reach the desired position via code ie:

    
    MySendBasicTrajectory(pointToSend)
    

    and then activate forcecontrol, it flips to joint space impedance. If I press the home button on the joystick for a brief moment just so the robot barely moves, the robot provides Cartesian impedance. I’m using the button press on the joystick as a work around.

    Thanks again for your help!
    Guru

    • This reply was modified 2 years ago by  gurusub90.
    #2135

    Anonymous

    Hi Guru

    IN the code you just sent, do you get the joint impedance problem?
    Do you get the same problem if you comment the line MySetCartesianControl?

    Thanks

    Martine

    #2136

    Anonymous

    HI Guru

    I would like to answer your previous questions:

    Hi Martine,
    Thanks for the response! It was very helpful! Is there any way to have gravity compensation on with the robot in a different orientation? My robot is a Mico2. R: For the moment, only the manual gravity compensation model is available for other orientation of the robot. This mode is activated by default on the robot. You just have to specify the new gravity vector using the Torque Console or the API. I am currently working on a version of the Optimal gravity compensation model that would work with the robot in any orientation.

    Do you have documents specifying the volume of usable space around the robot, locations of singularities etc, this would be very useful information. R: I have a quick sheet that describes the singularities. For the workspace, I do not have a formal sheet that shows you the volume of usable space around the robot. The Advanced Specification Guide tells you the angle limits of the robot. The reach or a Mico arm is 70 cm, which means the robot will be able to reach a maximum height of about 1 m over its base, and more or less 40 cm under. Does this helps you? I’ll see if I can provide a more visual representation of the workspace around the robot.

    Thank you!

    Martine

    #2144

    Anonymous

    Hi, I have a question about the direct torque control mode.
    I test the important api “sendAngularTorqueCommand()” and arm didn’t move as the code desired, just stop there. I check the example code and found the COMMAND_SIZE is 70. I changed it to 6 but it still can’t work. I was wondering why the size is 70 and why that api is not working.
    The system is windows 8. I also try the api in ubuntu and the same problem happens. I use the header file and .dll file from example/Lib_Examples folder, not the default.
    Is this related to version of firmware and sdk? How could I chack if the firmware and sdk are updated and workable.
    Any reply helps!

    #2145

    Anonymous

    The code is:

    #include "stdafx.h"
    #include "CommandLayer.h"
    #include "CommunicationLayerWindows.h"
    #include <conio.h>
    #include "KinovaTypes.h"
    #include <iostream>
    using namespace std;
    
    //A handle to the API.
    HINSTANCE commandLayer_handle;
    
    //Function pointers to the functions we need
    int(*MyInitAPI)();
    int(*MyCloseAPI)();
    int(*MyGetAngularCommand)(AngularPosition &);
    int(*MyGetAngularPosition)(AngularPosition &);
    int(*MyGetDevices)(KinovaDevice devices[MAX_KINOVA_DEVICE], int &result);
    int(*MySetActiveDevice)(KinovaDevice device);
    int(*MyGetActuatorAcceleration)(AngularAcceleration &Response);
    int(*MyGetAngularVelocity)(AngularPosition &Response);
    int(*MyMoveHome)();
    int(*MyRunGravityZEstimationSequence)(ROBOT_TYPE type, double OptimalzParam[OPTIMAL_Z_PARAM_SIZE]);
    int(*MySwitchTrajectoryTorque)(GENERALCONTROL_TYPE);
    int(*MySetTorqueSafetyFactor)(float factor);
    int(*MySendAngularTorqueCommand)(float Command[COMMAND_SIZE]);
    int(*MySendCartesianForceCommand)(float Command[COMMAND_SIZE]);
    int(*MySetGravityVector)(float Command[3]);
    int(*MySetGravityPayload)(float Command[GRAVITY_PAYLOAD_SIZE]);
    int(*MySetGravityOptimalZParam)(float Command[GRAVITY_PARAM_SIZE]);
    int(*MySetGravityType)(GRAVITY_TYPE Type);
    int(*MySetTorqueVibrationController)(float value);
    int(*MyGetAngularForceGravityFree)(AngularPosition &);
    int(*MyGetCartesianForce)(CartesianPosition &);
    int(*MySetTorqueControlType)(TORQUECONTROL_TYPE type);
    
    int main(int argc, char* argv[])
    {
    	int programResult = 0;
    	int result;
    
    	//We load the API.
    	commandLayer_handle = LoadLibrary(L"CommandLayerWindows.dll");
    
    	//We load the functions from the library (Under Windows, use GetProcAddress)
    	MyInitAPI = (int(*)()) GetProcAddress(commandLayer_handle, "InitAPI");
    	MyCloseAPI = (int(*)()) GetProcAddress(commandLayer_handle, "CloseAPI");
    	MyGetAngularCommand = (int(*)(AngularPosition &)) GetProcAddress(commandLayer_handle, "GetAngularCommand");
    	MyGetAngularPosition = (int(*)(AngularPosition &)) GetProcAddress(commandLayer_handle, "GetAngularPosition");
    	MyGetDevices = (int(*)(KinovaDevice devices[MAX_KINOVA_DEVICE], int &result)) GetProcAddress(commandLayer_handle, "GetDevices");
    	MySetActiveDevice = (int(*)(KinovaDevice devices)) GetProcAddress(commandLayer_handle, "SetActiveDevice");
    	MyGetActuatorAcceleration = (int(*)(AngularAcceleration &)) GetProcAddress(commandLayer_handle, "GetActuatorAcceleration");
    	MyGetAngularVelocity = (int(*)(AngularPosition &)) GetProcAddress(commandLayer_handle, "GetAngularVelocity");
    	MyRunGravityZEstimationSequence = (int(*)(ROBOT_TYPE, double OptimalzParam[OPTIMAL_Z_PARAM_SIZE])) GetProcAddress(commandLayer_handle, "RunGravityZEstimationSequence");
    	MySwitchTrajectoryTorque = (int(*)(GENERALCONTROL_TYPE)) GetProcAddress(commandLayer_handle, "SwitchTrajectoryTorque");
    	MySetTorqueSafetyFactor = (int(*)(float)) GetProcAddress(commandLayer_handle, "SetTorqueSafetyFactor");
    	MySendAngularTorqueCommand = (int(*)(float Command[COMMAND_SIZE])) GetProcAddress(commandLayer_handle, "SendAngularTorqueCommand");
    	MySendCartesianForceCommand = (int(*)(float Command[COMMAND_SIZE])) GetProcAddress(commandLayer_handle, "SendCartesianForceCommand");
    	MySetGravityVector = (int(*)(float Command[3])) GetProcAddress(commandLayer_handle, "SetGravityVector");
    	MySetGravityPayload = (int(*)(float Command[GRAVITY_PAYLOAD_SIZE])) GetProcAddress(commandLayer_handle, "SetGravityPayload");
    	MySetGravityOptimalZParam = (int(*)(float Command[GRAVITY_PARAM_SIZE])) GetProcAddress(commandLayer_handle, "SetGravityOptimalZParam");
    	MySetGravityType = (int(*)(GRAVITY_TYPE Type)) GetProcAddress(commandLayer_handle, "SetGravityType");
    	MyGetAngularForceGravityFree = (int(*)(AngularPosition &)) GetProcAddress(commandLayer_handle, "GetAngularForceGravityFree");
    	MyGetCartesianForce = (int(*)(CartesianPosition &)) GetProcAddress(commandLayer_handle, "GetCartesianForce");
    	MySetTorqueVibrationController = (int(*)(float)) GetProcAddress(commandLayer_handle, "SetTorqueVibrationController");
    	MySetTorqueControlType = (int(*)(TORQUECONTROL_TYPE)) GetProcAddress(commandLayer_handle, "SetTorqueControlType");
    	MyMoveHome = (int(*)()) GetProcAddress(commandLayer_handle, "MoveHome");
    
    	//If the was loaded correctly
    	//Verify that all functions has been loaded correctly
    	if ((MyInitAPI == NULL) || (MyCloseAPI == NULL) || (MyGetAngularCommand == NULL) || (MyGetAngularPosition == NULL)
    		|| (MySetActiveDevice == NULL) || (MyGetDevices == NULL) || (MyGetAngularVelocity == NULL))
    	{
    		cout << "* * *  E R R O R   D U R I N G   I N I T I A L I Z A T I O N  * * *" << endl;
    		programResult = 0;
    	}
    	else
    	{
    		cout << "I N I T I A L I Z A T I O N   C O M P L E T E D" << endl << endl;
    		result = (*MyInitAPI)();
    		int resultComm;
    		AngularPosition DataCommand;
    		// Get the angular command to test the communication with the robot
    		resultComm = MyGetAngularCommand(DataCommand);
    		cout << "Initialization's result :" << result << endl;
    		cout << "Communication result :" << resultComm << endl;
    		// If the API is initialized and the communication with the robot is working
    		if (result == 1 && resultComm == 1)
    		{
    			cout << "API initialization worked" << endl;
    			cout << "The robot will swich to torque control mode and move. Be cautious." << endl;
    			// Set to position mode
    			MySwitchTrajectoryTorque(POSITION);
    			// Move to home position
    			MyMoveHome();
    			// Set the torque control type to Direct Torque Control
    			MySetTorqueControlType(DIRECTTORQUE);
    			cout<<"start to do direct torque"<<endl;
    			// Set the safety factor to 0.6
    			MySetTorqueSafetyFactor(0.6);
    			// Set the vibration controller to 0.5
    			MySetTorqueVibrationController(0.5);
    			// Switch to torque control
    			// (Here we switch before sending torques. The switch is possible because the gravity torques are already taken into account.)
    			MySwitchTrajectoryTorque(TORQUE);
    			// Initialize the torque commands
    			float TorqueCommand[COMMAND_SIZE];
    			for (int i = 0; i < COMMAND_SIZE; i++)
    			{
    				TorqueCommand[i] = 0;
    			}
    			// Send the torque commands for 2 seconds
    			for (int i = 0; i < 200; i++)
    			{
    				// Torque of 0.5Nm on joint 6
    				TorqueCommand[5] = 0.7;
    				// Send the torques
    				MySendAngularTorqueCommand(TorqueCommand);
    				// Sleep 10 ms
    				Sleep(10);
    			}
    			cout<<"joint toruqe send end"<<endl;
    			// Wait
    			Sleep(2000);
    			// Initialize the Cartesian force commands
    			float CartForceCommand[COMMAND_SIZE];
    			for (int i = 0; i < COMMAND_SIZE; i++)
    			{
    				CartForceCommand[i] = 0;
    			}
    			// Send the force commands for 2 seconds
    			for (int i = 0; i < 200; i++)
    			{
    				// Force command in Y direction of Fy = -2
    				CartForceCommand[1] = -3;
    				// Send the force command
    				MySendCartesianForceCommand(CartForceCommand);
    				// Sleep of 10 ms
    				Sleep(10);
    			}
    			cout<<"cartesian force send end"<<endl;
    			//Wait
    			Sleep(2000);
    			// Switch back to position
    			MySwitchTrajectoryTorque(POSITION);
    		}
    		else
    		{
    			cout << "API initialization failed" << endl;
    		}
    		cout << endl << "C L O S I N G   A P I" << endl;
    		result = (*MyCloseAPI)();
    		programResult = 1;
    
    	}
    
    	FreeLibrary(commandLayer_handle);
    	system("pause");
    
    	return programResult;
    }
Viewing 15 posts - 1 through 15 (of 30 total)

You must be logged in to reply to this topic.

Trending now

Expert Talk

Hitting the Mark with Marketing: The Do’s and Don’ts of Creating a Message Around Robotics

While robotics companies have been around for decades, the spotlight on the industry has only really intensified in recent years as new technologies become more readily available and more implicated in the lives of humans.

Read more
Expert Talk

For the Greater Good: The Importance of Fulfilling Corporate Social Responsibility

By now, it goes without saying that the cornerstone of Kinova’s DNA is our desire to create products that empower humanity, and that fulfilling our ethical, human and social responsibilities is fundamental to maintaining the core of our culture. Always has been, always will be.

Read more
Expert Talk

FOR ETHICS IN AI, KEEP HUMANS in THE LOOP (UNTIL FURTHER NOTICE.)

As discussions around the advancement of AI evolve, so too do the concerns. And for good reason. Machines are getting smarter and smarter, able to complete complex tasks and process information incredibly fast. But, what happens when the developments in AI move too quickly — and we lose sight of human ethics?

Read more