How to control robot gripper without action manager for teleoperation

[mikelasa]

Hello,

I have been developing a simulation for robot teleoperation control using my own cartesian impedance controller and the spacemouse. The teleoperation example that isaac orbit and lab provides is using a registered gym environment and the manager based env which I dont have yet implemented and I found very difficult to understand how the commands are applied to the gripper.

So far, I control my robot by computing the joint torques and sending to the robot through set_joint_effort_target (I'm not using the manager system). I'm trying to open and close the gripper without success. I tried to use set_joint_velocity_target and set_joint_position_target but the only thing I see is a really tiny move in the left finger. Here is the code that handles the gripper commands, I dont know if the approach is the correct one but this is what achieved so far (I ommited some code for legibility):

# run simulation
def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene, teleop_interface: Se3SpaceMouse):
    robot1 = scene["robot1"]

    # create buffers to store actions 
    gripper_command_target = torch.zeros(args_cli.num_envs, 2, device=robot1.device)

    # robot parameterization (joint names and body names to use later)
    robot1_entity_cfg = SceneEntityCfg("robot1", joint_names=["panda_joint.*"], body_names=["panda_hand"])
    # robot fingers (gripper)
    robot1_entity_fingers = SceneEntityCfg("robot1", joint_names=["panda_finger_joint.*"], body_names=["panda_hand"])

    # resolve the scene entities
    robot1_entity_cfg.resolve(scene)
    robot1_entity_fingers.resolve(scene)

    while simulation_app.is_running():

        # define the target pose (right hand)
        robot1_teleop_command, gripper_command = teleop_interface.advance()
        robot1_teleop_command = robot1_teleop_command.astype("float32")

        # compute joint positions and velocities
        robot1_joint_pos = robot1.data.joint_pos[:, robot1_entity_cfg.joint_ids]
        robot1_joint_vel = robot1.data.joint_vel[:, robot1_entity_cfg.joint_ids]
        robot1_fingers_pos = robot1.data.joint_pos[:, robot1_entity_fingers.joint_ids]
        robot1_fingers_vel = robot1.data.joint_vel[:, robot1_entity_fingers.joint_ids]

        if gripper_command is True:
            # if gripper command is True, close gripper (-10N to first finger and 10N to second finger)
            gripper_command_target[:, 0] = robot1_fingers_vel[:, 0] - 1
            gripper_command_target[:, 1] = robot1_fingers_vel[:, 1] + 1
        else:
            # if gripper command is False, open gripper (10N to first and -10 second finger)
            gripper_command_target[:, 0] = robot1_fingers_vel[:, 0] + 1
            gripper_command_target[:, 1] = robot1_fingers_vel[:, 1] - 1

        # apply gripper command
        robot1.set_joint_velocity_target(gripper_command_target, joint_ids=robot1_entity_fingers.joint_ids)
        
        scene.write_data_to_sim()
        # perform step
        sim.step()
        sim.render()
        # update buffers
        scene.update(sim_dt)

Just in case, I'm not using the Manager based environment so far cause I have been working in my control development since orbit BaseEnv use.

Please, I would like to have some hints on how to do this.
Thanks in advance and regards

[Dhoeller19]

Hi @mikelasa,
Can you share more information on the robot and its config?

[mikelasa]

Sure!

I'm using franka model equivalent to the FRANKA_PANDA_CFG.:

