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

0
.pbignore
View File

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

11
data_gen_dependencies/manip_solver.py
View File

@@ -294,6 +294,8 @@ def parse_stage(stage, objects):
action_mapped = 'grasp'
elif action in ['pull_revolute']:
action_mapped = 'pull'
elif action in ['push_revolute']:
action_mapped = 'push'
elif action in ["press_prismatic"]:
action_mapped = 'press'
else:
@@ -345,7 +347,7 @@ def select_obj(objects, stages, robot):
if stages[0]['action'] in ['push', 'reset', 'press_prismatic']:
gripper2obj = current_gripper_pose
elif stages[0]['action'] in ['pick', 'grasp', 'hook', "pull_revolute", "twist"]:
elif stages[0]['action'] in ['pick', 'grasp', 'hook', "pull_revolute", "twist", "push_revolute"]:
action = stages[0]['action']
''' 筛掉无IK解的grasp pose '''
@@ -353,6 +355,10 @@ def select_obj(objects, stages, robot):
grasp_stage = parse_stage(stages[0], objects)
_, _, passive_obj_id, _, passive_element, _, _ = grasp_stage
grasp_obj_id = passive_obj_id
if stages[0]['action'] == "pull_revolute":
import ipdb;ipdb.set_trace()
if stages[0]['action'] == "push_revolute":
import ipdb;ipdb.set_trace()
grasp_poses_canonical = passive_element['grasp_pose'].copy()
grasp_widths = passive_element['width']
@@ -550,7 +556,7 @@ def generate_action_stages(objects, all_stages, robot):
substages = build_stage(action)(active_obj_id, passive_obj_id, active_elements, passive_elements,
gripper2obj, extra_params=stage.get('extra_params', None), objects=objects)
elif action in ['slide', 'shave', 'brush', 'wipe', 'pull', 'rotate', 'move',
'reset', "pull_revolute", "twist"]: # grasp + 物体自身运动
'reset', "pull_revolute","push_revolute", "twist"]: # grasp + 物体自身运动
passive_element = passive_elements[np.random.choice(len(passive_elements))]
substages = build_stage(action)(
active_obj_id=active_obj_id,
@@ -598,6 +604,7 @@ def generate_action_stages(objects, all_stages, robot):
if len(target_gripper_poses_pass_ik) == 0:
Log.warning(f'{action}: No target_obj_pose can pass isaac curobo IK')
import ipdb;ipdb.set_trace()
continue
target_joint_positions = []

24
data_gen_dependencies/omniagent.py
View File

@@ -205,6 +205,23 @@ class Agent(BaseAgent):
Log.error('-- Grasp file empty: %s' % grasp_file)
return False
return True
def visualize_gripper_pose(self, gripper_pose):
canonical_gripper_pts = np.zeros((60, 3))
for i in range(10):
canonical_gripper_pts[i] = np.array([0, 0.01, 0.002*i])
canonical_gripper_pts[i+10] = np.array([0, 0.01, 0.002*i + 0.001])
canonical_gripper_pts[i+20] = np.array([0, -0.01, 0.002*i])
canonical_gripper_pts[i+30] = np.array([0, -0.01, 0.002*i + 0.001])
canonical_gripper_pts[i+40] = np.array([0, -0.01 + 0.002 * i, 0])
for i in range(5):
canonical_gripper_pts[i+50] = np.array([0, 0, -0.002*i])
canonical_gripper_pts[i+55] = np.array([0, 0, -0.002*i + 0.001])
canonical_gripper_pts -= np.array([0, 0, 0.002*10 + 0.001])
canonical_gripper_pts = canonical_gripper_pts.reshape(-1, 3)
#simport ipdb; ipdb.set_trace()
gripper_pts = (gripper_pose[:3, :3] @ canonical_gripper_pts.T).T + gripper_pose[:3, 3]
return gripper_pts
def run(self, task_list, camera_list, use_recording, workspaces, fps=10, render_semantic=False):
for index, task_file in enumerate(task_list):
@@ -229,7 +246,6 @@ class Agent(BaseAgent):
#print('Reset pose:', self.robot.reset_pose)
task_info = json.load(open(task_file, 'rb'))
import ipdb; ipdb.set_trace()
stages, objects = load_task_solution(task_info)
objects = self.update_objects(objects)
split_stages = split_grasp_stages(stages)
@@ -287,6 +303,11 @@ class Agent(BaseAgent):
target_pose=target_gripper_pose)
if target_gripper_pose is not None:
self.robot.move_pose(target_gripper_pose, motion_type, arm=arm, block=True)
# vis_gripper_pose = self.visualize_gripper_pose(target_gripper_pose)
# import ipdb;ipdb.set_trace()
# np.savetxt('debug_gripper.txt', vis_gripper_pose)
set_gripper_open = gripper_action == 'open'
set_gripper_close = gripper_action == 'close'
@@ -432,6 +453,7 @@ class Agent(BaseAgent):
break
Log.success('Finished executing task.')
time.sleep(0.5)
#import ipdb; ipdb.set_trace()
if self.attached_obj_id is None:
self.robot.client.DetachObj()
self.robot.client.stop_recording()

