Gripper not gripping properly?

[Dassoo]

Hi everyone, I was testing the actual gripper capabilities by trying to lift a plate using a Franka robotic arm and its gripper, but the behaviour isn't the expected one. Once the gripper closes, it's almost like the weight of the plate forcefully opens the gripper, letting it slide off.

Note: I'm not trying to lift from the top as in the provided example in the docs, but from one side.

I may have forgot something, but I'm not sure where and why. I hope someone can help, thanks in advance!

Here's the actual code:

def main():
gs.init(backend=gs.cuda) # engine

############################### SCENE ####################################

scene = gs.Scene(
    sim_options=gs.options.SimOptions(
        dt=0.01,
        substeps=6
    ),
    coupler_options=gs.options.CouplerOptions(rigid_pbd=True, ),
    rigid_options=gs.options.RigidOptions(enable_collision=True, ),
    pbd_options=gs.options.PBDOptions(
        particle_size=0.01,
        gravity=(0, 0, -9.8),
    ),
    viewer_options=gs.options.ViewerOptions(
        camera_fov=50,
        res=(1920, 1080),
        max_FPS=60,
    ),
    show_viewer=True,
)

############################# ENTITIES ##################################

# PLANE
plane = scene.add_entity(
    morph=gs.morphs.Plane(
        pos=(0.0, 0.0, 0.0),
    ),
    surface=gs.surfaces.Aluminium(
        ior=10.0,
    )
)

# TABLE LEGS TRAY ROBOT
table1 = scene.add_entity(
    morph=gs.morphs.Box(
        pos=(0.0, 0.0, 0.2425),
        size=(1.2, 0.6, 0.485),
        euler=(0.0, 0.0, 90.0),
        fixed=True
    ),
    surface=gs.surfaces.Aluminium(
        color=(0.2, 0.2, 0.2),
    )
)

# TRAY TABLE
table3 = scene.add_entity(
    morph=gs.morphs.Box(
        pos=(1.1, 0.0, 0.5),
        size=(0.3, 0.3, 1.0),
        euler=(0.0, 0.0, 90.0),
        fixed=True
    ),
    surface=gs.surfaces.Aluminium(
        color=(0.2, 0.2, 0.2),
    )
)

# TRAY
tray = scene.add_entity(
    morph=gs.morphs.Box(
        pos=(1.0, 0.0, 1.005),
        size=(0.3, 0.6, 0.005),
        euler=(0.0, 0.0, 0.0),
        # fixed=True
    ),
    surface=gs.surfaces.Glass(

    ),
)

# TRAY ROBOT
franka1 = scene.add_entity(
    morph=gs.morphs.MJCF(
        file='xml/franka_emika_panda/panda.xml',
        pos=(0.05, 0.0, 0.505)
    ),
    material=gs.materials.Rigid(coup_friction=4.0)
)

scene.build()

############################ EXECUTION #################################

jnt_names = [
    'joint1',
    'joint2',
    'joint3',
    'joint4',
    'joint5',
    'joint6',
    'joint7',
    'finger_joint1',
    'finger_joint2',
]
dofs_idx = [franka1.get_joint(name).dof_idx_local for name in jnt_names]

motors_dof = np.arange(7)
fingers_dof = np.arange(7, 9)

# set control gains
franka1.set_dofs_kp(
    np.array([4500, 4500, 3500, 3500, 2000, 2000, 2000, 100, 100]),
)
franka1.set_dofs_kv(
    np.array([450, 450, 350, 350, 200, 200, 200, 10, 10]),
)
franka1.set_dofs_force_range(
    np.array([-87, -87, -87, -87, -12, -12, -12, -100, -100]),
    np.array([87, 87, 87, 87, 12, 12, 12, 100, 100]),
)


end_effector1 = franka1.get_link('hand')

# ready pose
qpos = franka1.inverse_kinematics(
    link=end_effector1,
    pos=np.array([0.7, 0.0, 1.01]),
    quat = np.array([0.5, 0.5, 0.5, 0.5])
)

# gripper open
qpos[-2:] = 0.04

path = franka1.plan_path(
    qpos_goal=qpos,
    num_waypoints=200,  # 2s duration
)
for waypoint in path:
    franka1.control_dofs_position(waypoint)
    scene.step()
