Scaling Franka arm

[AndreaCastiella]

Hello, I'm trying to work with the Franka arm, but I need to scale it by 2. What’s happening is that if I leave the scale at 1 (as in the example shown in the documentation), everything works fine. But if I set the scale to 2 (so far I’m only updating the initial position as shown in the code), the arm collapses as if it doesn’t have enough strength. Which parameters should I change, and how/what values should I use, in order to scale the robot by 2 and keep the simulation stable?

import argparse
import numpy as np
import debugpy

def main():
    parser = argparse.ArgumentParser()
    parser.add_argument("-v", "--vis", action="store_true", default=False)
    parser.add_argument("-rs", "--robot_scale", type=float, default=2.0)
    args = parser.parse_args()

    ########################## init ##########################
    gs.init(backend=gs.gpu)

    ########################## create a scene ##########################

    scene = gs.Scene(
        sim_options=gs.options.SimOptions(
            dt=5e-3,
            substeps=30,
        ),
        viewer_options=gs.options.ViewerOptions(
            camera_pos=(3, -1, 1.5),
            camera_lookat=(0.0, 0.0, 0.0),
            camera_fov=30,
            max_FPS=60,
        ),
        rigid_options=gs.options.RigidOptions(
            box_box_detection=True,
        ),
        show_viewer=args.vis,
        vis_options=gs.options.VisOptions(
            visualize_mpm_boundary=True,
        ),
        mpm_options=gs.options.MPMOptions(
            lower_bound=(-1.0, -1.0, -1.13),
            upper_bound=(1, 1.0, 3),
        ),
    )

    ########################## entities ##########################
    plane = scene.add_entity(
        gs.morphs.Plane(pos=(0.0, 0.0, 0.0)),
    )
    
    # when loading an entity, you can specify its pose in the morph.
    franka = scene.add_entity(
        gs.morphs.MJCF(
            file  = 'xml/franka_emika_panda/panda.xml',
            pos=(0.0, 0.0, 0.0),
            scale=args.robot_scale,
        ),
    )

    ########################## Add camera ####################
    cam = scene.add_camera(
        res=(1280, 960),
        pos=(6, 0.0, 2.5),
        lookat=(0, 0, 0.5),
        fov=30,
        GUI=False
    )
    ########################## build ##########################
    scene.build()
    
    cam.start_recording()

    motors_dof = np.arange(7)
    fingers_dof = np.arange(7, 9)
    
    # Optional: set control gains
    franka.set_dofs_kp(
        np.array([4500, 4500, 3500, 3500, 2000, 2000, 2000, 100, 100]),
    )
    franka.set_dofs_kv(
        np.array([450, 450, 350, 350, 200, 200, 200, 10, 10]),
    )
    franka.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_effector = franka.get_link("hand")

    # move to pre-grasp pose. 
    qpos = franka.inverse_kinematics(
        link=end_effector,
        pos  = np.array([args.robot_scale*0.65, 0.0, args.robot_scale*0.25]),
        quat=np.array([0, 1, 0, 0]),
    )
    qpos[-2:] = 0.1
    franka.set_dofs_position(qpos[:-2], motors_dof)
    franka.set_dofs_position(qpos[-2:], fingers_dof)
    
    for i in range(300):
        franka.control_dofs_position(qpos[:-2], motors_dof)
        franka.control_dofs_position(qpos[-2:], fingers_dof)
        scene.step()
        cam.render()
    
    cam.stop_recording(save_to_filename="video.mp4", fps=60)


if __name__ == "__main__":
    main()

• scale 1

robot_escala_1.mp4

• scale 2

robot-arm-scale-2.mp4

[duburcqa]

You need to update these parameters:

    franka.set_dofs_kp(
        np.array([4500, 4500, 3500, 3500, 2000, 2000, 2000, 100, 100]),
    )
    franka.set_dofs_kv(
        np.array([450, 450, 350, 350, 200, 200, 200, 10, 10]),
    )
    franka.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]),
    )

Multiply all of them by scale ** 3.

[AndreaCastiella]

Thank you!

[AndreaCastiella]

One more question, it looks like the robot's fingers are shifted to the left (from our viewpoint), is this something that happens when scaling?

[duburcqa]

Can you do a side by side comparison? Can you make sure if the finger position is "zero"?

[AndreaCastiella]

Using the following code:

import argparse
import numpy as np
import genesis as gs

