Franka arm not moving at all after applying an direct torque command using ArticulationAction

Hi,

I am just trying to figure out how everything works, and I noticed that if I change the pick_place.py example to

from omni.isaac.kit import SimulationApp

simulation_app = SimulationApp({"headless": False})

from omni.isaac.franka.tasks import PickPlace
from omni.isaac.franka.controllers import PickPlaceController
from omni.isaac.core import World
from omni.isaac.core.utils.types import ArticulationAction
from omni.isaac.core import SimulationContext
import numpy as np

my_world = World(stage_units_in_meters=1.0)
my_task = PickPlace()
my_world.add_task(my_task)
simulation_context = SimulationContext()
my_world.reset()
task_params = my_task.get_params()
my_franka = my_world.scene.get_object(task_params["robot_name"]["value"])

i = 0
while simulation_app.is_running():
    my_world.step(render=True)
    if my_world.is_playing():
        if my_world.current_time_step_index == 0:
            my_world.reset()
            my_controller.reset()
        observations = my_world.get_observations()
        if my_controller.is_done():
            print("done picking and placing")
        
        t = simulation_context.current_time
        actions = ArticulationAction(joint_positions=None, joint_velocities=None, joint_efforts=np.array([np.sin(t)] + [0.0] * 8))
        my_franka.apply_action(actions)
simulation_app.close()

The robot arm would not move at all, and it seems like gravity is not effecting it since it is not falling to the ground, it just stays there. I don’t understand why this is the case and how to make it follow my joint effort command. Can someone help me understand this?

I was able to make it move by adding the lines

articulation_controller = my_franka.get_articulation_controller()
articulation_controller.set_effort_modes("force")
articulation_controller.switch_control_mode("effort")

and applying the control via

q = my_franka.get_joint_positions()
dq = my_franka.get_joint_velocities()

tau = 0.01 * (q_des - q[:7]) - 0.03 * dq[:7]
tau = np.concatenate([tau, np.zeros(2)])

articulation_controller.apply_action(actions)

Then the robot starts to move toward the target but keeps doing small oscillations. Does anyone know why that happens?

Thanks in advance!

Hi @bolun - The robot arm not moving and not being affected by gravity could be due to the physics simulation not being properly initialized or the robot being in a fixed state.

In your code, you are creating a PickPlace task and adding it to the world, but you are not initializing the PickPlaceController which is responsible for controlling the robot’s movements.

The PickPlaceController needs to be initialized with the robot’s gripper and articulation. This is done in the setup_post_load method in the original pick_place.py example.

The oscillations you are observing when the robot moves towards the target could be due to the control gains being too high, causing the robot to overshoot the target and oscillate around it. You could try reducing the control gains to see if that helps.

Also, make sure that the joint_efforts you are applying are within the robot’s limits. Applying too high or too low efforts can cause unstable movements.

Here is a modified version of your code that initializes the PickPlaceController:

from omni.isaac.kit import SimulationApp
from omni.isaac.franka.tasks import PickPlace
from omni.isaac.franka.controllers import PickPlaceController
from omni.isaac.core import World
from omni.isaac.core.utils.types import ArticulationAction
from omni.isaac.core import SimulationContext
import numpy as np

simulation_app = SimulationApp({"headless": False})
my_world = World(stage_units_in_meters=1.0)
my_task = PickPlace()
my_world.add_task(my_task)
simulation_context = SimulationContext()
my_world.reset()
task_params = my_task.get_params()
my_franka = my_world.scene.get_object(task_params["robot_name"]["value"])

# Initialize the PickPlaceController
my_controller = PickPlaceController(
    name="pick_place_controller",
    gripper=my_franka.gripper,
    robot_articulation=my_franka,
)

i = 0
while simulation_app.is_running():
    my_world.step(render=True)
    if my_world.is_playing():
        if my_world.current_time_step_index == 0:
            my_world.reset()
            my_controller.reset()
        observations = my_world.get_observations()
        if my_controller.is_done():
            print("done picking and placing")

        t = simulation_context.current_time
        actions = ArticulationAction(joint_positions=None, joint_velocities=None, joint_efforts=np.array([np.sin(t)] + [0.0] * 8))
        my_franka.apply_action(actions)

simulation_app.close()

Please note that this is a simplified example and may not work perfectly for your specific use case. You may need to adjust the control gains, joint efforts, and other parameters to get the desired behavior.

@rthaker Thanks for the response!

After running the code you provided it still does not move. I think the reason is that I am sending the robot an articulated action instead of using my_controller.

I was not planning to use my_controller, I just want to understand why sending a torque command via ArticulationAction directly does not lead to any motion at all.

I was playing around with the PD gains, I set them to be 10 and 0.1, which generates reasonable motion in PyBullet. This causes a significant oscillation in the hand, which is quite weird to me. If you can help me understand why this is the case that would be great!

