hofee/gen_data_curobo
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add isaac sim 2023 support and dockerfiles

Browse Source
This commit is contained in:
Balakumar Sundaralingam
2023-11-04 09:32:30 -07:00
parent f2eb5f937a
commit 102c5d6ab2
41 changed files with 1284 additions and 622 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/curobo/content/configs/robot/franka.yml
View File

@@ -13,7 +13,7 @@ robot_cfg:
kinematics:
use_usd_kinematics: False
isaac_usd_path: "/Isaac/Robots/Franka/franka.usd"
usd_path: "robot/non_shipping/franka/franka_panda_meters1.usda"
usd_path: "robot/non_shipping/franka/franka_panda_meters.usda"
usd_robot_root: "/panda"
usd_flip_joints: ["panda_joint1","panda_joint2","panda_joint3","panda_joint4", "panda_joint5",
"panda_joint6","panda_joint7","panda_finger_joint1", "panda_finger_joint2"]

1
src/curobo/content/configs/robot/ur10e.yml
View File

@@ -40,6 +40,7 @@ robot_cfg:
'wrist_3_link' : 0,
'tool0': 0,
}
mesh_link_names: [ 'shoulder_link','upper_arm_link', 'forearm_link', 'wrist_1_link', 'wrist_2_link' ,'wrist_3_link' ]
lock_joints: null
cspace:

4
src/curobo/cuda_robot_model/cuda_robot_generator.py
View File

@@ -43,9 +43,9 @@ try:
# CuRobo
from curobo.cuda_robot_model.usd_kinematics_parser import UsdKinematicsParser
except ImportError:
log_warn(
log_info(
"USDParser failed to import, install curobo with pip install .[usd] "
+ "or pip install usd-core"
+ "or pip install usd-core, NOTE: Do not install this if using with ISAAC SIM."
)

16
src/curobo/cuda_robot_model/cuda_robot_model.py
View File

@@ -252,12 +252,19 @@ class CudaRobotModel(CudaRobotModelConfig):
)
return state
def get_robot_link_meshes(self):
m_list = [self.get_link_mesh(l) for l in self.kinematics_config.mesh_link_names]
return m_list
def get_robot_as_mesh(self, q: torch.Tensor):
# get all link meshes:
m_list = [self.get_link_mesh(l) for l in self.mesh_link_names]
pose = self.get_link_poses(q, self.mesh_link_names)
for li, l in enumerate(self.mesh_link_names):
m_list[li].pose = pose.get_index(0, li).tolist()
m_list = self.get_robot_link_meshes()
pose = self.get_link_poses(q, self.kinematics_config.mesh_link_names)
for li, l in enumerate(self.kinematics_config.mesh_link_names):
m_list[li].pose = (
Pose.from_list(m_list[li].pose).multiply(pose.get_index(0, li)).tolist()
)
return m_list
@@ -344,7 +351,6 @@ class CudaRobotModel(CudaRobotModelConfig):
def get_link_mesh(self, link_name: str) -> Mesh:
mesh = self.kinematics_parser.get_link_mesh(link_name)
mesh.pose = [0, 0, 0, 1, 0, 0, 0]
return mesh
def get_link_transform(self, link_name: str) -> Pose:

15
src/curobo/cuda_robot_model/urdf_kinematics_parser.py
View File

@@ -21,6 +21,7 @@ from curobo.cuda_robot_model.kinematics_parser import KinematicsParser, LinkPara
from curobo.cuda_robot_model.types import JointType
from curobo.geom.types import Mesh as CuroboMesh
from curobo.types.base import TensorDeviceType
from curobo.types.math import Pose
from curobo.util.logger import log_error, log_warn
from curobo.util_file import join_path
@@ -164,4 +165,16 @@ class UrdfKinematicsParser(KinematicsParser):
def get_link_mesh(self, link_name):
m = self._robot.link_map[link_name].visuals[0].geometry.mesh
return CuroboMesh(name=link_name, pose=None, scale=m.scale, file_path=m.filename)
mesh_pose = self._robot.link_map[link_name].visuals[0].origin
# read visual material:
if mesh_pose is None:
mesh_pose = [0, 0, 0, 1, 0, 0, 0]
else:
# convert to list:
mesh_pose = Pose.from_matrix(mesh_pose).to_list()
return CuroboMesh(
name=link_name,
pose=mesh_pose,
scale=m.scale,
file_path=m.filename,
)

