Protection Zone is ignored by Pose Action Server

[gbcarlos]

Hi,

I am working on a JACO2 Arm Gen2 and have been using the Kinova API to set up a protection zone as stated in the API documentation within the SDK.

The arm is entirely working and does what it should apart from the Protection Zone that is being ignored. AFAIK the protection zone only prevents pose goals within the zone, not joint goals. Is only the end-effector prevented from entering the zone or the entire arm?

In addition, I have configured the zone limitations for speed within the zone to zero, that should stop the arm when the zone is entered.

What I do now to test if the zone is respected:

1. Bring up the arm using roslaunch kinova_bringup kinova_robot.launch
2. In a new Terminal run my program that sets up the protection zone (similar to the example in the API docs, SetProtectionZone)
   • This works and it is shown that the protection zone has been set up properly, however after the program is down it results in an error within the bringup terminal, which shows a kinovaComm error 1015 which I found out has to do something with the connection. This might be due to the static custom end-effector and the missing finger information.
3. However after bringing up the arm again and check for protection zones it is still shown. That's why I assume the zone is still up.
4. Now when I manually command a pose goal to the /j2s6s300_driver/pose_action/tool_pose/goal topic that is within the protection zone, the arm happily moves inside.

What am I missing?

Edit: I am connected via USB and the topics stop being visible as soon as I execute the API program.

[felixmaisonneuve]

Hi @gbcarlos,

Protection zones work only for cartesian movements (e.g. moving the arm with the joystick in Cartesian Mode).

You set the zone velocity to 0, which is good, but can you on how you created your protection zone. What are all the parameters you used?

About your connection error, this is normal. You cannot run ROS at the same time as DevelopmentCenter/programs using the API. You should start directly by running your script, then, after making sure it is not running anymore and DevelopmentCenter is closed, run your roslaunch command.

I would also first change your script to clear every protection zones in your robot. Make sure you don't see any one left. After you are sure there is no protection zone set, you can run your script to add one and check again if you can see it. I think what you are doing is correct, but we want to be sure you see your new protection zone and not an old one.

However, I am pretty sure your problem does not come from the protection zone creation, but from the protection zone parameters. Maybe it is not set a the correct position or the correct orientation.

It might be a good test to move the arm around with the joystick once you create your protection zone to see if there is a zone where the arm can't go. Maybe it is simply not where you think it is.

Best, Felix

[gbcarlos]

Hi @felixmaisonneuve,

Thank you for your support. When I print out the current zone saved inside my robot, the one I defined is printed out and it is the only one, so this should not be the issue.

Moving the arm around in translation mode also didn't bring me further, there is no protection zone where the arm stops at all. In general I want the robot to not drop under a certain z value so I defined the zone as following:

`ZoneList zones; zones.NbZones = 1;

    zones.Zones[0].zoneShape.shapeType = PrismSquareBase_Z;
    zones.Zones[0].zoneLimitation.speedParameter1 = 0.0f;
    zones.Zones[0].zoneLimitation.speedParameter2 = 0.0f;
    zones.Zones[0].zoneLimitation.speedParameter3 = 0.0f;

    zones.Zones[0].zoneShape.Points[0].X = 0.5f;
    zones.Zones[0].zoneShape.Points[0].Y = 0.5f;
    zones.Zones[0].zoneShape.Points[0].Z = 0.1f;

    zones.Zones[0].zoneShape.Points[1].X = 0.5f;
    zones.Zones[0].zoneShape.Points[1].Y = -0.5f;
    zones.Zones[0].zoneShape.Points[1].Z = 0.1f;

    zones.Zones[0].zoneShape.Points[2].X = -0.5f;
    zones.Zones[0].zoneShape.Points[2].Y = -0.5f;
    zones.Zones[0].zoneShape.Points[2].Z = 0.1f;

    zones.Zones[0].zoneShape.Points[3].X = -0.5f;
    zones.Zones[0].zoneShape.Points[3].Y = 0.5f;
    zones.Zones[0].zoneShape.Points[3].Z = 0.1f;

    zones.Zones[0].zoneShape.Points[4].Z = 0.5f;`

From my understanding this should result in a box where the arm is located in the middle and the space between 0.1m and 0.5m should not be accessible. Hope this makes it a little clearer.

[felixmaisonneuve]

I have compared your zone with the description in the documentation. Your points seems to be defined in the correct order, however, your points are at the bottom of the prism and, according to the documentation, they need to be at the top of the prism.

You could try to change your z values to something like :

zones.Zones[0].zoneShape.Points[0].Z = 0.6f;
zones.Zones[0].zoneShape.Points[1].Z = 0.6f;
zones.Zones[0].zoneShape.Points[2].Z = 0.6f;
zones.Zones[0].zoneShape.Points[3].Z = 0.6f;
zones.Zones[0].zoneShape.Points[4].Z = 0.5f;

Your z value for Points[4] being the height of the prism I think you could achieve something similar by changing

zones.Zones[0].zoneShape.Points[4].Z = 0.5f;

to

zones.Zones[0].zoneShape.Points[4].Z = -0.5f;

