Franka PickBlockApproach

Hello, I’m trying to customize the picking scenario from the simple_stack.py. I want to pick up a different object, this one chiave_combinata_8_10.zip (68.6 KB).

I wanted to understand a few things:

  1. First of all in the PickBlockApproach class the robot arm is rotated based on the three values axis_x, axis_y, axis_z, which are specifically calculated for the cube orientation. Each axis is expressed with a 3d vector, so how can I modify them using the x,y,z angles rotations?

  2. There are two hardcoded variables (block_height and grasp_depth) in the Domain class used then to create the PickInfo object. I wanted to modify those variables to tell the robot at what height and with which grasp depth picking the object, but it isn’t working? Do I need to modify anything else?

  3. How can I get x,y,z dimensions for an asset?

Thanks in advance

Hi,

Question 1: I am conducting some tests with RMP Extension. This extension handles the motion and pose of the robot through goLocal method in Franka sample. So, I do not have a clear response to this question yet, sorry :(

Question 2: Besides the variables block_height and grasp_depth, maybe you can check and edit some default values that are also related to the control of the gripper.

  • The default values in method Gripper.open(...) in /isaac-sim/_build/linux-x86_64/release/exts/omni.isaac.samples/omni/isaac/samples/scripts/utils/franka.py line 57. This method also sets the self.width when it calls Gripper.move(...) internally
  • The value of width on franka.end_effector.gripper.move(width=0.025, speed=0.2, wait=False). The path of the file is /isaac-sim/_build/linux-x86_64/release/exts/omni.isaac.samples/omni/isaac/samples/scripts/utils/behavior_helpers.py in line 87.

Question 3: An approach to get the x, y, z dimensions of an asset can be to get the dimensions of its bounding box (probably this is not the best solution for that problem, but it works)

The next code can be help:

import omni
from pxr import UsdGeom

stage = omni.usd.get_context().get_stage()

path = "/environments/env/Blocks/block_01"

prim = stage.GetPrimAtPath(path)

imageable = UsdGeom.Imageable(prim)
bounds = imageable.ComputeLocalBound(0.0, "default")
box = bounds.GetBox()

print(box.GetMax()-box.GetMin())
print(box.GetSize())

Where bound is a BBox3d object to manage “Arbitrarily oriented 3D bounding box” and box is the bounding box (Range3d object). Then, you can get the x, y, z dimensions of the bounding box (in centimeters, I think) with GetSize(), or with the difference between GetMax() and GetMin()

Hello and thanks again for the reply,

Question 1: The three variables axis_x, axis_y and axis_z are represented each one as a 3d vector, based on the x,y,z object rotation angles. I just don’t get how this angle axis are represented (cos, sin…?)

Question 2: Ok for the other two variables, but I still have the following problem: the arm slightly lift while closing, so it does not take the object. How can I tell him to wait until full closing, then start lifting?

Question 3: Thanks for the suggestion ;)

Hi…

Question 2: It seems that the problem is the transition time between states within the implemented state machine. To avoid the arm slightly lift while closing put a time delay (time.sleep) inside PickBlockApproach.transition method before transitioning to the next state:

/isaac-sim/_build/linux-x86_64/release/exts/omni.isaac.samples/omni/isaac/samples/scripts/utils/behavior_states.py line 1284

def transition(self):
    if self.is_finished:
        import time
        time.sleep(2)
        return self.next_state
    else:
        return self

In this way it just waits before closing while slightly lifting, but anyway I could augment the object z-scale (which was too thin) and now everything is ok, so question 2 is solved.

Just waiting for any information about axis-angle representation and I’m done, thanks ;)

According to my understanding and my tests, the axis_x, axis_y, axis_z are the first, second, and third column of the rotation matrix, like the next image:

To get the axis_x, axis_y, axis_z for a specific rotation represented as quaternion this code can be help:

from pxr import Gf

quaternion = [ 0, 0.7071068, 0, 0.7071068 ]

gf_quaternion = Gf.Quatf(quaternion[3], Gf.Vec3f(quaternion[0], quaternion[1], quaternion[2]))
mat = Gf.Matrix3d(gf_quaternion).GetTranspose()

axis_x=[mat.GetColumn(0)[0], mat.GetColumn(0)[1], mat.GetColumn(0)[2]]
axis_y=[mat.GetColumn(1)[0], mat.GetColumn(1)[1], mat.GetColumn(1)[2]]
axis_z=[mat.GetColumn(2)[0], mat.GetColumn(2)[1], mat.GetColumn(2)[2]]

Ok, I solved it, your code has been helpful, thank you very much for your time ;)

Hi, I am glad to hear about that…

Btw, I made the next image (before seeing your last reply) that helps to understand that, the rotation angles (rotation matrix, quaternion, Euler angles, etc.) of the end effector are always relative to its initial reference. So, it is necessary to compute the final rotation between different reference systems…

Note: reference (red) is just for end effector rotation angles (the position in those examples are arbitrary, position use another reference system)

I wanted to add one last question. After the picking I need to lift the arm before going to the destination. In the PickBlockLift it says that the arm slightly lifts after the picking, but how can I control how much lifting the arm?

Hi…

It is necessary to change 2 things on the PickBlockLift class (/isaac-sim/_build/linux-x86_64/release/exts/omni.isaac.samples/omni/isaac/samples/scripts/utils/behavior_states.py) to control the lifting of the arm.

  • Position in axis Z

    The position of the arm after the picking is defined in the line 1308. You need to modify the third element of the NumPy array to set a new elevation.

    self.target = {"orig": self.pick_info.target_orig + np.array([0.0, 0.0, 0.025])}
    
  • Interval of time to execute the lifting

    Additionally, it is necessary to modify (to increase if you want to reach a higher position) the transition time to allow the arm to reach the new target (the self.max_wait_time defined in the constructor of the class, line 1303)

    Actually, this time is too short (250ms) to reach a bigger lift and the robot will be going to the new state without complete the newly defined elevation

    You can modify the time setting a new default value in __init__ method, line 1300

    def __init__(self, domain, pick_info, max_wait_time=0.25):
    

Note: it is possible that the line numeration I wrote doesn’t coincide with the original code, maybe it is because old modification of the code during some test, I cannot remember 😅

Again, Thank you very much for your help ;)

👍 …