for i in range(100):
    scene.step()

# move to pre-grasp pose
qpos = franka1.inverse_kinematics(
    link=end_effector1,
    pos=np.array([0.77, 0.0, 1.01]),
    quat = np.array([0.5, 0.5, 0.5, 0.5])

)
path = franka1.plan_path(
    qpos_goal=qpos,
    num_waypoints=200,
)
for waypoint in path:
    franka1.control_dofs_position(waypoint)
    scene.step()
for i in range(100):
    scene.step()

# grasp
franka1.control_dofs_position(qpos[:-2], motors_dof)
franka1.control_dofs_force(np.array([-0.5, -0.5]), fingers_dof)
for i in range(100):
    scene.step()

# move with tray
qpos = franka1.inverse_kinematics(
        link=end_effector1,
        pos=np.array([0.77, 0.0, 1.2]),
        quat=np.array([0.5, 0.5, 0.5, 0.5])

)
path = franka1.plan_path(
    qpos_goal=qpos,
    num_waypoints=200,
    ignore_collision=True,
    ignore_joint_limit=False
)
for waypoint in path:
    franka1.control_dofs_position(waypoint, dofs_idx)
    scene.step()
for i in range(200):
    scene.step()

[milong26]

Maybe you need to give it more force in
franka1.control_dofs_force(np.array([-0.5, -0.5]), fingers_dof)

SInce you don't set dofs of grippers after this code, it should keep grasping, becoming closed when your plate flying away

Btw, I met this problem too, the right pos and quat you want to do ik is important too. :)

[Dassoo]

Yeah I tried, but unfortunately increasing the force doesn't solve the problem. Not putting enough force will cause the plate to slide off the gripper, while putting too much force (anything lower than [-1, -1]) will cause the plate to... fly away.

The pos and quats I've been using should be quite simple and I'm actually just trying to lift the plate vertically by some centimeters without even changing orientation while doing so.

Did you manage to solve your problem or are we all waiting for something?

[milong26]

I know what happened... In your code moving

# move with tray
qpos = franka1.inverse_kinematics(
        link=end_effector1,
        pos=np.array([0.77, 0.0, 1.2]),
        quat=np.array([0.5, 0.5, 0.5, 0.5])

)
path = franka1.plan_path(
    qpos_goal=qpos,
    num_waypoints=200,
    ignore_collision=True,
    ignore_joint_limit=False
)
for waypoint in path:
    franka1.control_dofs_position(waypoint, dofs_idx)
    scene.step()

Your qpos[:-2] will be changed and the grippers were open while they should be closed.

The tourial gives example Inverse Kinematics & Motion Planning

[milong26]

I thought it was the problem of force or position, then I try to add force but it always slided down xD

If you want to add force since you say add force=fly away, you can try this:

1. set ready pose a bit farther, since i saw a little collision during this
2. set small force when grasp,like [-1,-1] make sure it can grasp it correctly (My grippers hit the plate when force too large)
3. franka1.control_dofs_force(np.array([-20, -20]), fingers_dof) after step2,this works well since your franka has grasped the plate well
4. then move.

I have one problem,will surface work? I haven't noticed it before,is there any friction on it?

[Dassoo]

Ok, that was a very silly mistake.

I appreciate your input since at least now the tray is not flying away, but as you suggested I was just changing the previously set state of the gripper, which was opening when lifting up. I was basically using all dofs instead of just the motor ones, removing now as well the "faulty" gripping positions from the planned waypoints.

# grasp
franka1.control_dofs_position(qpos[:-2], motors_dof)
franka1.control_dofs_force(np.array([-1, -1]), fingers_dof)
for i in range(100):
    scene.step()
franka1.control_dofs_force(np.array([-20, -20]), fingers_dof)
for i in range(100):
    scene.step()

# move tray
qpos = franka1.inverse_kinematics(
        link=end_effector1,
        pos=np.array([0.77, 0.0, 1.2]),
        quat=np.array([0.5, 0.5, 0.5, 0.5])

)

path = franka1.plan_path(
    qpos_goal=qpos,
    num_waypoints=200,
    ignore_collision=True,
    ignore_joint_limit=False
)
for waypoint in path:
    franka1.control_dofs_position(waypoint[:-2], motors_dof)
    scene.step()
for i in range(200):
    scene.step()