21
src/curobo/geom/types.py
View File

@@ -381,7 +381,6 @@ class Mesh(Obstacle):
m = trimesh.load(self.file_path, process=process, force="mesh")
if isinstance(m, trimesh.Scene):
m = m.dump(concatenate=True)
if isinstance(m.visual, trimesh.visual.texture.TextureVisuals):
m.visual = m.visual.to_color()
else:
@@ -397,6 +396,26 @@ class Mesh(Obstacle):
return m
def update_material(self):
if (
self.color is None
and self.vertex_colors is None
and self.face_colors is None
and self.file_path is not None
):
# try to load material:
m = trimesh.load(self.file_path, process=False, force="mesh")
if isinstance(m, trimesh.Scene):
m = m.dump(concatenate=True)
if isinstance(m.visual, trimesh.visual.texture.TextureVisuals):
m.visual = m.visual.to_color()
if isinstance(m.visual, trimesh.visual.color.ColorVisuals):
if isinstance(m.visual.vertex_colors[0], np.ndarray):
self.vertex_colors = m.visual.vertex_colors
self.face_colors = m.visual.face_colors
else:
self.vertex_colors = [m.visual.vertex_colors for x in range(len(m.vertices))]
def get_mesh_data(self, process: bool = True):
verts = faces = None
if self.file_path is not None:

10
src/curobo/types/math.py
View File

@@ -83,10 +83,16 @@ class Pose(Sequence):
self.quaternion = normalize_quaternion(self.quaternion)
@staticmethod
def from_matrix(matrix: np.ndarray):
def from_matrix(matrix: Union[np.ndarray, torch.Tensor]):
if not isinstance(matrix, torch.Tensor):
tensor_args = TensorDeviceType()
matrix = torch.as_tensor(matrix, device=tensor_args.device, dtype=tensor_args.dtype)
if len(matrix.shape) == 2:
matrix = matrix.view(-1, 4, 4)
return Pose(position=matrix[..., :3, 3], rotation=matrix[..., :3, :3])
return Pose(
position=matrix[..., :3, 3], rotation=matrix[..., :3, :3], normalize_rotation=True
)
def get_rotation(self):
if self.rotation is not None:

129
src/curobo/util/usd_helper.py
View File

