Hi @rthaker sorry for the very late response. I’ve resolved that issue by creating a reference point and basically moving the whole 3D cityEngine model to (0,0,0). Now my Jetbot is moving on the city roads, but with one issue - it does not move in negative X coordinates on straight line(e.g from (-7,200,0) to (-90,200,0)). Would you be able to point me why is it happening? Thanks!
P.S.: Code snippets are below
- Robot class
class RobotMoving(BaseTask):
def __init__(self, name, offset=None, robot_index=None):
super().__init__(name=name, offset=offset)
self._extension_data_path = "source/extensions/deloitte.cuopt_app.moving_to_target/data/" # noqa: E501
self._robot_index = robot_index
self.offset = offset
self._locations_obj = TransportOrders()
self._load_orders()
self._jetbot_goal_position = np.array(self._locations_obj.graph_locations[self._robot_index][1])
self._task_achieved = False
self._coord_indx = 1
print(f"JETBOT {self._robot_index} navigating to {self._jetbot_goal_position}")
return
# Here we setup all the assets that we care about in this task.
def set_up_scene(self, scene):
super().set_up_scene(scene)
scene.add_ground_plane(
size=1000,
color=np.array([0.38, 0.5, 0.5])
)
stage = omni.usd.get_context().get_stage()
# Find a unique scene name
jetbot_name = find_unique_string_name(
initial_name="fancy_jetbot", is_unique_fn=lambda x: not self.scene.object_exists(x)
)
# Find a unique prim path
jetbot_prim_path = find_unique_string_name(
initial_name="/World/Fancy_Jetbot", is_unique_fn=lambda x: not is_prim_path_valid(x)
)
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=jetbot_prim_path,
name=jetbot_name,
wheel_dof_names=["left_wheel_joint", "right_wheel_joint"],
create_robot=True,
usd_path=jetbot_asset_path,
# position=np.array([0, 0.3, 0]) + self._offset
# position=np.array([-7.19, 318.85, 0.0]) + self._offset
position=np.array(self._locations_obj.graph_locations[self._robot_index][0]) + self._offset
# position=np.array([-21.19, 3.12, 0.0]) + self._offset
)
)
- How I pass next goal to the robot
def pre_step(self, control_index, simulation_time):
current_jetbot_position, _ = self._jetbot.get_world_pose()
# np_mean = np.mean(np.abs(current_jetbot_position[:2] - self._jetbot_goal_position[:2]))
np_mean = np.mean(np.abs(self._jetbot_goal_position[:2] - current_jetbot_position[:2]))
if not self._task_achieved and np_mean < 0.6 and self._coord_indx == (len(self.task_data["task_locations"])-1):
print("GOAL HAS BEEN REACHED")
self._task_achieved = True
elif self._coord_indx < (len(self.task_data["task_locations"])-1) and not self._task_achieved and (np_mean > 1.2 and np_mean < 1.8):
self._coord_indx += 1
self._jetbot_goal_position = np.array(self.task_data["task_locations"][self._coord_indx]) # noqa E501
return
- That is the World where I use Robot’s class
class HelloWorld(BaseSample):
def __init__(self) -> None:
super().__init__()
# Add lists for tasks,
self._tasks = []
self._num_of_robots = 0
# Add lists for controllers
self._jetbot_controllers = []
# Add lists for variables needed for control
self._jetbots = []
return
def setup_scene(self):
self._num_of_robots = self.get_number_of_vehicles()
# self._num_of_robots = 1
world = self.get_world()
for i in range(self._num_of_robots):
world.add_task(RobotMoving(name="my_awesome_task_" + str(i), offset=np.array([0, (i * 3.5) - 1.0, 0]), robot_index=i)) # noqa: E501
return
async def setup_post_load(self):
self._world = self.get_world()
for i in range(self._num_of_robots):
self._tasks.append(self._world.get_task(name="my_awesome_task_" + str(i)))
# Get variables needed for control
task_params = self._tasks[i].get_params()
self._jetbots.append(self._world.scene.get_object(task_params["jetbot_name"]["value"]))
# Define Controllers
self._jetbot_controllers.append(WheelBasePoseController(name="cool_controller",
open_loop_wheel_controller=DifferentialController(name="simple_control", wheel_radius=0.198, wheel_base=0.66750))) # noqa: E501 # noqa: E128
self._world.add_physics_callback("sim_step", callback_fn=self.physics_step)
await self._world.play_async()
return
async def setup_post_reset(self):
for i in range(len(self._tasks)):
# Reset controller
self._jetbot_controllers[i].reset()
await self._world.play_async()
return
def physics_step(self, step_size):
# Gets all the tasks observations
current_observations = self._world.get_observations()
for i in range(len(self._tasks)):
goal_x, goal_y, goal_z = current_observations[self._jetbots[i].name]["goal_position"]
# print("Navigating directly to goal ({}, {}, {})".format(goal_x, goal_y, goal_z))
self._jetbots[i].apply_wheel_actions(
self._jetbot_controllers[i].forward(
start_position=current_observations[self._jetbots[i].name]["position"],
start_orientation=current_observations[self._jetbots[i].name]["orientation"],
goal_position=current_observations[self._jetbots[i].name]["goal_position"],
lateral_velocity=1.6,
yaw_velocity=0.8,))
return