531
data_gen_dependencies/robot_urdf/Franka_120s_robotiq_2f85.urdf Normal file
View File

@@ -0,0 +1,531 @@
<?xml version="1.0" ?>
<robot name="panda" xmlns:xacro="http://www.ros.org/wiki/xacro">
<link name="base_link"/>
<joint name="panda_fixed" type="fixed">
<origin rpy="0 0 0.0" xyz="0 0 0.0"/>
<parent link="base_link"/>
<child link="panda_link0"/>
<axis xyz="0 0 0"/>
</joint>
<link name="panda_link0">
<visual>
<geometry>
<mesh filename="franka_meshes/visual/link0.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="franka_meshes/collision/link0.obj"/>
</geometry>
</collision>
</link>
<link name="panda_link1">
<visual>
<geometry>
<mesh filename="franka_meshes/visual/link1.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="franka_meshes/collision/link1.obj"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.00033 -0.02204 -0.04762"/>
<mass value="2.797"/>
<inertia ixx="0.0156" ixy="0.0" ixz="0.0" iyy="0.01439" iyz="-0.0024" izz="0.005"/>
</inertial>
</link>
<joint name="panda_joint1" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
<origin rpy="0 0 0" xyz="0 0 0.333"/>
<parent link="panda_link0"/>
<child link="panda_link1"/>
<axis xyz="0 0 1"/>
<dynamics damping="10.0"/>
<limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
</joint>
<link name="panda_link2">
<visual>
<geometry>
<mesh filename="franka_meshes/visual/link2.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="franka_meshes/collision/link2.obj"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.00038 -0.09211 0.01908"/>
<mass value="2.542"/>
<inertia ixx="0.0166" ixy="0.0" ixz="0.0" iyy="0.0046" iyz="0.0035" izz="0.015"/>
</inertial>
</link>
<joint name="panda_joint2" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
<origin rpy="-1.57079632679 0 0" xyz="0 0 0"/>
<parent link="panda_link1"/>
<child link="panda_link2"/>
<axis xyz="0 0 1"/>
<dynamics damping="10.0"/>
<limit effort="87" lower="-1.7628" upper="1.7628" velocity="2.1750"/>
</joint>
<link name="panda_link3">
<visual>
<geometry>
<mesh filename="franka_meshes/visual/link3.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="franka_meshes/collision/link3.obj"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.05152 0.01696 -0.02971"/>
<mass value="2.2513"/>
<inertia ixx="0.006" ixy="0.009" ixz="0.003" iyy="0.0086" iyz="0.009" izz="0.0065"/>
</inertial>
</link>
<joint name="panda_joint3" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
<origin rpy="1.57079632679 0 0" xyz="0 -0.316 0"/>
<parent link="panda_link2"/>
<child link="panda_link3"/>
<axis xyz="0 0 1"/>
<dynamics damping="10.0"/>
<limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750"/>
</joint>
<link name="panda_link4">
<visual>
<geometry>
<mesh filename="franka_meshes/visual/link4.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="franka_meshes/collision/link4.obj"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.05113 0.05825 0.01698"/>
<mass value="2.2037"/>
<inertia ixx="0.0156" ixy="0.0" ixz="0.0" iyy="0.01439" iyz="-0.0024" izz="0.005"/>
</inertial>
</link>
<joint name="panda_joint4" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
<origin rpy="1.57079632679 0 0" xyz="0.0825 0 0"/>
<parent link="panda_link3"/>
<child link="panda_link4"/>
<axis xyz="0 0 1"/>
<dynamics damping="10.0"/>
<limit effort="87" lower="-3.0718" upper="-0.0698" velocity="2.1750"/>
<!-- something is weird with this joint limit config
<dynamics damping="10.0"/>
<limit effort="87" lower="-3.0" upper="0.087" velocity="2.1750"/> -->
</joint>
<link name="panda_link5">
<visual>
<geometry>
<mesh filename="franka_meshes/visual/link5.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="franka_meshes/collision/link5.obj"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.00005 0.03730 -0.09280"/>
<mass value="2.2855"/>
<inertia ixx="0.02297014781" ixy="0.0" ixz="0.0" iyy="0.02095060919" iyz="0.00382345782" izz="0.00430606551"/>
</inertial>
</link>
<joint name="panda_joint5" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
<origin rpy="-1.57079632679 0 0" xyz="-0.0825 0.384 0"/>
<parent link="panda_link4"/>
<child link="panda_link5"/>
<axis xyz="0 0 1"/>
<dynamics damping="10.0"/>
<limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
</joint>
<link name="panda_link6">
<visual>
<geometry>
<mesh filename="franka_meshes/visual/link6.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="franka_meshes/collision/link6.obj"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.06572 -0.00371 0.00153"/>
<mass value="1.353"/>
<inertia ixx="0.00087964522" ixy="-0.00021487814" ixz="-0.00011911662" iyy="0.00277796968" iyz="0.00001274322" izz="0.00286701969"/>
</inertial>
</link>
<joint name="panda_joint6" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
<origin rpy="1.57079632679 0 0" xyz="0 0 0"/>
<parent link="panda_link5"/>
<child link="panda_link6"/>
<axis xyz="0 0 1"/>
<dynamics damping="10.0"/>
<limit effort="12" lower="-0.0175" upper="3.7525" velocity="2.6100"/>
<!-- <dynamics damping="10.0"/>
<limit effort="12" lower="-0.0873" upper="3.0" velocity="2.6100"/> -->
</joint>
<link name="panda_link7">
<visual>
<geometry>
<mesh filename="franka_meshes/visual/link7.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="franka_meshes/collision/link7.obj"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.00089 -0.00044 0.05491"/>
<mass value="0.35973"/>
<inertia ixx="0.00019541063" ixy="0.00000165231" ixz="0.00000148826" iyy="0.00019210361" iyz="-0.00000131132" izz="0.00017936256"/>
</inertial>
</link>
<joint name="panda_joint7" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
<origin rpy="1.57079632679 0 0" xyz="0.088 0 0"/>
<parent link="panda_link6"/>
<child link="panda_link7"/>
<axis xyz="0 0 1"/>
<dynamics damping="10.0"/>
<limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
</joint>
<link name="panda_link8"/>
<joint name="panda_joint8" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0.107"/>
<parent link="panda_link7"/>
<child link="panda_link8"/>
<axis xyz="0 0 0"/>
</joint>
<link name="flange">
<visual>
<geometry>
<mesh filename="flange_meshes/flange.obj"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="flange_meshes/flange.obj"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.0 0 0.0"/>
<mass value="0.1"/>
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
</inertial>
</link>
<link name="zed_mini_camera">
<visual>
<geometry>
<mesh filename="zed_mini_camera_meshes/zed_mini_camera.obj" scale="0.001 0.001 0.001"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="zed_mini_camera_meshes/zed_mini_camera.obj" scale="0.001 0.001 0.001"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.0 0 0.0"/>
<mass value="0.1"/>
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001"/>
</inertial>
</link>
<joint name="panda_zed_mini_camera_joint" type="fixed">
<parent link="panda_link8"/>
<child link="zed_mini_camera"/>
<origin xyz="0 0 -0.01" rpy="-1.57079632679 -1.57079632679 0"/>
</joint>
<!-- flange 连接在 link8 上 -->
<joint name="panda_flange_joint" type="fixed">
<origin rpy="-1.57079632679 0 0" xyz="0 0 0.003"/>
<parent link="panda_link8"/>
<child link="flange"/>
</joint>
<!-- gripper 连接在 flange 上 -->
<joint name="flange_robotiq_fix_joint" type="fixed">
<origin rpy="1.57079632679 1.57079632679 0" xyz="0 -0.0035 0"/>
<parent link="flange"/>
<child link="robotiq_85_base_link"/>
</joint>
<link name="robotiq_85_base_link">
<visual>
<geometry>
<mesh filename="robotiq_2f85_meshes/visual/2f_85/robotiq_base.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="robotiq_2f85_meshes/collision/2f_85/robotiq_base.stl"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.0 2.274e-05 0.03232288"/>
<mass value="6.6320197e-01"/>
<inertia ixx="5.1617816e-04" ixy="2.936e-8" ixz="0.0" iyy="5.8802208e-04" iyz="-3.2296e-7" izz="3.9462776e-04"/>
</inertial>
</link>
<link name="robotiq_85_left_knuckle_link">
<visual>
<geometry>
<mesh filename="robotiq_2f85_meshes/visual/2f_85/left_knuckle.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="robotiq_2f85_meshes/collision/2f_85/left_knuckle.stl"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.01213197 0.0002 -0.00058647"/>
<mass value="1.384773208e-02"/>
<inertia ixx="3.5232e-7" ixy="0.0" ixz="1.1744e-7" iyy="2.31944e-6" iyz="0" izz="2.23136e-6"/>
</inertial>
</link>
<link name="robotiq_85_right_knuckle_link">
<visual>
<geometry>
<mesh filename="robotiq_2f85_meshes/visual/2f_85/right_knuckle.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="robotiq_2f85_meshes/collision/2f_85/right_knuckle.stl"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.01213197 0.0002 -0.00058647"/>
<mass value="1.384773208e-02"/>
<inertia ixx="3.5232e-7" ixy="0.0" ixz="-1.1744e-7" iyy="2.31944e-6" iyz="0.0" izz="2.23136e-6"/>
</inertial>
</link>
<link name="robotiq_85_left_finger_link">
<visual>
<geometry>
<mesh filename="robotiq_2f85_meshes/visual/2f_85/left_finger.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="robotiq_2f85_meshes/collision/2f_85/left_finger.stl"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.00346899 -0.00079447 0.01867121"/>
<mass value="4.260376752e-02"/>
<inertia ixx="1.385792000000000e-05" ixy="0.0" ixz="-2.17264e-06" iyy="1.183208e-05" iyz="0.0" izz="5.19672e-06"/>
</inertial>
</link>
<link name="robotiq_85_right_finger_link">
<visual>
<geometry>
<mesh filename="robotiq_2f85_meshes/visual/2f_85/right_finger.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="robotiq_2f85_meshes/collision/2f_85/right_finger.stl"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.00346899 -5.53e-06 0.01867121"/>
<mass value="4.260376752000000e-02"/>
<inertia ixx="1.385792e-05" ixy="0.0" ixz="2.17264e-06" iyy="1.183208e-05" iyz="0.0" izz="5.19672e-06"/>
</inertial>
</link>
<link name="robotiq_85_left_inner_knuckle_link">
<visual>
<geometry>
<mesh filename="robotiq_2f85_meshes/visual/2f_85/left_inner_knuckle.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="robotiq_2f85_meshes/collision/2f_85/left_inner_knuckle.stl"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.01897699 0.00015001 0.02247101"/>
<mass value="2.969376448e-02"/>
<inertia ixx="9.57136e-06" ixy="0.0" ixz="-3.93424e-06" iyy="8.69056e-06" iyz="0.0" izz="8.19144e-06"/>
</inertial>
</link>
<link name="robotiq_85_right_inner_knuckle_link">
<visual>
<geometry>
<mesh filename="robotiq_2f85_meshes/visual/2f_85/right_inner_knuckle.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="robotiq_2f85_meshes/collision/2f_85/right_inner_knuckle.stl"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.01926824 5.001e-05 0.02222178"/>
<mass value="2.969376448e-02"/>
<inertia ixx="9.42456e-06" ixy="0.0" ixz="3.9636e-06" iyy="8.69056e-06" iyz="0.0" izz="8.33824e-06"/>
</inertial>
</link>
<link name="robotiq_85_left_finger_tip_link">
<visual>
<geometry>
<mesh filename="robotiq_2f85_meshes/visual/2f_85/left_finger_tip.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="robotiq_2f85_meshes/collision/2f_85/left_finger_tip.stl"/>
</geometry>
<surface>
<friction>
<ode>
<mu1>100000.0</mu1>
<mu2>100000.0</mu2>
</ode>
</friction>
<contact>
<ode>
<kp>1e+5</kp>
<kd>1</kd>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<minDepth>0.002</minDepth>
<maxVel>0</maxVel>
</ode>
</contact>
</surface>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.01456706 -0.0008 0.01649701"/>
<mass value="4.268588744e-02"/>
<inertia ixx="1.048152e-05" ixy="0.0" ixz="3.5232e-6" iyy="1.197888e-05" iyz="0.0" izz="4.22784e-06"/>
</inertial>
</link>
<link name="robotiq_85_right_finger_tip_link">
<visual>
<geometry>
<mesh filename="robotiq_2f85_meshes/visual/2f_85/right_finger_tip.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="robotiq_2f85_meshes/collision/2f_85/right_finger_tip.stl"/>
</geometry>
<surface>
<friction>
<ode>
<mu1>100000.0</mu1>
<mu2>100000.0</mu2>
</ode>
</friction>
<contact>
<ode>
<kp>1e+5</kp>
<kd>1</kd>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<minDepth>0.002</minDepth>
<maxVel>0</maxVel>
</ode>
</contact>
</surface>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.01456706 5e-05 0.01649701"/>
<mass value="4.268588744e-02"/>
<inertia ixx="1.048152e-05" ixy="0.0" ixz="-3.5232e-06" iyy="1.197888e-05" iyz="0.0" izz="4.22784e-06"/>
</inertial>
</link>
<joint name="robotiq_85_left_knuckle_joint" type="revolute">
<parent link="robotiq_85_base_link"/>
<child link="robotiq_85_left_knuckle_link"/>
<axis xyz="0 -1 0"/>
<origin rpy="0 0 0" xyz="0.03060114 0.0 0.05490452"/>
<limit effort="50" lower="0.0" upper="0.8" velocity="0.5"/>
</joint>
<joint name="robotiq_85_right_knuckle_joint" type="revolute">
<parent link="robotiq_85_base_link"/>
<child link="robotiq_85_right_knuckle_link"/>
<axis xyz="0 -1 0"/>
<origin rpy="0 0 0" xyz="-0.03060114 0.0 0.05490452"/>
<limit effort="50" lower="-0.8" upper="0.0" velocity="0.5"/>
</joint>
<joint name="robotiq_85_left_finger_joint" type="fixed">
<parent link="robotiq_85_left_knuckle_link"/>
<child link="robotiq_85_left_finger_link"/>
<origin rpy="0 0 0" xyz="0.03152616 0.0 -0.00376347"/>
</joint>
<joint name="robotiq_85_right_finger_joint" type="fixed">
<parent link="robotiq_85_right_knuckle_link"/>
<child link="robotiq_85_right_finger_link"/>
<origin rpy="0 0 0" xyz="-0.03152616 0.0 -0.00376347"/>
</joint>
<joint name="robotiq_85_left_inner_knuckle_joint" type="revolute">
<parent link="robotiq_85_base_link"/>
<child link="robotiq_85_left_inner_knuckle_link"/>
<axis xyz="0 -1 0"/>
<origin rpy="0 0 0" xyz="0.0127 0.0 0.06142"/>
<limit effort="50" lower="0" upper="0.8" velocity="0.5"/>
</joint>
<joint name="robotiq_85_right_inner_knuckle_joint" type="revolute">
<parent link="robotiq_85_base_link"/>
<child link="robotiq_85_right_inner_knuckle_link"/>
<axis xyz="0 -1 0"/>
<origin rpy="0 0 0" xyz="-0.0127 0.0 0.06142"/>
<limit effort="50" lower="-0.8" upper="0" velocity="0.5"/>
</joint>
<joint name="robotiq_85_left_finger_tip_joint" type="revolute">
<parent link="robotiq_85_left_finger_link"/>
<child link="robotiq_85_left_finger_tip_link"/>
<axis xyz="0 -1 0"/>
<origin rpy="0 0 0" xyz="0.00563134 0.0 0.04718515"/>
<limit effort="50" lower="-0.8" upper="0" velocity="0.5"/>
</joint>
<joint name="robotiq_85_right_finger_tip_joint" type="revolute">
<parent link="robotiq_85_right_finger_link"/>
<child link="robotiq_85_right_finger_tip_link"/>
<axis xyz="0 -1 0"/>
<origin rpy="0 0 0" xyz="-0.00563134 0.0 0.04718515"/>
<limit effort="50" lower="0" upper="0.8" velocity="0.5"/>
</joint>
<link name="gripper_center"/>
<joint name="ee_fixed_joint" type="fixed">
<parent link="robotiq_85_base_link"/>
<child link="gripper_center"/>
<origin rpy="0 0.0 -1.57079632679" xyz="0.0 0. 0.1635"/>
</joint>
</robot>

8400
pull_revolute_traj.txt
View File

File diff suppressed because it is too large Load Diff