@@ -38,6 +38,7 @@ from curobo.types.state import JointState
from curobo.util.logger import log_error, log_warn
from curobo.util_file import (
file_exists,
get_assets_path,
get_filename,
get_files_from_dir,
get_robot_configs_path,
@@ -314,7 +315,6 @@ def get_sphere_attrs(prim, cache=None, transform=None) -> Sphere:
size = 1.0
radius = prim.GetAttribute("radius").Get()
scale = prim.GetAttribute("xformOp:scale").Get()
print(radius, scale)
if scale is not None:
radius = radius * max(list(scale)) * size
@@ -620,7 +620,7 @@ class UsdHelper:
root_path = join_path(base_frame, obstacle.name)
obj_geom = UsdGeom.Mesh.Define(self.stage, root_path)
obj_prim = self.stage.GetPrimAtPath(root_path)
# obstacle.update_material() # This does not get the correct materials
set_geom_mesh_attrs(obj_geom, obstacle, timestep=timestep)
obj_prim.CreateAttribute("physics:rigidBodyEnabled", Sdf.ValueTypeNames.Bool, custom=False)
@@ -664,10 +664,20 @@ class UsdHelper:
)
for i, i_val in enumerate(prim_names):
curr_prim = self.stage.GetPrimAtPath(i_val)
form = UsdGeom.Xformable(curr_prim).GetOrderedXformOps()[0]
form = UsdGeom.Xformable(curr_prim).GetOrderedXformOps()
if len(form) < 2:
log_warn("Pose transformation not found" + i_val)
continue
pos_form = form[0]
quat_form = form[1]
use_float = True # default is float
for t in range(pose.batch):
c_t = get_transform(pose.get_index(t, i).tolist())
form.Set(time=t * self.interpolation_steps, value=c_t)
c_p, c_q = get_position_quat(pose.get_index(t, i).tolist(), use_float)
pos_form.Set(time=t * self.interpolation_steps, value=c_p)
quat_form.Set(time=t * self.interpolation_steps, value=c_q)
# c_t = get_transform(pose.get_index(t, i).tolist())
# form.Set(time=t * self.interpolation_steps, value=c_t)
def create_obstacle_animation(
self,
@@ -785,33 +795,64 @@ class UsdHelper:
save_path: str = "out.usd",
tensor_args: TensorDeviceType = TensorDeviceType(),
interpolation_steps: float = 1.0,
robot_color: List[float] = [0.8, 0.8, 0.8, 1.0],
robot_base_frame="robot",
base_frame="/world",
kin_model: Optional[CudaRobotModel] = None,
visualize_robot_spheres: bool = True,
robot_color: Optional[List[float]] = None,
):
config_file = load_yaml(join_path(get_robot_configs_path(), robot_model_file))
config_file["robot_cfg"]["kinematics"]["load_link_names_with_mesh"] = True
robot_cfg = CudaRobotModelConfig.from_data_dict(
config_file["robot_cfg"]["kinematics"], tensor_args=tensor_args
)
kin_model = CudaRobotModel(robot_cfg)
m = kin_model.get_robot_as_mesh(q_start.position)
if kin_model is None:
config_file = load_yaml(join_path(get_robot_configs_path(), robot_model_file))
config_file["robot_cfg"]["kinematics"]["load_link_names_with_mesh"] = True
robot_cfg = CudaRobotModelConfig.from_data_dict(
config_file["robot_cfg"]["kinematics"], tensor_args=tensor_args
)
kin_model = CudaRobotModel(robot_cfg)
m = kin_model.get_robot_link_meshes()
offsets = [x.pose for x in m]
robot_mesh_model = WorldConfig(mesh=m)
robot_mesh_model.add_color(robot_color)
robot_mesh_model.add_material(Material(metallic=0.4))
if robot_color is not None:
robot_mesh_model.add_color(robot_color)
robot_mesh_model.add_material(Material(metallic=0.4))
usd_helper = UsdHelper()
usd_helper.create_stage(
save_path,
timesteps=q_traj.position.shape[0],
dt=dt,
interpolation_steps=interpolation_steps,
base_frame=base_frame,
)
usd_helper.add_world_to_stage(world_model)
if world_model is not None:
usd_helper.add_world_to_stage(world_model, base_frame=base_frame)
animation_links = kin_model.mesh_link_names
animation_links = kin_model.kinematics_config.mesh_link_names
animation_poses = kin_model.get_link_poses(q_traj.position, animation_links)
usd_helper.create_animation(robot_mesh_model, animation_poses)
usd_helper.write_stage_to_file(save_path + ".usd")
# add offsets for visual mesh:
for i, ival in enumerate(offsets):
offset_pose = Pose.from_list(ival)
new_pose = Pose(
animation_poses.position[:, i, :], animation_poses.quaternion[:, i, :]
).multiply(offset_pose)
animation_poses.position[:, i, :] = new_pose.position
animation_poses.quaternion[:, i, :] = new_pose.quaternion
robot_base_frame = join_path(base_frame, robot_base_frame)
usd_helper.create_animation(
robot_mesh_model, animation_poses, base_frame, robot_frame=robot_base_frame
)
if visualize_robot_spheres:
# visualize robot spheres:
sphere_traj = kin_model.get_robot_as_spheres(q_traj.position)
# change color:
for s in sphere_traj:
for k in s:
k.color = [0, 0.27, 0.27, 1.0]
usd_helper.create_obstacle_animation(
sphere_traj, base_frame=base_frame, obstacles_frame="curobo/robot_collision"
)
usd_helper.write_stage_to_file(save_path)
@staticmethod
def load_robot(
@@ -848,7 +889,11 @@ class UsdHelper:
kin_model: Optional[CudaRobotModel] = None,
visualize_robot_spheres: bool = True,
robot_asset_prim_path=None,
robot_color: Optional[List[float]] = None,
):
usd_exists = False
# if usd file doesn't exist, fall back to urdf animation script
if kin_model is None:
config_file = load_yaml(join_path(get_robot_configs_path(), robot_model_file))
if "robot_cfg" in config_file:
@@ -856,6 +901,36 @@ class UsdHelper:
config_file["kinematics"]["load_link_names_with_mesh"] = True
config_file["kinematics"]["use_usd_kinematics"] = True
usd_exists = file_exists(
join_path(get_assets_path(), config_file["kinematics"]["usd_path"])
)
else:
if kin_model.generator_config.usd_path is not None:
usd_exists = file_exists(kin_model.generator_config.usd_path)
if robot_color is not None:
log_warn(
"robot_color is not supported when using robot from usd, "
+ "using urdf mode instead to write usd file"
)
usd_exists = False
if not usd_exists:
log_warn("robot usd not found, using urdf animation instead")
return UsdHelper.write_trajectory_animation(
robot_model_file,
world_model,
q_start,
q_traj,
dt,
save_path,
tensor_args,
interpolation_steps,
robot_base_frame=robot_base_frame,
base_frame=base_frame,
kin_model=kin_model,
visualize_robot_spheres=visualize_robot_spheres,
robot_color=robot_color,
)
if kin_model is None:
robot_cfg = CudaRobotModelConfig.from_data_dict(
config_file["kinematics"], tensor_args=tensor_args
)
@@ -1064,9 +1139,17 @@ class UsdHelper:
write_robot_usd_path="assets/",
robot_asset_prim_path="/panda",
):
# if goal_object is None:
# log_warn("Using franka gripper as goal object")
# goal_object =
if goal_object is None:
log_warn("Using franka gripper as goal object")
goal_object = Mesh(
name="target_gripper",
file_path=join_path(
get_assets_path(),
"robot/franka_description/meshes/visual/hand.dae",
),
color=[0.0, 0.8, 0.1, 1.0],
pose=goal_pose.to_list(),
)
if goal_object is not None:
goal_object.pose = np.ravel(goal_pose.tolist()).tolist()
world_config = world_config.clone()