def main():
    parser = argparse.ArgumentParser()
    parser.add_argument("-v", "--vis", action="store_true", default=False)
    parser.add_argument("-rs", "--robot_scale", type=float, default=2.0)
    args = parser.parse_args()

    ########################## init ##########################
    gs.init(backend=gs.gpu)

    ########################## create a scene ##########################

    scene = gs.Scene(
        sim_options=gs.options.SimOptions(
            dt=5e-3,
            substeps=30,
        ),
        viewer_options=gs.options.ViewerOptions(
            camera_pos=(3, -1, 1.5),
            camera_lookat=(0.0, 0.0, 0.0),
            camera_fov=30,
            max_FPS=60,
        ),
        rigid_options=gs.options.RigidOptions(
            box_box_detection=True,
        ),
        show_viewer=args.vis,
        vis_options=gs.options.VisOptions(
            visualize_mpm_boundary=True,
        ),
        mpm_options=gs.options.MPMOptions(
            lower_bound=(-0.0, -1.0, -0.05),
            upper_bound=(2.3, 1.5, 3),
        ),
    )

    ########################## entities ##########################
    plane = scene.add_entity(
        gs.morphs.Plane(pos=(0.0, 0.0, 0.0)),
    )
    
    mpm_cube = scene.add_entity(
        material=gs.materials.MPM.Elastic(), 
        morph=gs.morphs.Box(
            pos=(args.robot_scale*0.65, args.robot_scale*0.25, args.robot_scale*0.05),
            size=(args.robot_scale*0.1, args.robot_scale*0.1, args.robot_scale*0.1),
        ),
        surface=gs.surfaces.Default(
            color=(0.0, 0.4, 0.4),
            vis_mode="particle",
        ),
    )
    cube = scene.add_entity(
        morph=gs.morphs.Box(
            pos=(args.robot_scale*0.65, args.robot_scale*0.0, args.robot_scale*0.025),
            size=(args.robot_scale*0.05, args.robot_scale*0.05, args.robot_scale*0.05),
        ),
        surface=gs.surfaces.Default(
            color=(1.0, 0.4, 0.4),
        ),
    )
    
    # when loading an entity, you can specify its pose in the morph.
    franka = scene.add_entity(
        gs.morphs.MJCF(
            file  = 'xml/franka_emika_panda/panda.xml',
            pos=(0.0, 0.0, 0.0),
            scale=args.robot_scale,
        ),
    )
    
    ########################## Add camera ####################
    cam = scene.add_camera(
        res=(1280, 960),
        pos=(6, 0.0, 2.5),
        lookat=(0, 0, 0.5),
        fov=30,
        GUI=False
    )
    ########################## build ##########################
    scene.build()
    
    cam.start_recording()
    
    ########## Prepare robot ##########
    # Get the dof indices of the motors and fingers
    motors_dof = np.arange(7)
    fingers_dof = np.arange(7, 9)
        
    # Optional: set control gains
    franka.set_dofs_kp(
        (args.robot_scale ** 3) * np.array([4500, 4500, 3500, 3500, 2000, 2000, 2000, 100, 100]),
    )
    franka.set_dofs_kv(
        (args.robot_scale ** 3) * np.array([450, 450, 350, 350, 200, 200, 200, 10, 10]),
    )
    franka.set_dofs_force_range(
        (args.robot_scale ** 3) * np.array([-87, -87, -87, -87, -12, -12, -12, -100, -100]),
        (args.robot_scale ** 3) * np.array([ 87,  87,  87,  87,  12,  12,  12,  100,  100]),
    )
    
    # Print fingers position
    print(franka.get_dofs_position(fingers_dof))

    # Get hand link
    end_effector = franka.get_link("hand")

    ########## Move to pre-grasp pose ##########
    # Set position of objects and robot
    franka_hand_zpos = 0.1084 * args.robot_scale # From specification of the robot 0.1084 is the distance between the fingertips and the center of the hand
    cube_pos = cube.get_pos().cpu().numpy()
    robot_z_up = 0.25 * args.robot_scale # z position when arm is up
    fingers_open = 0.07 * args.robot_scale # Open fingers position
    fingers_close = 0.0 * args.robot_scale  # Close fingers position
    
    # Get position of all the links of the robot so that we can move the end effector to the right position
    qpos = franka.inverse_kinematics(
        link=end_effector,
        pos  = np.array([cube_pos[0], cube_pos[1], robot_z_up]),
        quat=np.array([0, 1, 0, 0]),
    )
    # Set finger position open overwriting the last two values of qpos
    qpos[-2:] = fingers_open
    
    # Set the initial position of the robot
    franka.set_dofs_position(qpos[:-2], motors_dof)
    franka.set_dofs_position(qpos[-2:], fingers_dof)
    
    # Move the robot to the pre-grasp position
    for i in range(100):
        franka.control_dofs_position(qpos[:-2], motors_dof)
        franka.control_dofs_position(qpos[-2:], fingers_dof)
        scene.step()
        cam.render()
    
    ########## Grasp ##########
    qpos = franka.inverse_kinematics(
        link=end_effector,
        pos=np.array([cube_pos[0], cube_pos[1], franka_hand_zpos + cube_pos[2]]),
        quat=np.array([0, 1, 0, 0]),
    )
    qpos[-2:] = fingers_open

    for i in range(100):
        franka.control_dofs_position(qpos[:-2], motors_dof)
        franka.control_dofs_position(qpos[-2:], fingers_dof)
        scene.step()
        cam.render()
    
    finger_pos = fingers_close
    franka.control_dofs_position((np.array([finger_pos, finger_pos])), fingers_dof)

    for i in range(100):
        franka.control_dofs_position(qpos[:-2], motors_dof)
        franka.control_dofs_position(np.array([finger_pos, finger_pos]), fingers_dof)
        scene.step()
        cam.render()

    ########## Lift ##########
    for i in range(300):
        qpos = franka.inverse_kinematics(
            link=end_effector,
            pos=np.array([cube_pos[0], cube_pos[1], franka_hand_zpos + cube_pos[2] + (args.robot_scale * 0.0005) * i]),
            quat=np.array([0, 1, 0, 0]),
        )
        franka.control_dofs_position(qpos[:-2], motors_dof)
        franka.control_dofs_position((np.array([finger_pos, finger_pos])), fingers_dof)
        scene.step()
        cam.render()
        
    ########## Move arm to the right ##########
    # Get arm position
    hand_pos = end_effector.get_pos().cpu().numpy()
    
    # Fix the grip 
    grip_qpos = franka.get_dofs_position(fingers_dof)
    franka.control_dofs_position(grip_qpos, fingers_dof)
    
    # Get the mean position of the mpm simulated object
    mpm_pos_all_particles = mpm_cube.get_particles()
    mpm_mean_pos = np.mean(mpm_pos_all_particles, axis=0)    
    
    qpos = franka.inverse_kinematics(
        link=end_effector,
        pos=np.array([hand_pos[0], mpm_mean_pos[1], hand_pos[2]]),
        quat=np.array([0, 1, 0, 0]),
    )
    for i in range(100):
        franka.control_dofs_position(qpos[:-2], motors_dof)
        franka.control_dofs_position((np.array([finger_pos, finger_pos])), fingers_dof)
        scene.step()
        cam.render()
        
    # Wait a bit
    for i in range(100):
        scene.step()
        cam.render()
        
    ########## Let the object fall ##########
    qpos[-2:] = fingers_open
    motors_qpos = franka.get_dofs_position(motors_dof)
    
    for i in range(300):
        franka.control_dofs_position(qpos[-2:], fingers_dof)
        # Fix the motors
        franka.control_dofs_position(motors_qpos, motors_dof)
        scene.step()
        cam.render()
    
    # Stop recording and save video. 
    cam.stop_recording(save_to_filename="output-video.mp4", fps=60)