I attached the entire code for reproduction

from omni.isaac.kit import SimulationApp

simulation_app = SimulationApp({"headless": False})

from omni.isaac.franka.tasks import PickPlace
from omni.isaac.franka.controllers import PickPlaceController
from omni.isaac.core import World
from omni.isaac.core.utils.types import ArticulationAction
from omni.isaac.core import SimulationContext
import numpy as np

my_world = World(stage_units_in_meters=1.0, physics_dt=1/1000)
my_task = PickPlace()
my_world.add_task(my_task)
simulation_context = SimulationContext()

my_world.reset()
task_params = my_task.get_params()
my_franka = my_world.scene.get_object(task_params["robot_name"]["value"])
my_franka.disable_gravity()

articulation_controller = my_franka.get_articulation_controller()
articulation_controller.set_effort_modes("force")
articulation_controller.switch_control_mode("effort")

q_des = np.array(
    [
        np.pi/4,
        -0.785398163,
        0.0,
        -2.35619449,
        0.0,
        1.57079632679,
        0.0,
        0.0,
        0.0
    ]
)

q_init = np.array(
    [
        0.0,
        -0.785398163,
        0.0,
        -2.35619449,
        0.0,
        1.57079632679,
        0.785398163397,
        0.0,
        0.0,
    ]
)
my_franka.set_joint_positions(q_init)

while simulation_app.is_running():

    q = my_franka.get_joint_positions()
    dq = my_franka.get_joint_velocities()

    Kp = np.diag([10.0]*6 + [0.001] + [0.1]*2)
    Kd = np.diag([1.0]*6 + [0.0001] + [0.1]*2)

    tau = Kp @ (q_des - q) - Kd @ dq

    print(tau)

    actions = ArticulationAction(
        joint_positions=None,
        joint_velocities=None,
        joint_efforts=tau,
    )

    articulation_controller.apply_action(actions)
    my_world.step()

simulation_app.close()

You can find the video here: https://www.youtube.com/watch?v=obr8GjoGDPM&ab_channel=BolunDai

After further testing, I believe the issue lies with the USD file, if I import the provided URDF then everything works fine. For future reference, the controller is implemented as

from omni.isaac.kit import SimulationApp

simulation_app = SimulationApp({"headless": False})

from omni.isaac.core.utils.extensions import get_extension_path_from_name
from omni.isaac.urdf import _urdf
from omni.isaac.core import World
import omni.kit.commands
from omni.isaac.core.utils.types import ArticulationAction
from omni.isaac.core import SimulationContext
import numpy as np
from fr3 import FR3


def main():
    world = World(stage_units_in_meters=1.0, physics_dt=1 / 1000)
    world.scene.add_default_ground_plane()
    simulation_context = SimulationContext()

    # Acquire the URDF extension interface
    urdf_interface = _urdf.acquire_urdf_interface()

    # Set the settings in the import config
    import_config = _urdf.ImportConfig()
    import_config.merge_fixed_joints = False
    import_config.convex_decomp = False
    import_config.import_inertia_tensor = True
    import_config.fix_base = True
    import_config.make_default_prim = True
    import_config.self_collision = False
    import_config.create_physics_scene = True
    import_config.import_inertia_tensor = False
    import_config.default_drive_strength = 1047.19751
    import_config.default_position_drive_damping = 52.35988
    import_config.default_drive_type = _urdf.UrdfJointTargetType.JOINT_DRIVE_POSITION
    import_config.distance_scale = 1
    import_config.density = 0.0

    # Get the urdf file path
    extension_path = get_extension_path_from_name("omni.isaac.urdf")
    root_path = extension_path + "/data/urdf/robots/franka_description/robots"
    file_name = "panda_arm_hand.urdf"

    # Finally import the robot
    result, prim_path = omni.kit.commands.execute(
        "URDFParseAndImportFile",
        urdf_path="{}/{}".format(root_path, file_name),
        import_config=import_config,
    )

    fr3_robot = FR3(
        prim_path=prim_path, name="FR3_robot", position=np.array([0, 0, 0.0])
    )

    world.scene.add(fr3_robot)

    world.reset()

    fr3_robot.disable_gravity()
    articulation_controller = fr3_robot.get_articulation_controller()
    articulation_controller.set_effort_modes("force")
    articulation_controller.switch_control_mode("effort")

    q_des = np.array(
        [np.pi / 4, -0.785398163, 0.0, -2.35619449, 0.0, 1.57079632679, 0.785398163397, 0.0, 0.0]
    )

    q_init = np.array(
        [
            0.0,
            -0.785398163,
            0.0,
            -2.35619449,
            0.0,
            1.57079632679,
            0.785398163397,
            0.0,
            0.0,
        ]
    )
    fr3_robot.set_joint_positions(q_init)

    while simulation_app.is_running():
        q = fr3_robot.get_joint_positions()
        dq = fr3_robot.get_joint_velocities()
        t = simulation_context.current_time

        tau = 10 * (q_des - q) - 10 * dq

        action = ArticulationAction(joint_efforts=tau)
        articulation_controller.apply_action(action)
        world.step(render=True)

    simulation_app.close()


