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

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))