robot1 = ArticulationCfg(
        prim_path="{ENV_REGEX_NS}/robot1",
        spawn=sim_utils.UsdFileCfg(
            usd_path="/home/mikel/Desktop/IsaacSimControl/src/orbit/source/extensions/omni.isaac.orbit_assets/data/panda.usd",
            activate_contact_sensors=False,
            rigid_props=sim_utils.RigidBodyPropertiesCfg(
                disable_gravity=True,
                max_depenetration_velocity=5.0,
            ),
            articulation_props=sim_utils.ArticulationRootPropertiesCfg(
                enabled_self_collisions=True,
                solver_position_iteration_count=8,
                solver_velocity_iteration_count=0,
                fix_root_link=True
            ),
        ),
        init_state=ArticulationCfg.InitialStateCfg(
            joint_pos={
                "panda_joint1": 0.0,
                "panda_joint2": -0.569,
                "panda_joint3": 0.0,
                "panda_joint4": -2.094,
                "panda_joint5": 0.0,
                "panda_joint6": 2.094,
                "panda_joint7": 0.741,
                "panda_finger_joint.*": 0.04,
            },
        ),
        actuators={
            "panda_shoulder": ImplicitActuatorCfg(
                joint_names_expr=["panda_joint[1-4]"],
                effort_limit=87.0,
                velocity_limit=2.175,
                stiffness=80.0,
                damping=4.0,
            ),
            "panda_forearm": ImplicitActuatorCfg(
                joint_names_expr=["panda_joint[5-7]"],
                effort_limit=12.0,
                velocity_limit=2.61,
                stiffness=80.0,
                damping=4.0,
            ),
            "panda_hand": ImplicitActuatorCfg(
                joint_names_expr=["panda_finger_joint.*"],
                effort_limit=200.0,
                velocity_limit=0.2,
                stiffness=2e3,
                damping=1e2,
            ),
        },
        soft_joint_pos_limit_factor=1.0,
    )
    robot1.actuators["panda_shoulder"].stiffness = 0
    robot1.actuators["panda_forearm"].stiffness = 0
    robot1.actuators["panda_shoulder"].damping = 0
    robot1.actuators["panda_forearm"].damping = 0
    robot1.init_state.pos = (0.0, 0.5, 0.0)
    robot1.init_state.rot = (0.0, 0.0, 0.0, 0.0)

Thanks for helping!

off topic:
I started implementing the ManagerBasedEnv and so far I would really appreciate a tutorial as the one with the cartpole "Creating a Manager-Based Base Environment" but using a franka robot. I'm using the teleop_se3_agent.pyas base which is based in a ManagerBasedRLEnv but is difficult to understand comparing to the cartpole example.

[xiaolijz]

Sure!

I'm using franka model equivalent to the FRANKA_PANDA_CFG.:

robot1 = ArticulationCfg(
        prim_path="{ENV_REGEX_NS}/robot1",
        spawn=sim_utils.UsdFileCfg(
            usd_path="/home/mikel/Desktop/IsaacSimControl/src/orbit/source/extensions/omni.isaac.orbit_assets/data/panda.usd",
            activate_contact_sensors=False,
            rigid_props=sim_utils.RigidBodyPropertiesCfg(
                disable_gravity=True,
                max_depenetration_velocity=5.0,
            ),
            articulation_props=sim_utils.ArticulationRootPropertiesCfg(
                enabled_self_collisions=True,
                solver_position_iteration_count=8,
                solver_velocity_iteration_count=0,
                fix_root_link=True
            ),
        ),
        init_state=ArticulationCfg.InitialStateCfg(
            joint_pos={
                "panda_joint1": 0.0,
                "panda_joint2": -0.569,
                "panda_joint3": 0.0,
                "panda_joint4": -2.094,
                "panda_joint5": 0.0,
                "panda_joint6": 2.094,
                "panda_joint7": 0.741,
                "panda_finger_joint.*": 0.04,
            },
        ),
        actuators={
            "panda_shoulder": ImplicitActuatorCfg(
                joint_names_expr=["panda_joint[1-4]"],
                effort_limit=87.0,
                velocity_limit=2.175,
                stiffness=80.0,
                damping=4.0,
            ),
            "panda_forearm": ImplicitActuatorCfg(
                joint_names_expr=["panda_joint[5-7]"],
                effort_limit=12.0,
                velocity_limit=2.61,
                stiffness=80.0,
                damping=4.0,
            ),
            "panda_hand": ImplicitActuatorCfg(
                joint_names_expr=["panda_finger_joint.*"],
                effort_limit=200.0,
                velocity_limit=0.2,
                stiffness=2e3,
                damping=1e2,
            ),
        },
        soft_joint_pos_limit_factor=1.0,
    )
    robot1.actuators["panda_shoulder"].stiffness = 0
    robot1.actuators["panda_forearm"].stiffness = 0
    robot1.actuators["panda_shoulder"].damping = 0
    robot1.actuators["panda_forearm"].damping = 0
    robot1.init_state.pos = (0.0, 0.5, 0.0)
    robot1.init_state.rot = (0.0, 0.0, 0.0, 0.0)

Thanks for helping!

off topic: I started implementing the ManagerBasedEnv and so far I would really appreciate a tutorial as the one with the cartpole "Creating a Manager-Based Base Environment" but using a franka robot. I'm using the teleop_se3_agent.pyas base which is based in a ManagerBasedRLEnv but is difficult to understand comparing to the cartpole example.

Hello, I'm encountering the same question. I really don't how to close the gripper, I apply the API-set_joint_position_target and I can see the gripper close in the scene, but the robot arm can't lift the object.
Looking forward to your advice.