Implementing Zero Gravity Mode for Jaco Arms follow up to Issue #34

[jdchang1]

Hi,

I hope you are well. I am currently trying to implement the Zero G mode described in issue #34, and I have a few questions.

The first step that was mentioned was to set the gravity vector in the correct orientation using SetGravityVector. I have located this function; however, what should the arguments to this function be for the correct orientation? Moreover for the wrapper, currently I am planning on adding to the movo_joint_interface/kinova_api_wrapper.py by creating a self.setGravityVector =self.kinova.Ethernet_SetGravityVector but what would have to go into the setGravityVector.argtypes?

The second step that was mentioned was to activate the Reactive force control mode by calling SetForceControl; however, I did not find this function. The closest functions I found were StartForceContorl and StopForceControl. How would I go about completing this step of the process?

Finally, where in the pipeline of the system would a script that calls these functions belong? More specifically, after updating the wrapper to include the necessary functions, could I just run a script that calls these functions to activate zero g mode?

Thank you very much for your help!

Best,
Jonathan

[martine1406]

Hi Jonathan

The arguments to SetGravityVector should be the gravity vector. On movo, the gravity (downward) is oriented in the -Y direction with respect to the Kinova arm base frame. So you would call SetGravityVector (float gravityVector[GRAVITY_VECTOR_SIZE]), with gravityVector=[0.0, -9.81, 0.0].

setGravityVector.argtypes should be [POINTER(c_float)]

SetForceControl is an error. Sorry about that... I was writing too fast...I ments Start instead of Set! You start the Reactive Force control with StartForceControl and stop it with StopForceControl. Again, you will have to add those functions to kinova_api_wrapper.py

The specific implementation is your choice. Either you modify an existing node (e.g. jaco_joint_controller.py, or you create your own. As a first step, you could do your own script to call to those functions, then you<ll see if you want to join this code as an extra mode in an existing node (e.g. declare specific services to start and stop the reactive force control, and then have a client that calls to this service).

I hope this helps
Martine

[jdchang1]

Hi Martine

So I am currently in the process of implementing it, and I am running into some problems. I have added to the Kinova API wrapper and have also added to the set_control_mode method so that the parameter 2 tries to do StartForceControl. Currently I am trying to initialize the KinovaAPI like the following:

Left Arm
api = KinovaAPI('left','eth0',"10.66.171.16",'255.255.255.0',24000,24024,44000, "7dof")
Right Arm
api = KinovaAPI('right','eth0',"10.66.171.15",'255.255.255.0',25000,25025,55000, "7dof")

When I try to run this, I am getting the error:
[ERROR] [WallTime: 1533752146.166996] Init API result: 1
[ERROR] [WallTime: 1533752146.167990] GetDevices result: 1
[ERROR] [WallTime: 1533752146.168753] Number of arms: 0
[ERROR] [WallTime: 1533752146.169534] Initialization failed, could not find Kinova devices (see Kinova.API.CommLayerUbuntu.h for details)

What is causing this error? Thank you!

Jonathan

[martine1406]

Hmm okay. well those lines are already called in jaco_joint_controller.py . Since jaco_joint_controller.py is called when the movo initializes, maybe there is a conflict between your code and this code. Can you add your set_control_mode to jaco_joint_controller.py ?

[jdchang1]

So we have added code to kinova_api_wrapper.py that implements StartForceControl, StopForceControl, and SetGravityVector. We added code in jaco_joint_controller.py that calls StartForceControl and commented out the line that turns on CartesianControl, which is chosen by default.

Code in kinova_api_wrapper.py

self.StartForceControl = self.kinova.Ethernet_StartForceControl
self.StopForceControl = self.kinova.Ethernet_StopForceControl
self.SetGravityVector = self.kinova.Ethernet_SetGravityVector
self.SetGravityVector.argtypes = [POINTER(c_float)]
...
def set_control_mode(self,mode):
        if (AUTONOMOUS_CONTROL == mode):
            self.SetAngularControl()
        elif (TELEOP_CONTROL == mode):
            self.SetCartesianControl()
        elif (FORCE_CONTROL == mode):
            # code for zero-g
            gravityVectorArray = c_float * 3
            gravityVector = gravityVectorArray(0.0, -9.81, 0.0)
            self.SetGravityVector(byref(gravityVector))
            self.StartForceControl()
	    rospy.loginfo("INFO: In zero-g mode")
        else:
            rospy.logerr("ERROR: Invalid contol mode")

code in jaco_joint_controller.py

#self.api.SetCartesianControl()
self.api.set_control_mode(2)

Upon reboot, the robot does the startup sequence, but does not enter ForceControlMode. Any advice would be appreciated. Ideally we would want to be able to switch ForceControl on and off at runtime, and not need to reboot to change modes.

[martine1406]

Have you modified the code on movo1 and movo2? Both computers have to run the same code...
I do not have any other insight right now, but I'll let you know if I think of something else.