In 'Pick and Place' tutorial(4.4. Using the PickAndPlace Controller), initial gripper open position is too narrow

https://docs.omniverse.nvidia.com/app_isaacsim/app_isaacsim/tutorial_core_adding_manipulator.html#using-the-pickandplace-controller

In the above example, the Franka cannot grasp the cube due to too narrow gripper’s open position. I tried to change the inial condition but failed. How can I open Franka’s gripper wider?

Is it wrong to add ‘gripper_open_position=np.array(0.05)’ when creating a Franka object?

[2023. 01. 10. added 1.]
The Franka in 5.6 Robot Handover also starts with narrowed gripper width different from the video.

I found the solution for handling the issue. Please refer *** Set initial joint position of joints 7, 8 which are gripper *** at async def setup_post_load(self): and async def setup_post_reset(self):. However, I still don’t know why the basic example of picking a block with Panda only works without any changes.

from omni.isaac.examples.base_sample import BaseSample
from omni.isaac.franka.tasks import PickPlace
from omni.isaac.wheeled_robots.robots import WheeledRobot
from omni.isaac.core.utils.nucleus import get_assets_root_path
from omni.isaac.motion_generation import WheelBasePoseController
from omni.isaac.franka.controllers import PickPlaceController
from omni.isaac.wheeled_robots.controllers.differential_controller import DifferentialController
from omni.isaac.core.tasks import BaseTask
from omni.isaac.core.utils.types import ArticulationAction
import numpy as np


class RobotsPlaying(BaseTask):
    def __init__(
        self,
        name
    ):
        super().__init__(name=name, offset=None)
        self._jetbot_goal_position = np.array([1.3, 0.3, 0])
        self._task_event = 0
        self._pick_place_task = PickPlace(cube_initial_position=np.array([0.1, 0.3, 0.05]),
                                        target_position=np.array([0.7, -0.3, 0.0515 / 2.0]),)
        return

    def set_up_scene(self, scene):
        super().set_up_scene(scene)
        self._pick_place_task.set_up_scene(scene)
        assets_root_path = get_assets_root_path()
        jetbot_asset_path = assets_root_path + "/Isaac/Robots/Jetbot/jetbot.usd"
        self._jetbot = scene.add(
            WheeledRobot(
                prim_path="/World/Fancy_Robot",
                name="fancy_robot",
                wheel_dof_names=["left_wheel_joint", "right_wheel_joint"],
                create_robot=True,
                usd_path=jetbot_asset_path,
                position=np.array([0, 0.3, 0]))
        )
        pick_place_params = self._pick_place_task.get_params()
        self._franka = scene.get_object(pick_place_params["robot_name"]["value"])
        # Changes Franka's default position
        # so that it is set at this position after reset
        self._franka.set_world_pose(position=np.array([1.0, 0, 0]))
        self._franka.set_default_state(position=np.array([1.0, 0, 0]))
        return

    def get_observations(self):
        current_jetbot_position, current_jetbot_orientation = self._jetbot.get_world_pose()
        observations= {
            "task_event": self._task_event,
            self._jetbot.name: {
                "position": current_jetbot_position,
                "orientation": current_jetbot_orientation,
                "goal_position": self._jetbot_goal_position
            }
        }
        observations.update(self._pick_place_task.get_observations())
        return observations

    def get_params(self):
        # To avoid hard coding names..etc.
        pick_place_params = self._pick_place_task.get_params()
        params_representation = pick_place_params
        params_representation["jetbot_name"] = {"value": self._jetbot.name, "modifiable": False}
        params_representation["franka_name"] = pick_place_params["robot_name"]
        return params_representation
    
    def pre_step(self, control_index, simulation_time):
        if self._task_event == 0:
            current_jetbot_position, _ = self._jetbot.get_world_pose()
            # print(f'control_index: {control_index}')
            goal_and_current_distance = np.mean(np.abs(current_jetbot_position[:2] - self._jetbot_goal_position[:2]))
            # print(np.round(goal_and_current_distance, 3))
            if goal_and_current_distance < 0.04:
                print(f'Jetbot has arrived at the goal position')
                self._task_event += 1
                self._cube_arrive_step_index = control_index
        elif self._task_event == 1:
            # Jetbot has 200 time steps to back off from Franka
            if control_index - self._cube_arrive_step_index == 200:
                print(f'Jetbot has backed off from Franka then stop')
                self._task_event += 1
        return

    def post_reset(self):
        self._task_event = 0
        return