if __name__ == "__main__":
    main()

I've seen that:

• The position of the fingers after building the scene is always: print(franka.get_dofs_position(fingers_dof)) tensor([0., 0.], device=‘cuda:0’). The position after setting them to 0.07args.robot_scale is 0.07args.robot_scale for both fingers. However, they seem to be shifted to the left as seen in the videos with scale=2.
• If I multiply the control gains by scale ** 3 the output is not what I expected. I have tried multiplying it by scale ** 4 and it seems to work better (using scale=2).

Videos:

• Scale 1
  https://github.com/user-attachments/assets/c38cf872-68c5-4276-92da-aa8891a56008
• Scale 2, control gains * scale **3
  https://github.com/user-attachments/assets/799519c1-1296-45e0-a114-ec92a24e1662
• Scale 2, control gains * scale **4
  https://github.com/user-attachments/assets/7a00ee81-6d3c-4f51-b193-80e54e297ffe

[duburcqa]

Sorry for the delay. I will have a look right now.

[duburcqa]

There is at least one issue about the dofs limits not being updated when scaling the robot. I need to fix this.

[duburcqa]

I found the root cause of the issue. Trying to fix it.

[duburcqa]

Here it is ! Sorry for the delay: #1039

[AndreaCastiella]

Thank you so much! And don’t worry that was a super quick response! Really appreciate you taking the time to look into it :)