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

add articulation & robotiq_2f85 urdf

Browse Source
This commit is contained in:
hofee
2025-10-21 19:57:42 +08:00
parent 65a39a4994
commit 6f43878922
10 changed files with 572 additions and 8413 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
data_gen_dependencies/action/__init__.py
View File

@@ -6,6 +6,7 @@ from .pour import PourStage
from .pull import PullStage
from .push import PushStage
from .pull_revolute import PullRevoluteStage
from .push_revolute import PushRevoluteStage
from .press import PressPrismaticStage
from .twist import TwistStage
@@ -27,7 +28,7 @@ ACTION_STAGE = {
"pull": PullStage,
# ---- Articulate ----
"pull_revolute": PullRevoluteStage,
"push_revolute": NotImplemented,
"push_revolute": PushRevoluteStage,
"pull_prismatic":PullStage,
"push_prismatic":PushStage,
"twist": TwistStage,

2
data_gen_dependencies/action/base.py
View File

@@ -82,7 +82,7 @@ class StageTemplate:
delta_pose = gripper_pose_canonical
gripper_pose = objects['gripper'].obj_pose
target_gripper_pose = gripper_pose @ delta_pose
motion_type = 'Straight'
motion_type = 'Simple'
else:
if gripper_pose_canonical is None:

3
data_gen_dependencies/action/pull_revolute.py
View File

@@ -62,7 +62,6 @@ class PullRevoluteStage(StageTemplate):
self.sub_stages.append([free_delta_pose, None, np.eye(4), 'local_gripper'])
def generate_trajectory(self, gripper2passive_obj, init_vector_direction):
trajectory = []
total_angle = self.joint_position_threshold * (self.joint_upper_limit - self.joint_lower_limit)
@@ -120,7 +119,7 @@ class PullRevoluteStage(StageTemplate):
gripper_trajectory = gripper_trajectory.reshape(-1, 4, 4)
gripper_pts_2_obj = (gripper_trajectory[:, :3, :3] @ gripper_pts[..., None]).squeeze(-1)+ gripper_trajectory[:, :3, 3]
np.savetxt(save_path, gripper_pts_2_obj, fmt="%.6f")
def axis_angle_to_matrix(self, axis, angle):
axis = axis / np.linalg.norm(axis)

10
data_gen_dependencies/action/push_revolute.py
View File

@@ -30,7 +30,7 @@ class PushRevoluteStage(StageTemplate):
self.joint_upper_limit = correspond_joint_info["upper_bound"]
self.joint_axis = correspond_joint_info["joint_axis"]
self.joint_type = correspond_joint_info["joint_type"]
assert self.joint_type == 'revolute', "joint_type must be revolute for pull_revolute action"
assert self.joint_type == 'revolute', "joint_type must be revolute for push_revolute action"
if self.joint_position_threshold is None:
self.joint_position_threshold = 0.7
assert self.joint_position_threshold >= 0 and self.joint_position_threshold <= 1
@@ -50,10 +50,10 @@ class PushRevoluteStage(StageTemplate):
arc_length = abs(self.revolute_radius * total_angle)
step = int(arc_length / self.DELTA_DISTANCE)
from pyboot.utils.log import Log
Log.debug(f'PullRevoluteStage: total_angle={total_angle}, arc_length={arc_length}, step={step}')
Log.debug(f'PushRevoluteStage: total_angle={total_angle}, arc_length={arc_length}, step={step}')
trajectory = self.generate_trajectory(gripper2obj, vector_direction)
self.save_visualized_gripper_trajectory(trajectory, "pull_revolute_traj.txt")
self.save_visualized_gripper_trajectory(trajectory, "push_revolute_traj.txt")
self.sub_stages.append([trajectory, None, np.eye(4), 'Trajectory'])
self.sub_stages.append([None, "open", np.eye(4), 'Simple'])
@@ -66,7 +66,7 @@ class PushRevoluteStage(StageTemplate):
def generate_trajectory(self, gripper2passive_obj, init_vector_direction):
trajectory = []
total_angle = self.joint_position_threshold * (self.joint_upper_limit - self.joint_lower_limit)
total_angle *= self.joint_direction
total_angle *= self.joint_direction
arc_length = abs(self.revolute_radius * total_angle)
num_steps = int(np.ceil(arc_length / self.DELTA_DISTANCE))

1
data_gen_dependencies/action/twist.py
View File

@@ -39,7 +39,6 @@ class TwistStage(StageTemplate):
def generate_substage(self, target_pose, vector_direction):
vector_direction = vector_direction / np.linalg.norm(vector_direction)
delta_twist_pose = self.axis_angle_to_matrix(np.asarray([0,0,1]), self.twist_degree_range * self.joint_position_threshold * self.joint_direction)
import ipdb;ipdb.set_trace()
self.sub_stages.append([delta_twist_pose, None, np.eye(4), 'local_gripper'])
def axis_angle_to_matrix(self, axis, angle):