I had the same problem and managed to reach all three points by modifying the KUKAâs .urdf file, allowing the revolute joint between the base and link1 to rotate within these limits: lower=â-6.283â upper=â6.283â
Iâm not sure if this is the correct way to solve the problem. Below, I paste my scenario.py.
import numpy as np
import os
import lula
from scipy.spatial.transform import Rotation as R
import carb
from omni.isaac.core.utils.extensions import get_extension_path_from_name
from omni.isaac.core.utils.stage import add_reference_to_stage
from omni.isaac.core.articulations import Articulation
from omni.isaac.core.utils.nucleus import get_assets_root_path
from omni.isaac.core.objects.cuboid import FixedCuboid
from omni.isaac.core.prims import XFormPrim
from omni.isaac.core.utils.numpy.rotations import rot_matrices_to_quats
from omni.isaac.core.utils.prims import delete_prim, get_prim_at_path
from omni.isaac.motion_generation import (
LulaCSpaceTrajectoryGenerator,
LulaTaskSpaceTrajectoryGenerator,
LulaKinematicsSolver,
ArticulationTrajectory
)
class KukaTrajectoryGenerationExample():
def init(self):
self._c_space_trajectory_generator = None
self._taskspace_trajectory_generator = None
self._kinematics_solver = None
self._action_sequence = []
self._action_sequence_index = 0
self._articulation = None
def load_example_assets(self):
# Add the Franka and target to the stage
# The position in which things are loaded is also the position in which they
robot_prim_path = "/Kuka_KR210"
path_to_robot_usd = "C:/Users/albertazzia2/Desktop/Daniellerobot/urdf_files_dataset-main/urdf_files/ros-industrial/kuka/kuka_kr210_support/urdf/kr210l150/kr210l150.usd"
add_reference_to_stage(path_to_robot_usd, robot_prim_path)
self._articulation = Articulation(robot_prim_path)
# Return assets that were added to the stage so that they can be registered with the core.World
return [self._articulation]
def setup(self):
# Config files for supported robots are stored in the motion_generation extension under "/motion_policy_configs"
mg_extension_path = get_extension_path_from_name("omni.isaac.motion_generation")
rmp_config_dir = os.path.join(mg_extension_path, "motion_policy_configs")
self._taskspace_trajectory_generator = LulaTaskSpaceTrajectoryGenerator(
robot_description_path = rmp_config_dir + "/Kuka/kr210/rmpflow/kuka_kr210_robot_description.yaml",
urdf_path = "C:/Users/albertazzia2/Desktop/Daniellerobot/urdf_files_dataset-main/urdf_files/ros-industrial/kuka/kuka_kr210_support/urdf/kr210l150.urdf"
)
self._end_effector_name = "tool0"
def setup_taskspace_trajectory(self):
task_space_position_targets = np.array([
[1.0, -1.0, 1.0],
[-1.0, -1.0, 1.0],
[-1.0, 1.0, 1.0],
])
quat = R.from_euler('xyz', [0, 90, 0], degrees=True).as_quat() # [x, y, z, w]
x,y,z,w = quat
# Ora crea gli orientation targets
task_space_orientation_targets = np.tile([w,x,y,z], (5, 1))
trajectory = self._taskspace_trajectory_generator.compute_task_space_trajectory_from_points(
task_space_position_targets, task_space_orientation_targets, self._end_effector_name
)
# Visualize task-space targets in task space
for i,(position,orientation) in enumerate(zip(task_space_position_targets,task_space_orientation_targets)):
add_reference_to_stage(get_assets_root_path() + "/Isaac/Props/UIElements/frame_prim.usd", f"/visualized_frames/target_{i}")
frame = XFormPrim(f"/visualized_frames/target_{i}",scale=[.04,.04,.04])
frame.set_world_pose(position,orientation)
if trajectory is None:
carb.log_warn("No trajectory could be computed")
self._action_sequence = []
else:
physics_dt = 1/60
articulation_trajectory = ArticulationTrajectory(self._articulation, trajectory, physics_dt)
# Get a sequence of ArticulationActions that are intended to be passed to the robot at 1/60 second intervals
self._action_sequence = articulation_trajectory.get_action_sequence()
def update(self, step: float):
if len(self._action_sequence) == 0:
return
if self._action_sequence_index >= len(self._action_sequence):
self._action_sequence_index += 1
self._action_sequence_index %= len(self._action_sequence) + 10 # Wait 10 frames before repeating trajectories
return
if self._action_sequence_index == 0:
self._teleport_robot_to_position(self._action_sequence[0])
self._articulation.apply_action(self._action_sequence[self._action_sequence_index])
self._action_sequence_index += 1
self._action_sequence_index %= len(self._action_sequence) + 10 # Wait 10 frames before repeating trajectories
def reset(self):
# Delete any visualized frames
if get_prim_at_path("/visualized_frames"):
delete_prim("/visualized_frames")
self._action_sequence = []
self._action_sequence_index = 0
def _teleport_robot_to_position(self, articulation_action):
initial_positions = np.zeros(self._articulation.num_dof)
initial_positions[articulation_action.joint_indices] = articulation_action.joint_positions
self._articulation.set_joint_positions(initial_positions)
self._articulation.set_joint_velocities(np.zeros_like(initial_positions))