6
src/curobo/wrap/reacher/trajopt.py
View File

@@ -23,7 +23,7 @@ import torch.autograd.profiler as profiler
# CuRobo
from curobo.cuda_robot_model.cuda_robot_model import CudaRobotModel, CudaRobotModelState
from curobo.geom.sdf.utils import create_collision_checker
from curobo.geom.sdf.world import WorldCollision, WorldCollisionConfig
from curobo.geom.sdf.world import CollisionCheckerType, WorldCollision, WorldCollisionConfig
from curobo.geom.types import WorldConfig
from curobo.opt.newton.lbfgs import LBFGSOpt, LBFGSOptConfig
from curobo.opt.newton.newton_base import NewtonOptBase, NewtonOptConfig
@@ -104,6 +104,7 @@ class TrajOptSolverConfig:
num_seeds: int = 2,
seed_ratio: Dict[str, int] = {"linear": 1.0, "bias": 0.0, "start": 0.0, "end": 0.0},
use_particle_opt: bool = True,
collision_checker_type: Optional[CollisionCheckerType] = CollisionCheckerType.PRIMITIVE,
traj_evaluator_config: TrajEvaluatorConfig = TrajEvaluatorConfig(),
traj_evaluator: Optional[TrajEvaluator] = None,
minimize_jerk: Optional[bool] = None,
@@ -144,6 +145,9 @@ class TrajOptSolverConfig:
if not minimize_jerk:
filter_robot_command = False
if collision_checker_type is not None:
base_config_data["world_collision_checker_cfg"]["checker_type"] = collision_checker_type
if world_coll_checker is None and world_model is not None:
world_cfg = WorldCollisionConfig.load_from_dict(
base_config_data["world_collision_checker_cfg"], world_model, tensor_args