but I am not sure how the code will handle a negative height.

Try this and let me know how it goes

Best, Felix

[gbcarlos]

After changing the zone as you suggested it looks like this:

    ZoneList zones;
    zones.NbZones = 1;

    zones.Zones[0].zoneShape.shapeType = PrismSquareBase_Z;
    zones.Zones[0].zoneLimitation.speedParameter1 = 0.0f;
    zones.Zones[0].zoneLimitation.speedParameter2 = 0.0f;
    zones.Zones[0].zoneLimitation.speedParameter3 = 0.0f;

    zones.Zones[0].zoneShape.Points[0].X = 0.5f;
    zones.Zones[0].zoneShape.Points[0].Y = 0.5f;
    zones.Zones[0].zoneShape.Points[0].Z = 0.5f;

    zones.Zones[0].zoneShape.Points[1].X = 0.5f;
    zones.Zones[0].zoneShape.Points[1].Y = -0.5f;
    zones.Zones[0].zoneShape.Points[1].Z = 0.5f;

    zones.Zones[0].zoneShape.Points[2].X = -0.5f;
    zones.Zones[0].zoneShape.Points[2].Y = -0.5f;
    zones.Zones[0].zoneShape.Points[2].Z = 0.5f;

    zones.Zones[0].zoneShape.Points[3].X = -0.5f;
    zones.Zones[0].zoneShape.Points[3].Y = 0.5;//0.5f;
    zones.Zones[0].zoneShape.Points[3].Z = 0.5f;

    zones.Zones[0].zoneShape.Points[4].Z = 0.5f;

And still, driving around manually or giving pose goals there is no protection zone in the way. I can read out the zone parameters of the set zone and see that they are correct, I have turned the limitations on so that they apply and yet, when I move the arm into the defined zone and read out the cartesian position using the API functions, I can see that the zone has not been respected.

I am using a custom end effector so the finger positions are always zero might that be an issue?

Any other suggestions?

Btw. using firmware 6.2.10

Kind Regards,

Carlos

[felixmaisonneuve]

Hi @gbcarlos,

I managed to make your protection zone work. I retain three things from my testing :

1. Points order for your zoneShape is very important
2. The order in the documentation image (that I copied last week) is incorrect, but the order in the example is correct
3. You need to reboot the arm for the protection zone to be effective

About 2, if you compare the points order in the example (in the documntation) and the one you use (that corresponds to the image order), you get this

The points from the domentation example are in blue, and, in red, your points. We see the order is different.

I swapped points 0<->2 and 1<->3 to get this :

        ZoneList zones;
        zones.NbZones = 1;
        zones.Zones[0].zoneShape.shapeType = PrismSquareBase_Z;
        zones.Zones[0].zoneLimitation.speedParameter1 = 0.0f;
        zones.Zones[0].zoneLimitation.speedParameter2 = 0.0f;
        zones.Zones[0].zoneLimitation.speedParameter3 = 0.0f;
        zones.Zones[0].zoneShape.Points[0].X = -0.5f;
        zones.Zones[0].zoneShape.Points[0].Y = -0.5f;
        zones.Zones[0].zoneShape.Points[0].Z = 0.5f;
        zones.Zones[0].zoneShape.Points[1].X = -0.5f;
        zones.Zones[0].zoneShape.Points[1].Y = 0.5f;
        zones.Zones[0].zoneShape.Points[1].Z = 0.5f;
        zones.Zones[0].zoneShape.Points[2].X = 0.5f;
        zones.Zones[0].zoneShape.Points[2].Y = 0.5f;
        zones.Zones[0].zoneShape.Points[2].Z = 0.5f;
        zones.Zones[0].zoneShape.Points[3].X = 0.5f;
        zones.Zones[0].zoneShape.Points[3].Y = -0.5;//0.5f;
        zones.Zones[0].zoneShape.Points[3].Z = 0.5f;
        zones.Zones[0].zoneShape.Points[4].Z = 0.5f;

After that, you reboot your arm. This is very important. Even if the zone is correctly set when calling GetProtectionZone, the zone will not work until you reboot the arm

This should fix your issue Regards, Felix

[gbcarlos]

Hi @felixmaisonneuve,

Thanks I'm finally seeing my protection zone. However there seems to be something wrong with the z value of the plane and dZ. In my case I changed the height of the plane (all 4 points) to 0.1 and dZ stayed 0.5. Since the plane is meant to be on top I figured that I would define the space below as the protection zone, but instead it seemed to be the space on top. Changed it now to 0.0 for the 4 points and dz = 0.1, now it works fine.

Thank you for the support, this can be closed but maybe you want to check the mentioned issue out.

Kind Regards,

Carlos

[felixmaisonneuve]

Thank you for this inormation, I have seen (I went over it very quickly) how the protection zone is implemented and it is very dependant on the values you put in each point and the order of the points. I might give it a try and check if the dz values goes up or down (might also be dependant on the Z values of your points)

For sure, you might have to do some trial and error to get the exact zone you want.

I am glad you found a combinaison that works for you I will close this issue

Regards, Felix