class HelloWorld(BaseSample):
    def __init__(self) -> None:
        super().__init__()
        return

    def setup_scene(self):
        world = self.get_world()
        # Add task here
        world.add_task(RobotsPlaying(name="awesome_task"))
        return

    async def setup_post_load(self):
        self._world = self.get_world()
        task_params = self._world.get_task("awesome_task").get_params()        

        # We need franka later to apply to it actions
        self._franka = self._world.scene.get_object(task_params["franka_name"]["value"])
        

        '''*** Set initial joint position of joint 7, 8 which are gripper ***'''
        joint_position = self._franka.get_joint_positions()
        print(joint_position)
        joint_position[7:] = np.array([0.04, 0.04])
        print('>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>')
        print(joint_position)
        print('>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>')
        self._franka.set_joint_positions(positions=joint_position)
        '''*** Set initial joint position of joint 7, 8 which are gripper ***'''


        self._jetbot = self._world.scene.get_object(task_params["jetbot_name"]["value"])
        # We need the cube later on for the pick place controller
        self._cube_name = task_params["cube_name"]["value"]
        # Add Franka Controller
        self._franka_controller = PickPlaceController(name="pick_place_controller",
                                                    gripper=self._franka.gripper,
                                                    robot_articulation=self._franka)
        # Add Jetbot Controller
        self._jetbot_controller = WheelBasePoseController(name="cool_controller",
                                                        open_loop_wheel_controller=
                                                            DifferentialController(name="simple_control",
                                                                                wheel_radius=0.03, wheel_base=0.1125))
        self._world.add_physics_callback("sim_step", callback_fn=self.physics_step)
        await self._world.play_async()
        return

    async def setup_post_reset(self):
        self._franka_controller.reset()
        self._jetbot_controller.reset()

        '''*** Reset initial joint position of joint 7, 8 which are gripper ***'''
        joint_position = self._franka.get_joint_positions()
        joint_position[7:] = np.array([0.04, 0.04])
        self._franka.set_joint_positions(positions=joint_position)
        '''*** Reset initial joint position of joint 7, 8 which are gripper ***'''

        await self._world.play_async()
        return

    def physics_step(self, step_size):
        current_observations = self._world.get_observations()
        if current_observations["task_event"] == 0:
            self._jetbot.apply_action(
                self._jetbot_controller.forward(
                    start_position=current_observations[self._jetbot.name]["position"],
                    start_orientation=current_observations[self._jetbot.name]["orientation"],
                    goal_position=current_observations[self._jetbot.name]["goal_position"]))
        elif current_observations["task_event"] == 1:
            # Go backwards
            self._jetbot.apply_wheel_actions(ArticulationAction(joint_velocities=[-8, -8]))
        elif current_observations["task_event"] == 2:
            self._jetbot.apply_wheel_actions(ArticulationAction(joint_velocities=[0.0, 0.0]))
            # Pick up the block
            actions = self._franka_controller.forward(
                picking_position=current_observations[self._cube_name]["position"],
                placing_position=current_observations[self._cube_name]["target_position"],
                current_joint_positions=current_observations[self._franka.name]["joint_positions"])
            self._franka.apply_action(actions)
        # Pause once the controller is done
        if self._franka_controller.is_done():
            self._world.pause()
        return
3 Likes

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