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

update to 0.6.2

Browse Source
This commit is contained in:
Balakumar Sundaralingam
2023-12-15 02:01:33 -08:00
parent d85ae41fba
commit 58958bbcce
105 changed files with 2514 additions and 934 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -136,6 +136,9 @@ class CudaRobotGeneratorConfig:
use_external_assets: bool = False
external_asset_path: Optional[str] = None
external_robot_configs_path: Optional[str] = None
#: Create n collision spheres for links with name
extra_collision_spheres: Optional[Dict[str, int]] = None
@@ -144,18 +147,20 @@ class CudaRobotGeneratorConfig:
def __post_init__(self):
# add root path:
if self.urdf_path is not None:
self.urdf_path = join_path(get_assets_path(), self.urdf_path)
if self.usd_path is not None:
self.usd_path = join_path(get_assets_path(), self.usd_path)
if self.asset_root_path != "":
if self.use_external_assets:
with importlib_resources.files("content") as path:
content_dir_posix = path
self.asset_root_path = join_path(content_dir_posix, self.asset_root_path)
# Check if an external asset path is provided:
asset_path = get_assets_path()
robot_path = get_robot_configs_path()
if self.external_asset_path is not None:
asset_path = self.external_asset_path
if self.external_robot_configs_path is not None:
robot_path = self.external_robot_configs_path
else:
self.asset_root_path = join_path(get_assets_path(), self.asset_root_path)
if self.urdf_path is not None:
self.urdf_path = join_path(asset_path, self.urdf_path)
if self.usd_path is not None:
self.usd_path = join_path(asset_path, self.usd_path)
if self.asset_root_path != "":
self.asset_root_path = join_path(asset_path, self.asset_root_path)
elif self.urdf_path is not None:
self.asset_root_path = os.path.dirname(self.urdf_path)
@@ -178,7 +183,7 @@ class CudaRobotGeneratorConfig:
self.link_names.append(self.ee_link)
if self.collision_spheres is not None:
if isinstance(self.collision_spheres, str):
coll_yml = join_path(get_robot_configs_path(), self.collision_spheres)
coll_yml = join_path(robot_path, self.collision_spheres)
coll_params = load_yaml(coll_yml)
self.collision_spheres = coll_params["collision_spheres"]
@@ -399,7 +404,7 @@ class CudaRobotGenerator(CudaRobotGeneratorConfig):
joint_map = [
-1 if i not in self._controlled_joints else i for i in range(len(self._bodies))
]
] #
joint_map_type = [
-1 if i not in self._controlled_joints else i for i in range(len(self._bodies))
]
@@ -885,4 +890,11 @@ class CudaRobotGenerator(CudaRobotGeneratorConfig):
self.cspace.max_acceleration.unsqueeze(0),
]
)
# clip joint position:
# TODO: change this to be per joint
joint_limits["position"][0] += self.cspace.position_limit_clip
joint_limits["position"][1] -= self.cspace.position_limit_clip
joint_limits["velocity"][0] *= self.cspace.velocity_scale
joint_limits["velocity"][1] *= self.cspace.velocity_scale
self._joint_limits = JointLimits(joint_names=self.joint_names, **joint_limits)

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

@@ -116,7 +116,8 @@ class CudaRobotModelConfig:
@staticmethod
def from_data_dict(
data_dict: Dict[str, Any], tensor_args: TensorDeviceType = TensorDeviceType()
data_dict: Dict[str, Any],
tensor_args: TensorDeviceType = TensorDeviceType(),
):
return CudaRobotModelConfig.from_config(
CudaRobotGeneratorConfig(**data_dict, tensor_args=tensor_args)

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

@@ -34,6 +34,12 @@ class JointType(Enum):
X_ROT = 3
Y_ROT = 4
Z_ROT = 5
X_PRISM_NEG = 6
Y_PRISM_NEG = 7
Z_PRISM_NEG = 8
X_ROT_NEG = 9
Y_ROT_NEG = 10
Z_ROT_NEG = 11
@dataclass
@@ -82,6 +88,7 @@ class CSpaceConfig:
velocity_scale: Union[float, List[float]] = 1.0
acceleration_scale: Union[float, List[float]] = 1.0
jerk_scale: Union[float, List[float]] = 1.0
position_limit_clip: Union[float, List[float]] = 0.01
def __post_init__(self):
if self.retract_config is not None:

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

@@ -95,7 +95,7 @@ class UrdfKinematicsParser(KinematicsParser):
"velocity": joint.limit.velocity,
}
else:
# log_warn("Converting continuous joint to revolute")
log_warn("Converting continuous joint to revolute")
joint_type = "revolute"
joint_limits = {
"effort": joint.limit.effort,
@@ -105,9 +105,7 @@ class UrdfKinematicsParser(KinematicsParser):
}
joint_axis = joint.axis
if (joint_axis < 0).any():
log_warn("Joint axis has negative value (-1). This is not supported.")
joint_axis = np.abs(joint_axis)
body_params["joint_limits"] = [joint_limits["lower"], joint_limits["upper"]]
body_params["joint_velocity_limits"] = [
-1.0 * joint_limits["velocity"],
@@ -128,6 +126,13 @@ class UrdfKinematicsParser(KinematicsParser):
joint_type = JointType.Y_PRISM
if joint_axis[2] == 1:
joint_type = JointType.Z_PRISM
if joint_axis[0] == -1:
joint_type = JointType.X_PRISM_NEG
if joint_axis[1] == -1:
joint_type = JointType.Y_PRISM_NEG
if joint_axis[2] == -1:
joint_type = JointType.Z_PRISM_NEG
elif joint_type == "revolute":
if joint_axis[0] == 1:
joint_type = JointType.X_ROT
@@ -135,6 +140,12 @@ class UrdfKinematicsParser(KinematicsParser):
joint_type = JointType.Y_ROT
if joint_axis[2] == 1:
joint_type = JointType.Z_ROT
if joint_axis[0] == -1:
joint_type = JointType.X_ROT_NEG
if joint_axis[1] == -1:
joint_type = JointType.Y_ROT_NEG
if joint_axis[2] == -1:
joint_type = JointType.Z_ROT_NEG
else:
log_error("Joint type not supported")