if __name__ == "__main__":
    main()

The Robot class fr3.FR3 is implemented as

from typing import Optional, List
import numpy as np
from omni.isaac.core.robots.robot import Robot
from omni.isaac.core.prims.rigid_prim import RigidPrim
from omni.isaac.core.utils.prims import get_prim_at_path
from omni.isaac.core.utils.stage import get_stage_units
from omni.isaac.manipulators.grippers.parallel_gripper import ParallelGripper


class FR3(Robot):
    """[summary]

    Args:
        prim_path (str): [description]
        name (str, optional): [description]. Defaults to "franka_robot".
        usd_path (Optional[str], optional): [description]. Defaults to None.
        position (Optional[np.ndarray], optional): [description]. Defaults to None.
        orientation (Optional[np.ndarray], optional): [description]. Defaults to None.
        end_effector_prim_name (Optional[str], optional): [description]. Defaults to None.
        gripper_dof_names (Optional[List[str]], optional): [description]. Defaults to None.
        gripper_open_position (Optional[np.ndarray], optional): [description]. Defaults to None.
        gripper_closed_position (Optional[np.ndarray], optional): [description]. Defaults to None.
    """

    def __init__(
        self,
        prim_path: str,
        name: str = "franka_robot",
        usd_path: Optional[str] = None,
        position: Optional[np.ndarray] = None,
        orientation: Optional[np.ndarray] = None,
        end_effector_prim_name: Optional[str] = None,
        gripper_dof_names: Optional[List[str]] = None,
        gripper_open_position: Optional[np.ndarray] = None,
        gripper_closed_position: Optional[np.ndarray] = None,
        deltas: Optional[np.ndarray] = None,
    ) -> None:
        prim = get_prim_at_path(prim_path)
        self._end_effector = None
        self._gripper = None
        self._end_effector_prim_name = end_effector_prim_name

        if self._end_effector_prim_name is None:
            self._end_effector_prim_path = prim_path + "/panda_rightfinger"
        else:
            self._end_effector_prim_path = prim_path + "/" + end_effector_prim_name
        if gripper_dof_names is None:
            gripper_dof_names = ["panda_finger_joint1", "panda_finger_joint2"]
        if gripper_open_position is None:
            gripper_open_position = np.array([0.05, 0.05]) / get_stage_units()
        if gripper_closed_position is None:
            gripper_closed_position = np.array([0.0, 0.0])

        super().__init__(
            prim_path=prim_path,
            name=name,
            position=position,
            orientation=orientation,
            articulation_controller=None,
        )

        if gripper_dof_names is not None:
            if deltas is None:
                deltas = np.array([0.05, 0.05]) / get_stage_units()
            self._gripper = ParallelGripper(
                end_effector_prim_path=self._end_effector_prim_path,
                joint_prim_names=gripper_dof_names,
                joint_opened_positions=gripper_open_position,
                joint_closed_positions=gripper_closed_position,
                action_deltas=deltas,
            )
        return

    @property
    def end_effector(self) -> RigidPrim:
        """[summary]

        Returns:
            RigidPrim: [description]
        """
        return self._end_effector

    @property
    def gripper(self) -> ParallelGripper:
        """[summary]

        Returns:
            ParallelGripper: [description]
        """
        return self._gripper

    def initialize(self, physics_sim_view=None) -> None:
        """[summary]"""
        super().initialize(physics_sim_view)
        self._end_effector = RigidPrim(
            prim_path=self._end_effector_prim_path, name=self.name + "_end_effector"
        )
        self._end_effector.initialize(physics_sim_view)
        self._gripper.initialize(
            physics_sim_view=physics_sim_view,
            articulation_apply_action_func=self.apply_action,
            get_joint_positions_func=self.get_joint_positions,
            set_joint_positions_func=self.set_joint_positions,
            dof_names=self.dof_names,
        )
        return

    def post_reset(self) -> None:
        """[summary]"""
        super().post_reset()
        self._gripper.post_reset()
        self._articulation_controller.switch_dof_control_mode(
            dof_index=self.gripper.joint_dof_indicies[0], mode="position"
        )
        self._articulation_controller.switch_dof_control_mode(
            dof_index=self.gripper.joint_dof_indicies[1], mode="position"
        )
        return

The generated motion is shown in this YouTube video

2 Likes

This topic was automatically closed 14 days after the last reply. New replies are no longer allowed.