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

Significantly improved convergence for mesh and cuboid, new ESDF collision.

Browse Source
This commit is contained in:
Balakumar Sundaralingam
2024-03-18 11:19:48 -07:00
parent 286b3820a5
commit b1f63e8778
100 changed files with 7587 additions and 2589 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -180,7 +180,7 @@ class CudaRobotModel(CudaRobotModelConfig):
self._batch_robot_spheres = torch.zeros(
(self._batch_size, self.kinematics_config.total_spheres, 4),
device=self.tensor_args.device,
dtype=self.tensor_args.dtype,
dtype=self.tensor_args.collision_geometry_dtype,
)
self._grad_out_q = torch.zeros(
(self._batch_size, self.get_dof()),

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

@@ -23,6 +23,7 @@ from curobo.types.base import TensorDeviceType
from curobo.types.math import Pose
from curobo.types.state import JointState
from curobo.types.tensor import T_DOF
from curobo.util.logger import log_error
from curobo.util.tensor_util import clone_if_not_none, copy_if_not_none
@@ -138,6 +139,13 @@ class CSpaceConfig:
self.acceleration_scale = self.tensor_args.to_device(self.acceleration_scale)
if isinstance(self.jerk_scale, List):
self.jerk_scale = self.tensor_args.to_device(self.jerk_scale)
# check shapes:
if self.retract_config is not None:
dof = self.retract_config.shape
if self.cspace_distance_weight is not None and self.cspace_distance_weight.shape != dof:
log_error("cspace_distance_weight shape does not match retract_config")
if self.null_space_weight is not None and self.null_space_weight.shape != dof:
log_error("null_space_weight shape does not match retract_config")
def inplace_reindex(self, joint_names: List[str]):
new_index = [self.joint_names.index(j) for j in joint_names]
@@ -207,8 +215,8 @@ class CSpaceConfig:
):
retract_config = ((joint_position_upper + joint_position_lower) / 2).flatten()
n_dof = retract_config.shape[-1]
null_space_weight = torch.ones(n_dof, **vars(tensor_args))
cspace_distance_weight = torch.ones(n_dof, **vars(tensor_args))
null_space_weight = torch.ones(n_dof, **(tensor_args.as_torch_dict()))
cspace_distance_weight = torch.ones(n_dof, **(tensor_args.as_torch_dict()))
return CSpaceConfig(
joint_names,
retract_config,
@@ -289,8 +297,8 @@ class KinematicsTensorConfig:
retract_config = (
(self.joint_limits.position[1] + self.joint_limits.position[0]) / 2
).flatten()
null_space_weight = torch.ones(self.n_dof, **vars(self.tensor_args))
cspace_distance_weight = torch.ones(self.n_dof, **vars(self.tensor_args))
null_space_weight = torch.ones(self.n_dof, **(self.tensor_args.as_torch_dict()))
cspace_distance_weight = torch.ones(self.n_dof, **(self.tensor_args.as_torch_dict()))
joint_names = self.joint_names
self.cspace = CSpaceConfig(
joint_names,

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

@@ -175,7 +175,11 @@ class UrdfKinematicsParser(KinematicsParser):
return txt
def get_link_mesh(self, link_name):
m = self._robot.link_map[link_name].visuals[0].geometry.mesh
link_data = self._robot.link_map[link_name]
if len(link_data.visuals) == 0:
log_error(link_name + " not found in urdf, remove from mesh_link_names")
m = link_data.visuals[0].geometry.mesh
mesh_pose = self._robot.link_map[link_name].visuals[0].origin
# read visual material:
if mesh_pose is None: