Compare commits
52
Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 LiuzhengSJ
|
d1e096cb3d | ||
|
LiuzhengSJ
|
60eb37b0ed | ||
|
LiuzhengSJ
|
c0e181cc50 | ||
|
LiuzhengSJ
|
2e25951bc0 | ||
|
LiuzhengSJ
|
d86438115f | ||
|
LiuzhengSJ
|
8ff29b1cc9 | ||
|
LiuzhengSJ
|
0c121a7425 | ||
|
LiuzhengSJ
|
e001f2e1f1 | ||
|
LiuzhengSJ
|
ace5dea9a2 | ||
|
LiuzhengSJ
|
2713c54707 | ||
|
LiuzhengSJ
|
9849466430 | ||
|
LiuzhengSJ
|
86c40ee380 | ||
|
LiuzhengSJ
|
9513dfa4ce | ||
|
LiuzhengSJ
|
e1d833812b | ||
|
LiuzhengSJ
|
ceb80a8b17 | ||
|
LiuzhengSJ
|
d1080638c1 | ||
|
emboddied
|
5cac8c5a73 | ||
|
LiuzhengSJ
|
d0ca4d6115 | ||
|
LiuzhengSJ
|
579abe6b67 | ||
|
emboddied
|
2bd6bf510f | ||
|
LiuzhengSJ
|
a58e1b9f59 | ||
|
LiuzhengSJ
|
6a50db91a4 | ||
|
emboddied
|
c6458248a7 | ||
|
LiuzhengSJ
|
4add432f53 | ||
|
LiuzhengSJ
|
58e84c6a33 | ||
 ZhengLiu-cart
|
7050c93c84 | ||
|
ZhengLiu-cart
|
6db8b2f254 | ||
|
ZhengLiu-cart
|
a5fb40d1a6 | ||
|
ZhengLiu-cart
|
223b29f37d | ||
|
emboddied
|
cb51ecf2eb | ||
|
emboddied
|
75ba51c609 | ||
|
LiuzhengSJ
|
74d1623b8a | ||
|
LiuzhengSJ
|
b32199e316 | ||
|
LiuzhengSJ
|
e06e48f21b | ||
|
LiuzhengSJ
|
fb414078f1 | ||
|
LiuzhengSJ
|
12ead6a191 | ||
|
LiuzhengSJ
|
319c1765bc | ||
|
LiuzhengSJ
|
dfaeb95282 | ||
|
LiuzhengSJ
|
7246710a7d | ||
|
ZhengLiu-cart
|
4b5eeccf7f | ||
|
LiuzhengSJ
|
f1846ffe1e | ||
|
LiuzhengSJ
|
58452bce90 | ||
|
LiuzhengSJ
|
6c8a335e1d | ||
|
LiuzhengSJ
|
2ca5033b46 | ||
|
LiuzhengSJ
|
aefc7bacd5 | ||
|
LiuzhengSJ
|
ddbbb1746e | ||
|
LiuzhengSJ
|
48453fa5c8 | ||
|
LiuzhengSJ
|
cee1a191ea | ||
|
LiuzhengSJ
|
f10152fc29 | ||
|
LiuzhengSJ
|
c0458f0e12 | ||
|
LiuzhengSJ
|
0bed2f87f4 | ||
|
LiuzhengSJ
|
fb64f3c73a |
@@ -1,14 +1,42 @@
|
||||
### This repo is for inverse kinematics and verification
|
||||
|
||||
Inverse Kinematics (IK) is numerically obtained through quadratic programming (QP).
|
||||
In this branch, the **integrated** inverse kinematics method is packed into one python class **rm75_kinematics** in ``rm75_kinematics.py``.
|
||||
|
||||
Verification is done with Mujoco simulation.
|
||||
The user can call it as in `test1.py`, `test2.py`.
|
||||
|
||||
Key specifications:
|
||||
1. Time consumption.
|
||||
2. Success rate
|
||||
3. Minial joint variation.
|
||||
How to use
|
||||
|
||||
Next:\
|
||||
Comparison with Realman official IK method.
|
||||
Embedded with current demo.
|
||||
```aiignore
|
||||
from rm75_kinematics import rm75_kinematics
|
||||
robot_kine = rm75_kinematics(urdf_path='./urdf_rm75/RM75-SCI.urdf',
|
||||
mesh_dir='./urdf_rm75',
|
||||
tcps=["scissor_tcp", "camera_tcp"],
|
||||
tools_in_ee=tools_in_ee,min_j=lb, max_j=ub)
|
||||
```
|
||||
|
||||
Parameter definition:
|
||||
|
||||
- urdf_path: the robot description file, ending with `.urdf`.
|
||||
- mesh_dir: the robot parts, ending with `.stl`.
|
||||
- tcps: the tool central points defined in urdf file, if no, ignore it.
|
||||
- tools_in_ee: the installation of different tools attached to the end-effector of the arm (jont7+link7).
|
||||
|
||||
|
||||
### Current functions ###
|
||||
|
||||
1. `get_ik_result`
|
||||
```aiignore
|
||||
ret_ik, q = robot_kine.get_ik_result(target_position=[0.2, -0.2 , 0.5 ],
|
||||
target_rpy=[0.2022060487764064, -0.0097962261845583, -0.6518417572686532],
|
||||
initial_guess=[0.1] * 7, tool=tool_name)
|
||||
```
|
||||
|
||||
2. `get_fk_result`
|
||||
```aiignore
|
||||
p = robot_kine.get_fk_result(joint_angles=q,tool=tool_name)
|
||||
```
|
||||
|
||||
3. `get_self_collision`
|
||||
```aiignore
|
||||
self_collision_sts = robot_kine.get_self_collision(q)
|
||||
```
|
||||
@@ -1,195 +0,0 @@
|
||||
|
||||
# conda activate coppeliasim
|
||||
# env fix, in terminal: ~/fix_robotics_env.sh
|
||||
|
||||
from rm75_kine_qp import KinematicsSolver as kine_ctrl
|
||||
from rm75_mjc import MuJoCoPositionController
|
||||
from Robotic_Arm.rm_robot_interface import *
|
||||
|
||||
import time
|
||||
from math import radians, degrees, pi, cos, sin
|
||||
import numpy as np
|
||||
|
||||
def demo_position_control():
|
||||
"""Demonstrate pure position control"""
|
||||
|
||||
urdf_path = "/home/zl/Downloads/urdf_rm75/RM75-B.urdf"
|
||||
|
||||
|
||||
|
||||
# Create controller
|
||||
robot_mjk = MuJoCoPositionController(urdf_path, smoothness=0.05, enable_viewer=True)
|
||||
robot_mjk.start()
|
||||
time.sleep(1)
|
||||
|
||||
print("\n[Test 1] Move joint 1 to 45 degrees")
|
||||
robot_mjk.send_command([0.785, 0, 0, 0, 0, 0, 0])
|
||||
robot_mjk.wait_until_reached()
|
||||
robot_mjk.print_state()
|
||||
time.sleep(0.5)
|
||||
|
||||
print("\n[Test 2] Move joint 2 to -30 degrees")
|
||||
robot_mjk.send_command([0, -0.524, 0, 0, 0, 0, 0])
|
||||
robot_mjk.wait_until_reached()
|
||||
robot_mjk.print_state()
|
||||
time.sleep(0.5)
|
||||
|
||||
print("\n[Test 3] Move multiple joints simultaneously")
|
||||
robot_mjk.send_command([0.5, -0.4, 0.3, 0.2, 0.1, 0, 0])
|
||||
robot_mjk.wait_until_reached()
|
||||
robot_mjk.print_state()
|
||||
time.sleep(0.5)
|
||||
|
||||
print("\n[Test 4] Return home")
|
||||
robot_mjk.send_command([0, 0, 0, 0, 0, 0, 0])
|
||||
robot_mjk.wait_until_reached()
|
||||
robot_mjk.print_state()
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
robot_kine = kine_ctrl()
|
||||
|
||||
# Test 1: Forward Kinematics
|
||||
print("\n1. Forward Kinematics Test")
|
||||
print("-" * 40)
|
||||
|
||||
tool_name = "scissor"
|
||||
joint_angles_zero = [0.1] * 7
|
||||
fk_result = robot_kine.forward_kinematics(joint_angles_zero, tool=tool_name)
|
||||
|
||||
# Test 2: Inverse Kinematics with more reachable target
|
||||
print("\n2. Inverse Kinematics Test")
|
||||
print("-" * 40)
|
||||
|
||||
# Try a simpler target first
|
||||
target_pos = [0.3, 0.2, 0.4] # More reachable position
|
||||
target_rpy = [0.0, 0.0, radians(45)] # Simpler orientation
|
||||
|
||||
print(f"Target: ({target_pos[0]:.3f}, {target_pos[1]:.3f}, {target_pos[2]:.3f}) m")
|
||||
|
||||
init_joints = [0.2] * 7
|
||||
time0 = time.time()
|
||||
for ii in range(100):
|
||||
joint_solution, success, error = robot_kine.inverse_kinematics(
|
||||
target_pos, target_rpy=target_rpy, initial_guess=init_joints,
|
||||
max_iter=500, debug=False, tool=tool_name
|
||||
)
|
||||
time1 = time.time()
|
||||
print(f"Time: {time1 - time0}")
|
||||
|
||||
if success:
|
||||
print(f"✓ Solution found! Error: {error:.6f} m")
|
||||
for i, angle in enumerate(joint_solution):
|
||||
print(f" Joint {i + 1}: {degrees(angle):7.2f}°")
|
||||
|
||||
# Verify
|
||||
fk_verify = robot_kine.forward_kinematics(joint_solution,tool=tool_name)
|
||||
print(
|
||||
f" Position: ({fk_verify['position'][0]:.3f}, {fk_verify['position'][1]:.3f}, {fk_verify['position'][2]:.3f}) m")
|
||||
else:
|
||||
print("✗ IK failed to find a solution!")
|
||||
|
||||
# Test 3: Jacobian
|
||||
print("\n3. Jacobian Matrix")
|
||||
print("-" * 40)
|
||||
|
||||
J = robot_kine.compute_jacobian(joint_angles_zero, tool=tool_name)
|
||||
print(f"Jacobian shape: {J.shape}")
|
||||
for i in range(min(3, J.shape[0])):
|
||||
row_str = " ".join([f"{J[i, j]:7.3f}" for j in range(7)])
|
||||
print(f" Row {i + 1}: {row_str}")
|
||||
|
||||
# Test 4: Trajectory Planning with reachable positions
|
||||
print("\n4. Cartesian Trajectory Planning")
|
||||
print("-" * 40)
|
||||
|
||||
start_pos = [0.3, 0.0, 0.4] # Start position
|
||||
end_pos = [0.3, 0.0, 0.55] # End position (smaller movement)
|
||||
|
||||
fk0 = robot_kine.forward_kinematics([0.1] * 7, tool=tool_name)
|
||||
|
||||
trajectory = robot_kine.plan_cartesian_trajectory(
|
||||
start_pos,
|
||||
end_pos,
|
||||
start_rpy=fk0['rpy'],
|
||||
end_rpy=[
|
||||
fk0['rpy'][0] + radians(10),
|
||||
fk0['rpy'][1],
|
||||
fk0['rpy'][2]
|
||||
],
|
||||
num_steps=10,
|
||||
tool=tool_name
|
||||
)
|
||||
|
||||
if trajectory:
|
||||
print(f"\n✓ Generated {len(trajectory)} waypoints")
|
||||
|
||||
if success:
|
||||
print("✓ Inverse kinematics working (with simplified target)")
|
||||
else:
|
||||
print("⚠ Inverse kinematics may need tuning - try different targets")
|
||||
|
||||
|
||||
print("\n" + "=" * 60)
|
||||
print(f'test subchain Jacobian, for future obstacle avoidance')
|
||||
frame_names = [
|
||||
"link_2",
|
||||
"link_4",
|
||||
"link_7"
|
||||
]
|
||||
Js_sub = robot_kine.get_subchain_jacobian(
|
||||
joint_angles=joint_angles_zero,
|
||||
frame_names=frame_names
|
||||
)
|
||||
print(f'Js_sub: {Js_sub}')
|
||||
|
||||
|
||||
# ---------- rm75 official algorithm -----------
|
||||
arm_model = rm_robot_arm_model_e.RM_MODEL_RM_75_E # RM_65 Robotic arm
|
||||
force_type = rm_force_type_e.RM_MODEL_RM_B_E # Standard version
|
||||
# Initialize the robotic arm model and sensor type in the algorithm
|
||||
robot_kine_rm = Algo(arm_model, force_type)
|
||||
frame = rm_frame_t("work", [0.0, 0.0, 0.0, 0.0, 0, 0.0])
|
||||
robot_kine_rm.rm_algo_set_workframe(frame)
|
||||
print(robot_kine_rm.rm_algo_get_curr_workframe())
|
||||
frame = rm_frame_t("work", [0.0, 0.0, 0.0, 0.0, 0, 0.0])
|
||||
robot_kine_rm.rm_algo_set_toolframe(frame)
|
||||
print(robot_kine_rm.rm_algo_get_curr_toolframe())
|
||||
joint_max_limit = np.array([
|
||||
3.14159, 2.2689, 3.14159, 2.3562, 3.14159, 2.234, 3.14159
|
||||
])*57
|
||||
robot_kine_rm.rm_algo_set_joint_max_limit(joint_max_limit.tolist())
|
||||
joint_min_limit = np.array([
|
||||
-3.14159, -2.2689, -3.14159, -2.3562, -3.14159, -2.234, -3.14159
|
||||
]) * 57
|
||||
robot_kine_rm.rm_algo_set_joint_min_limit(joint_max_limit.tolist())
|
||||
|
||||
q_ref = [0.0, 110.0, 20.0, 40.0, 30.0, 180.0, 20.0]
|
||||
ret, phi = robot_kine_rm.rm_algo_calculate_arm_angle_from_config_rm75(q_ref)
|
||||
params = rm_inverse_kinematics_params_t([0.0, 110.0, 20.0, 40.0, 30.0, 180.0, 20.0],
|
||||
[0.3, 0.0, 0.3, 3.14, 0.0, 3.14], 1)
|
||||
ret, q_out = robot_kine_rm.rm_algo_inverse_kinematics_rm75_for_arm_angle(params, phi)
|
||||
print(f"rm_algo_inverse_kinematics_rm75_for_arm_angle ret: {ret} q_out: {q_out}")
|
||||
|
||||
|
||||
|
||||
|
||||
try:
|
||||
while robot_mjk.viewer and robot_mjk.viewer.is_running():
|
||||
time.sleep(0.1)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
robot_mjk.stop()
|
||||
|
||||
|
||||
|
||||
def main():
|
||||
|
||||
demo_position_control()
|
||||
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -1,803 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
import sys
|
||||
import os
|
||||
|
||||
import pinocchio as pin
|
||||
import numpy as np
|
||||
import osqp
|
||||
from scipy import sparse
|
||||
from math import radians, degrees, pi, cos, sin
|
||||
import time
|
||||
|
||||
|
||||
|
||||
class KinematicsSolver():
|
||||
def __init__(self,urdf_path="/home/zl/Downloads/urdf_rm75/RM75-B.urdf", mesh_dir="/home/zl/Downloads/meshes"):
|
||||
"""
|
||||
for realman 75b
|
||||
Initialize robotic arm kinematics using Pinocchio (ROS2 version).
|
||||
"""
|
||||
|
||||
self.model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_path, mesh_dir)
|
||||
|
||||
# -------------------------------------------------
|
||||
# ee
|
||||
# -------------------------------------------------
|
||||
ee_offset = pin.SE3(np.eye(3), np.array([0, 0, 0.0]))
|
||||
self.model.addFrame(
|
||||
pin.Frame(
|
||||
"ee",
|
||||
self.model.getJointId("joint_7"),
|
||||
self.model.getFrameId("link_7"),
|
||||
ee_offset,
|
||||
pin.FrameType.OP_FRAME
|
||||
)
|
||||
)
|
||||
|
||||
|
||||
# -------------------------------------------------
|
||||
# Scissor tool
|
||||
# -------------------------------------------------
|
||||
scissor_offset = pin.SE3(
|
||||
np.eye(3),
|
||||
np.array([0.0, 0.0, 0.144])
|
||||
)
|
||||
self.model.addFrame(
|
||||
pin.Frame(
|
||||
"scissor_tcp",
|
||||
self.model.getJointId("joint_7"),
|
||||
self.model.getFrameId("link_7"),
|
||||
scissor_offset,
|
||||
pin.FrameType.OP_FRAME
|
||||
)
|
||||
)
|
||||
|
||||
# -------------------------------------------------
|
||||
# Camera tool
|
||||
# -------------------------------------------------
|
||||
camera_rotation = pin.rpy.rpyToMatrix(
|
||||
radians(-90),
|
||||
0,
|
||||
radians(-90)
|
||||
)
|
||||
camera_offset = pin.SE3(
|
||||
camera_rotation,
|
||||
np.array([0.05, 0.02, 0.10])
|
||||
)
|
||||
self.model.addFrame(
|
||||
pin.Frame(
|
||||
"camera_frame",
|
||||
self.model.getJointId("joint_7"),
|
||||
self.model.getFrameId("link_7"),
|
||||
camera_offset,
|
||||
pin.FrameType.OP_FRAME
|
||||
)
|
||||
)
|
||||
|
||||
# -------------------------------------------------
|
||||
# Store tool frame IDs
|
||||
# -------------------------------------------------
|
||||
|
||||
self.tool_frames = {
|
||||
"scissor": self.model.getFrameId("scissor_tcp"),
|
||||
"camera": self.model.getFrameId("camera_frame"),
|
||||
"ee": self.model.getFrameId("ee")
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
self.data = self.model.createData()
|
||||
|
||||
# Joint limits (radians) - expanded for better reachability
|
||||
self.lower_limits = np.array([
|
||||
-3.14159, -2.2689, -3.14159, -2.3562, -3.14159, -2.234, -3.14159
|
||||
])
|
||||
self.upper_limits = np.array([
|
||||
3.14159, 2.2689, 3.14159, 2.3562, 3.14159, 2.234, 3.14159
|
||||
])
|
||||
|
||||
# Set joint limits in the model
|
||||
for i in range(7):
|
||||
self.model.lowerPositionLimit[i] = self.lower_limits[i]
|
||||
self.model.upperPositionLimit[i] = self.upper_limits[i]
|
||||
|
||||
# ---------- for reused qp_solver ------------------
|
||||
self.nv = 7
|
||||
|
||||
# Full dense symmetric matrix structure
|
||||
P_template = np.triu(np.ones((7, 7)))
|
||||
|
||||
P_sparse = sparse.csc_matrix(P_template)
|
||||
|
||||
A_sparse = sparse.eye(7, format='csc')
|
||||
|
||||
self.osqp_solver = osqp.OSQP()
|
||||
|
||||
self.osqp_solver.setup(
|
||||
P=P_sparse,
|
||||
q=np.zeros(7),
|
||||
A=A_sparse,
|
||||
l=-np.ones(7),
|
||||
u=np.ones(7),
|
||||
verbose=False,
|
||||
warm_start=True,
|
||||
polish=False
|
||||
)
|
||||
|
||||
self.W = np.diag([1, 1, 1, 0.2, 0.2, 0.2])
|
||||
|
||||
|
||||
def forward_kinematics(self, joint_angles, tool="ee"):
|
||||
"""
|
||||
Compute forward kinematics.
|
||||
|
||||
Args:
|
||||
joint_angles: List or array of 7 joint angles (radians)
|
||||
tool: Name of frame to compute
|
||||
|
||||
Returns:
|
||||
dict: Position, rotation, rpy, quaternion
|
||||
"""
|
||||
if len(joint_angles) != 7:
|
||||
raise ValueError(f"RM75 has 7 joints, got {len(joint_angles)}")
|
||||
|
||||
# Create configuration vector
|
||||
q = pin.neutral(self.model)
|
||||
for i, angle in enumerate(joint_angles):
|
||||
q[i] = angle
|
||||
|
||||
# Compute forward kinematics
|
||||
pin.forwardKinematics(self.model, self.data, q)
|
||||
pin.updateFramePlacements(self.model, self.data)
|
||||
|
||||
# Get frame transform
|
||||
frame_id = self.tool_frames[tool]
|
||||
frame_transform = self.data.oMf[frame_id]
|
||||
|
||||
# Extract results
|
||||
position = frame_transform.translation.copy()
|
||||
rotation = frame_transform.rotation.copy()
|
||||
|
||||
# Compute RPY
|
||||
rpy = pin.rpy.matrixToRpy(rotation)
|
||||
|
||||
# Compute quaternion
|
||||
quat = pin.Quaternion(rotation)
|
||||
|
||||
return {
|
||||
'position': position,
|
||||
'rotation': rotation,
|
||||
'rpy': rpy,
|
||||
'quaternion': [quat.x, quat.y, quat.z, quat.w],
|
||||
'transform': frame_transform
|
||||
}
|
||||
|
||||
def inverse_kinematics(self, target_position, target_rpy=None,
|
||||
target_quat=None, initial_guess=None,
|
||||
max_iter=200, tolerance=1e-3, debug=False, tool="ee"):
|
||||
"""
|
||||
Compute inverse kinematics using differential IK with multiple strategies.
|
||||
|
||||
Args:
|
||||
target_position: [x, y, z] target position (meters)
|
||||
target_rpy: [roll, pitch, yaw] target orientation (radians)
|
||||
target_quat: [x, y, z, w] target orientation as quaternion
|
||||
initial_guess: Initial joint angles (radians)
|
||||
max_iter: Maximum iterations
|
||||
tolerance: Error tolerance
|
||||
debug: Print debug information
|
||||
tool: the frame name ('scissor', 'camera', 'ee')
|
||||
|
||||
Returns:
|
||||
tuple: (joint_angles, success, error)
|
||||
"""
|
||||
# Build target SE3 placement
|
||||
if target_quat is not None:
|
||||
quat = pin.Quaternion(target_quat[3], target_quat[0],
|
||||
target_quat[1], target_quat[2])
|
||||
target_rotation = quat.matrix()
|
||||
elif target_rpy is not None:
|
||||
target_rotation = pin.rpy.rpyToMatrix(target_rpy[0],
|
||||
target_rpy[1],
|
||||
target_rpy[2])
|
||||
else:
|
||||
target_rotation = np.eye(3)
|
||||
|
||||
target_placement = pin.SE3(target_rotation, np.array(target_position))
|
||||
|
||||
# Try multiple initial guesses
|
||||
initial_guesses = []
|
||||
|
||||
if initial_guess is not None:
|
||||
initial_guesses.append(initial_guess)
|
||||
else:
|
||||
# Try different initial configurations
|
||||
initial_guesses.append([0.1] * 7) # Zero config
|
||||
initial_guesses.append([radians(30), radians(45), radians(30),
|
||||
radians(-45), radians(30), radians(-30), 0])
|
||||
initial_guesses.append([radians(-30), radians(45), radians(-30),
|
||||
radians(45), radians(30), radians(30), 0])
|
||||
|
||||
best_solution = None
|
||||
best_error = float('inf')
|
||||
|
||||
for guess_idx, guess in enumerate(initial_guesses):
|
||||
q = pin.neutral(self.model)
|
||||
for i, angle in enumerate(guess):
|
||||
if i < len(q):
|
||||
q[i] = np.clip(angle, self.model.lowerPositionLimit[i],
|
||||
self.model.upperPositionLimit[i])
|
||||
|
||||
# Differential IK with adaptive damping
|
||||
damping = 0.01
|
||||
damping_reduction = 0.95
|
||||
iter_count = 0
|
||||
prev_error = float('inf')
|
||||
|
||||
ee_frame_id = self.tool_frames[tool]
|
||||
|
||||
J = pin.computeFrameJacobian(
|
||||
self.model,
|
||||
self.data,
|
||||
q,
|
||||
ee_frame_id,
|
||||
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
|
||||
)
|
||||
|
||||
while iter_count < max_iter:
|
||||
# Compute forward kinematics
|
||||
|
||||
pin.computeJointJacobians(self.model, self.data, q)
|
||||
pin.framesForwardKinematics(self.model, self.data, q)
|
||||
|
||||
# Get current end-effector placement
|
||||
|
||||
current_placement = self.data.oMf[ee_frame_id]
|
||||
|
||||
# Compute error
|
||||
error_SE3 = current_placement.actInv(target_placement)
|
||||
error_vec = pin.log(error_SE3).vector
|
||||
error_norm = np.linalg.norm(error_vec)
|
||||
|
||||
if error_norm < tolerance:
|
||||
joint_angles = q[:7].copy()
|
||||
fk_result = self.forward_kinematics(joint_angles,tool=tool)
|
||||
position_error = np.linalg.norm(fk_result['position'] - np.array(target_position))
|
||||
|
||||
if position_error < best_error:
|
||||
best_error = position_error
|
||||
best_solution = joint_angles
|
||||
break
|
||||
|
||||
# Check if error is increasing (diverging)
|
||||
if error_norm > prev_error * 1.1 and iter_count > 10:
|
||||
damping = min(1.0, damping * 1.5)
|
||||
else:
|
||||
damping = max(0.01, damping * damping_reduction)
|
||||
|
||||
|
||||
J = pin.getFrameJacobian(
|
||||
self.model,
|
||||
self.data,
|
||||
ee_frame_id,
|
||||
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
|
||||
)
|
||||
|
||||
# =========================
|
||||
# QP-based IK
|
||||
# =========================
|
||||
|
||||
H = J.T @ self.W @ J
|
||||
H += damping * damping * np.eye(7)
|
||||
|
||||
H_triu = sparse.triu(H).tocsc()
|
||||
|
||||
g = -J.T @ self.W @ error_vec
|
||||
|
||||
# -------------------------
|
||||
# Joint velocity constraints
|
||||
# -------------------------
|
||||
|
||||
dq_limit = 0.05 # rad per iteration
|
||||
|
||||
lb = -dq_limit * np.ones(7)
|
||||
ub = dq_limit * np.ones(7)
|
||||
|
||||
# -------------------------
|
||||
# Joint position constraints
|
||||
# -------------------------
|
||||
|
||||
q_min_step = self.model.lowerPositionLimit[:7] - q[:7]
|
||||
q_max_step = self.model.upperPositionLimit[:7] - q[:7]
|
||||
|
||||
lb = np.maximum(lb, q_min_step)
|
||||
ub = np.minimum(ub, q_max_step)
|
||||
|
||||
# -------------------------
|
||||
# Solve QP
|
||||
# ------------------------
|
||||
# Update solver
|
||||
self.osqp_solver.update(
|
||||
Px=H_triu.data,
|
||||
q=g,
|
||||
l=lb,
|
||||
u=ub
|
||||
)
|
||||
|
||||
# Solve
|
||||
result = self.osqp_solver.solve()
|
||||
|
||||
if result.info.status != 'solved':
|
||||
break
|
||||
|
||||
dq = result.x
|
||||
|
||||
|
||||
if dq is None:
|
||||
break
|
||||
|
||||
# Apply joint limits with scaling
|
||||
alpha = 0.5
|
||||
q = pin.integrate(self.model, q, alpha * dq)
|
||||
|
||||
prev_error = error_norm
|
||||
iter_count += 1
|
||||
|
||||
if best_solution is not None:
|
||||
return best_solution, True, best_error
|
||||
else:
|
||||
return None, False, None
|
||||
|
||||
def invese_kinematics_velocity(self, target_position, target_rpy=None,
|
||||
target_quat=None, initial_guess=None, tool="ee"):
|
||||
"""
|
||||
Compute the converging velocity (motion direction) of joints based on qp inverse kinematics.
|
||||
|
||||
Args:
|
||||
target_position: [x, y, z] target position (meters)
|
||||
target_rpy: [roll, pitch, yaw] target orientation (radians)
|
||||
target_quat: [x, y, z, w] target orientation as quaternion
|
||||
initial_guess: Initial joint angles (radians)
|
||||
tool: the frame name ('scissor', 'camera', 'ee')
|
||||
|
||||
Returns:
|
||||
joint_velocity: np.array()
|
||||
"""
|
||||
# Build target SE3 placement
|
||||
if target_quat is not None:
|
||||
quat = pin.Quaternion(target_quat[3], target_quat[0],
|
||||
target_quat[1], target_quat[2])
|
||||
target_rotation = quat.matrix()
|
||||
elif target_rpy is not None:
|
||||
target_rotation = pin.rpy.rpyToMatrix(target_rpy[0],
|
||||
target_rpy[1],
|
||||
target_rpy[2])
|
||||
else:
|
||||
target_rotation = np.eye(3)
|
||||
|
||||
target_placement = pin.SE3(target_rotation, np.array(target_position))
|
||||
|
||||
# Try multiple initial guesses
|
||||
initial_guesses = []
|
||||
|
||||
if initial_guess is not None:
|
||||
initial_guesses.append(initial_guess)
|
||||
else:
|
||||
# Try different initial configurations
|
||||
initial_guesses.append([0.1] * 7) # Zero config
|
||||
initial_guesses.append([radians(30), radians(45), radians(30),
|
||||
radians(-45), radians(30), radians(-30), 0])
|
||||
initial_guesses.append([radians(-30), radians(45), radians(-30),
|
||||
radians(45), radians(30), radians(30), 0])
|
||||
|
||||
best_solution = None
|
||||
best_error = float('inf')
|
||||
|
||||
for guess_idx, guess in enumerate(initial_guesses):
|
||||
q = pin.neutral(self.model)
|
||||
for i, angle in enumerate(guess):
|
||||
if i < len(q):
|
||||
q[i] = np.clip(angle, self.model.lowerPositionLimit[i],
|
||||
self.model.upperPositionLimit[i])
|
||||
|
||||
# Differential IK with adaptive damping
|
||||
damping = 0.01
|
||||
damping_reduction = 0.95
|
||||
iter_count = 0
|
||||
prev_error = float('inf')
|
||||
|
||||
ee_frame_id = self.tool_frames[tool]
|
||||
|
||||
J = pin.computeFrameJacobian(
|
||||
self.model,
|
||||
self.data,
|
||||
q,
|
||||
ee_frame_id,
|
||||
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
|
||||
)
|
||||
|
||||
while iter_count < max_iter:
|
||||
# Compute forward kinematics
|
||||
|
||||
pin.computeJointJacobians(self.model, self.data, q)
|
||||
pin.framesForwardKinematics(self.model, self.data, q)
|
||||
|
||||
# Get current end-effector placement
|
||||
|
||||
current_placement = self.data.oMf[ee_frame_id]
|
||||
|
||||
# Compute error
|
||||
error_SE3 = current_placement.actInv(target_placement)
|
||||
error_vec = pin.log(error_SE3).vector
|
||||
error_norm = np.linalg.norm(error_vec)
|
||||
|
||||
if error_norm < tolerance:
|
||||
joint_angles = q[:7].copy()
|
||||
fk_result = self.forward_kinematics(joint_angles, tool=tool)
|
||||
position_error = np.linalg.norm(fk_result['position'] - np.array(target_position))
|
||||
|
||||
if position_error < best_error:
|
||||
best_error = position_error
|
||||
best_solution = joint_angles
|
||||
break
|
||||
|
||||
# Check if error is increasing (diverging)
|
||||
if error_norm > prev_error * 1.1 and iter_count > 10:
|
||||
damping = min(1.0, damping * 1.5)
|
||||
else:
|
||||
damping = max(0.01, damping * damping_reduction)
|
||||
|
||||
J = pin.getFrameJacobian(
|
||||
self.model,
|
||||
self.data,
|
||||
ee_frame_id,
|
||||
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
|
||||
)
|
||||
|
||||
# =========================
|
||||
# QP-based IK
|
||||
# =========================
|
||||
|
||||
H = J.T @ self.W @ J
|
||||
H += damping * damping * np.eye(7)
|
||||
|
||||
H_triu = sparse.triu(H).tocsc()
|
||||
|
||||
g = -J.T @ self.W @ error_vec
|
||||
|
||||
# -------------------------
|
||||
# Joint velocity constraints
|
||||
# -------------------------
|
||||
|
||||
dq_limit = 0.05 # rad per iteration
|
||||
|
||||
lb = -dq_limit * np.ones(7)
|
||||
ub = dq_limit * np.ones(7)
|
||||
|
||||
# -------------------------
|
||||
# Joint position constraints
|
||||
# -------------------------
|
||||
|
||||
q_min_step = self.model.lowerPositionLimit[:7] - q[:7]
|
||||
q_max_step = self.model.upperPositionLimit[:7] - q[:7]
|
||||
|
||||
lb = np.maximum(lb, q_min_step)
|
||||
ub = np.minimum(ub, q_max_step)
|
||||
|
||||
# -------------------------
|
||||
# Solve QP
|
||||
# ------------------------
|
||||
# Update solver
|
||||
self.osqp_solver.update(
|
||||
Px=H_triu.data,
|
||||
q=g,
|
||||
l=lb,
|
||||
u=ub
|
||||
)
|
||||
|
||||
# Solve
|
||||
result = self.osqp_solver.solve()
|
||||
|
||||
if result.info.status != 'solved':
|
||||
break
|
||||
|
||||
dq = result.x
|
||||
|
||||
if dq is None:
|
||||
break
|
||||
|
||||
# Apply joint limits with scaling
|
||||
alpha = 0.5
|
||||
q = pin.integrate(self.model, q, alpha * dq)
|
||||
|
||||
prev_error = error_norm
|
||||
iter_count += 1
|
||||
|
||||
if best_solution is not None:
|
||||
return best_solution, True, best_error
|
||||
else:
|
||||
return None, False, None
|
||||
|
||||
def compute_jacobian(self, joint_angles, tool="ee"):
|
||||
"""Compute geometric Jacobian (6x7)"""
|
||||
q = pin.neutral(self.model)
|
||||
for i, angle in enumerate(joint_angles):
|
||||
q[i] = angle
|
||||
|
||||
pin.forwardKinematics(self.model, self.data, q)
|
||||
pin.updateFramePlacements(self.model, self.data)
|
||||
ee_frame_id = self.tool_frames[tool]
|
||||
J = pin.computeFrameJacobian(self.model, self.data, q, ee_frame_id)
|
||||
|
||||
return J
|
||||
|
||||
def get_subchain_jacobian(self,
|
||||
joint_angles,
|
||||
frame_names
|
||||
):
|
||||
|
||||
q = pin.neutral(self.model)
|
||||
|
||||
all_active_joints = self.get_active_joints_from_frame(frame_names)
|
||||
|
||||
for i in range(7):
|
||||
q[i] = joint_angles[i]
|
||||
|
||||
pin.forwardKinematics(self.model, self.data, q)
|
||||
pin.updateFramePlacements(self.model, self.data)
|
||||
pin.computeJointJacobians(self.model, self.data, q)
|
||||
|
||||
Js = []
|
||||
|
||||
for frame_name, active_joints in zip(frame_names, all_active_joints):
|
||||
frame_id = self.model.getFrameId(frame_name)
|
||||
|
||||
J = pin.getFrameJacobian(
|
||||
self.model,
|
||||
self.data,
|
||||
frame_id,
|
||||
pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
|
||||
)
|
||||
Js.append(J[:, active_joints])
|
||||
|
||||
return Js
|
||||
|
||||
def get_active_joints_from_frame(self, frame_names):
|
||||
"""
|
||||
Return active joint indices affecting a frame.
|
||||
|
||||
Example:
|
||||
frame_name='link_4'
|
||||
-> [0,1,2,3]
|
||||
"""
|
||||
all_active_joint_ids = []
|
||||
for frame_name in frame_names:
|
||||
frame_id = self.model.getFrameId(frame_name)
|
||||
|
||||
# Parent joint of this frame
|
||||
joint_id = self.model.frames[frame_id].parentJoint
|
||||
|
||||
print(f'frame_id = {frame_id}, and joint_id = {joint_id}')
|
||||
|
||||
active_joint_ids = []
|
||||
|
||||
|
||||
# Traverse upward to root
|
||||
while joint_id > 0:
|
||||
# Pinocchio joint indexing:
|
||||
# universe joint = 0
|
||||
# robot joints start from 1
|
||||
|
||||
active_joint_ids.append(joint_id - 1)
|
||||
|
||||
# Move to parent joint
|
||||
joint_id = self.model.parents[joint_id]
|
||||
|
||||
# Reverse so order becomes base -> tip
|
||||
active_joint_ids.reverse()
|
||||
all_active_joint_ids.append(active_joint_ids)
|
||||
|
||||
return all_active_joint_ids
|
||||
|
||||
def plan_cartesian_trajectory(self, start_pos, end_pos,
|
||||
start_rpy=None, end_rpy=None,
|
||||
num_steps=20, tool='ee'):
|
||||
"""
|
||||
Plan a Cartesian trajectory with IK for each waypoint.
|
||||
"""
|
||||
# Get current end-effector pose if start_rpy not provided
|
||||
if start_rpy is None:
|
||||
# Try to find a valid starting configuration
|
||||
test_angles = [0.1] * 7
|
||||
fk_test = self.forward_kinematics(test_angles,tool=tool)
|
||||
start_rpy = fk_test['rpy']
|
||||
|
||||
if end_rpy is None:
|
||||
end_rpy = start_rpy
|
||||
|
||||
# First, check if target is reachable
|
||||
print(f"\nChecking if target is reachable...")
|
||||
target_pos = end_pos
|
||||
target_rpy = end_rpy
|
||||
|
||||
test_solution, success, error = self.inverse_kinematics(
|
||||
target_pos, target_rpy=target_rpy, initial_guess=[0.1] * 7, max_iter=500, tool=tool
|
||||
)
|
||||
|
||||
if not success:
|
||||
print(f"Warning: Target may be unreachable or difficult to reach")
|
||||
print(f"Trying with relaxed tolerance...")
|
||||
|
||||
# Initial guess for IK (start with zero configuration)
|
||||
current_angles = [0.1] * 7
|
||||
trajectory = []
|
||||
|
||||
print(f"\nPlanning trajectory from ({start_pos[0]:.2f}, {start_pos[1]:.2f}, {start_pos[2]:.2f})")
|
||||
print(f"To ({end_pos[0]:.2f}, {end_pos[1]:.2f}, {end_pos[2]:.2f})")
|
||||
print("-" * 60)
|
||||
|
||||
for i in range(num_steps + 1):
|
||||
t = i / num_steps
|
||||
|
||||
# Interpolate position
|
||||
pos = [
|
||||
start_pos[0] + t * (end_pos[0] - start_pos[0]),
|
||||
start_pos[1] + t * (end_pos[1] - start_pos[1]),
|
||||
start_pos[2] + t * (end_pos[2] - start_pos[2])
|
||||
]
|
||||
|
||||
# Interpolate orientation
|
||||
rpy = [
|
||||
start_rpy[0] + t * (end_rpy[0] - start_rpy[0]),
|
||||
start_rpy[1] + t * (end_rpy[1] - start_rpy[1]),
|
||||
start_rpy[2] + t * (end_rpy[2] - start_rpy[2])
|
||||
]
|
||||
|
||||
# Compute IK
|
||||
joint_angles, success, error = self.inverse_kinematics(
|
||||
pos, target_rpy=rpy, initial_guess=current_angles, max_iter=300, tool=tool
|
||||
)
|
||||
|
||||
if not success:
|
||||
print(f" Waypoint {i}: IK failed!")
|
||||
break
|
||||
|
||||
# Verify
|
||||
fk_verify = self.forward_kinematics(joint_angles, tool=tool)
|
||||
|
||||
trajectory.append({
|
||||
'step': i,
|
||||
't': t,
|
||||
'position': pos,
|
||||
'rpy': rpy,
|
||||
'joint_angles': joint_angles,
|
||||
'actual_position': fk_verify['position'],
|
||||
'error': error
|
||||
})
|
||||
|
||||
# Update current angles for next iteration
|
||||
current_angles = joint_angles
|
||||
|
||||
if i % 5 == 0 or i == num_steps:
|
||||
print(f" Waypoint {i:3d}: pos=({pos[0]:.3f}, {pos[1]:.3f}, {pos[2]:.3f}), "
|
||||
f"error={error:.6f}m")
|
||||
|
||||
return trajectory
|
||||
|
||||
|
||||
|
||||
def main():
|
||||
"""Main test function"""
|
||||
|
||||
|
||||
rm75 = KinematicsSolver()
|
||||
|
||||
# Test 1: Forward Kinematics
|
||||
print("\n1. Forward Kinematics Test")
|
||||
print("-" * 40)
|
||||
|
||||
tool_name = "scissor"
|
||||
joint_angles_zero = [0.1] * 7
|
||||
fk_result = rm75.forward_kinematics(joint_angles_zero, tool=tool_name)
|
||||
|
||||
print(f"Init configuration:")
|
||||
print(f" Position: ({fk_result['position'][0]:.3f}, "
|
||||
f"{fk_result['position'][1]:.3f}, {fk_result['position'][2]:.3f}) m")
|
||||
|
||||
# Test 2: Inverse Kinematics with more reachable target
|
||||
print("\n2. Inverse Kinematics Test")
|
||||
print("-" * 40)
|
||||
|
||||
# Try a simpler target first
|
||||
target_pos = [0.3, 0.2, 0.4] # More reachable position
|
||||
target_rpy = [0.0, 0.0, radians(45)] # Simpler orientation
|
||||
|
||||
print(f"Target: ({target_pos[0]:.3f}, {target_pos[1]:.3f}, {target_pos[2]:.3f}) m")
|
||||
|
||||
import time
|
||||
init_joints = [0.2] * 7
|
||||
time0 = time.time()
|
||||
for ii in range(100):
|
||||
joint_solution, success, error = rm75.inverse_kinematics(
|
||||
target_pos, target_rpy=target_rpy, initial_guess=init_joints,
|
||||
max_iter=500, debug=False, tool=tool_name
|
||||
)
|
||||
time1 = time.time()
|
||||
print(f"Time: {time1 - time0}")
|
||||
|
||||
if success:
|
||||
print(f"✓ Solution found! Error: {error:.6f} m")
|
||||
for i, angle in enumerate(joint_solution):
|
||||
print(f" Joint {i + 1}: {degrees(angle):7.2f}°")
|
||||
|
||||
# Verify
|
||||
fk_verify = rm75.forward_kinematics(joint_solution,tool=tool_name)
|
||||
print(
|
||||
f" Position: ({fk_verify['position'][0]:.3f}, {fk_verify['position'][1]:.3f}, {fk_verify['position'][2]:.3f}) m")
|
||||
else:
|
||||
print("✗ IK failed to find a solution!")
|
||||
|
||||
# Test 3: Jacobian
|
||||
print("\n3. Jacobian Matrix")
|
||||
print("-" * 40)
|
||||
|
||||
J = rm75.compute_jacobian(joint_angles_zero, tool=tool_name)
|
||||
print(f"Jacobian shape: {J.shape}")
|
||||
for i in range(min(3, J.shape[0])):
|
||||
row_str = " ".join([f"{J[i, j]:7.3f}" for j in range(7)])
|
||||
print(f" Row {i + 1}: {row_str}")
|
||||
|
||||
# Test 4: Trajectory Planning with reachable positions
|
||||
print("\n4. Cartesian Trajectory Planning")
|
||||
print("-" * 40)
|
||||
|
||||
start_pos = [0.3, 0.0, 0.4] # Start position
|
||||
end_pos = [0.3, 0.0, 0.55] # End position (smaller movement)
|
||||
|
||||
fk0 = rm75.forward_kinematics([0.1] * 7, tool=tool_name)
|
||||
|
||||
trajectory = rm75.plan_cartesian_trajectory(
|
||||
start_pos,
|
||||
end_pos,
|
||||
start_rpy=fk0['rpy'],
|
||||
end_rpy=[
|
||||
fk0['rpy'][0] + radians(10),
|
||||
fk0['rpy'][1],
|
||||
fk0['rpy'][2]
|
||||
],
|
||||
num_steps=10,
|
||||
tool=tool_name
|
||||
)
|
||||
|
||||
if trajectory:
|
||||
print(f"\n✓ Generated {len(trajectory)} waypoints")
|
||||
|
||||
if success:
|
||||
print("✓ Inverse kinematics working (with simplified target)")
|
||||
else:
|
||||
print("⚠ Inverse kinematics may need tuning - try different targets")
|
||||
|
||||
|
||||
print("\n" + "=" * 60)
|
||||
print(f'test subchain Jacobian, for future obstacle avoidance')
|
||||
frame_names = [
|
||||
"link_2",
|
||||
"link_4",
|
||||
"link_7"
|
||||
]
|
||||
Js_sub = rm75.get_subchain_jacobian(
|
||||
joint_angles=joint_angles_zero,
|
||||
frame_names=frame_names
|
||||
)
|
||||
print(f'Js_sub: {Js_sub}')
|
||||
|
||||
return rm75, trajectory
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
rm75, trajectory = main()
|
||||
|
||||
print("\n" + "=" * 60)
|
||||
print("All tests completed!")
|
||||
print("=" * 60)
|
||||
@@ -1,298 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Pure Position Control for MuJoCo - No velocity commands, no forces
|
||||
Direct joint position control with smoothing
|
||||
"""
|
||||
|
||||
import mujoco
|
||||
import mujoco.viewer
|
||||
import numpy as np
|
||||
import threading
|
||||
import time
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
class MuJoCoPositionController:
|
||||
"""
|
||||
Pure position control - directly sets joint positions
|
||||
No velocity commands, no forces - completely stable
|
||||
"""
|
||||
|
||||
def __init__(self, urdf_path, smoothness=0.2, enable_viewer=True):
|
||||
"""
|
||||
Args:
|
||||
urdf_path: Path to URDF file
|
||||
smoothness: Motion smoothness (0.02=very smooth, 0.1=fast)
|
||||
enable_viewer: Show MuJoCo viewer
|
||||
"""
|
||||
# Load model
|
||||
self.model = mujoco.MjModel.from_xml_path(urdf_path)
|
||||
self.data = mujoco.MjData(self.model)
|
||||
|
||||
self.time_interval = 0.02
|
||||
|
||||
print(f'time interval: {self.model.opt.timestep}')
|
||||
|
||||
# Robot info
|
||||
self.n_joints = self.model.njnt
|
||||
|
||||
# Get joint limits
|
||||
self.joint_lower_limits = []
|
||||
self.joint_upper_limits = []
|
||||
for i in range(self.n_joints):
|
||||
self.joint_lower_limits.append(self.model.jnt_range[i, 0])
|
||||
self.joint_upper_limits.append(self.model.jnt_range[i, 1])
|
||||
|
||||
print(f"Loaded robot: {self.n_joints} joints")
|
||||
for i in range(self.n_joints):
|
||||
print(
|
||||
f" {self.model.joint(i).name}: limit [{self.joint_lower_limits[i]:.2f}, {self.joint_upper_limits[i]:.2f}]")
|
||||
|
||||
# Target positions (in radians)
|
||||
self.target_positions = self.data.qpos[:self.n_joints].copy()
|
||||
|
||||
# Smoothing factor (0-1, lower = smoother)
|
||||
self.smoothness = smoothness
|
||||
|
||||
# Thread safety
|
||||
self.command_lock = threading.Lock()
|
||||
self.feedback_lock = threading.Lock()
|
||||
self.current_feedback = self.data.qpos[:self.n_joints].copy()
|
||||
|
||||
self.max_pos_inc = 0.02
|
||||
|
||||
# Control flags
|
||||
self.running = False
|
||||
self.simulation_thread = None
|
||||
|
||||
# Viewer
|
||||
self.viewer = None
|
||||
if enable_viewer:
|
||||
try:
|
||||
self.viewer = mujoco.viewer.launch_passive(self.model, self.data)
|
||||
print("Viewer launched")
|
||||
except Exception as e:
|
||||
print(f"Viewer warning: {e}")
|
||||
|
||||
print("Robot controller ready - Pure Position Mode")
|
||||
|
||||
def start(self):
|
||||
"""Start the simulation thread"""
|
||||
if self.running:
|
||||
return
|
||||
|
||||
self.running = True
|
||||
self.simulation_thread = threading.Thread(target=self._simulation_loop, daemon=True)
|
||||
self.simulation_thread.start()
|
||||
print("Simulation thread started")
|
||||
|
||||
def stop(self):
|
||||
"""Stop the simulation thread"""
|
||||
self.running = False
|
||||
if self.simulation_thread:
|
||||
self.simulation_thread.join(timeout=2.0)
|
||||
if self.viewer:
|
||||
self.viewer.close()
|
||||
print("Simulation stopped")
|
||||
|
||||
def send_command(self, joint_positions):
|
||||
"""
|
||||
Send target joint positions
|
||||
|
||||
Args:
|
||||
joint_positions: Array of target joint angles (radians)
|
||||
"""
|
||||
cmd = np.array(joint_positions[:self.n_joints], dtype=np.float64)
|
||||
|
||||
# Apply joint limits
|
||||
for i in range(self.n_joints):
|
||||
cmd[i] = np.clip(cmd[i], self.joint_lower_limits[i], self.joint_upper_limits[i])
|
||||
|
||||
with self.command_lock:
|
||||
self.target_positions = cmd
|
||||
|
||||
def get_feedback(self):
|
||||
"""Get current joint positions"""
|
||||
with self.feedback_lock:
|
||||
return self.current_feedback.copy()
|
||||
|
||||
def get_target(self):
|
||||
"""Get current target positions"""
|
||||
with self.command_lock:
|
||||
return self.target_positions.copy()
|
||||
|
||||
def _simulation_loop(self):
|
||||
"""
|
||||
Main simulation loop - PURE POSITION CONTROL
|
||||
No velocity commands, no forces - just direct position setting
|
||||
"""
|
||||
last_time = time.time()
|
||||
|
||||
# For smooth interpolation
|
||||
current_positions = self.data.qpos[:self.n_joints].copy()
|
||||
|
||||
while self.running:
|
||||
# Get target command
|
||||
with self.command_lock:
|
||||
target = self.target_positions.copy()
|
||||
|
||||
# Get current positions
|
||||
current_positions = self.data.qpos[:self.n_joints].copy()
|
||||
|
||||
# Smooth interpolation toward target
|
||||
# This creates natural motion without velocity commands
|
||||
alpha = self.smoothness
|
||||
|
||||
next_positions = current_positions + np.clip(alpha * (target - current_positions) , -self.max_pos_inc, self.max_pos_inc)
|
||||
|
||||
# DIRECT POSITION CONTROL - Set joint positions
|
||||
self.data.qpos[:self.n_joints] = next_positions
|
||||
|
||||
# IMPORTANT: Set velocities to zero to prevent physics from moving joints
|
||||
# This ensures pure kinematic control
|
||||
self.data.qvel[:self.n_joints] = 0
|
||||
|
||||
# Step physics (this will apply gravity, collisions, etc. to other bodies)
|
||||
mujoco.mj_step(self.model, self.data)
|
||||
|
||||
# After step, ensure our joint positions are maintained
|
||||
# (Physics might have altered them slightly)
|
||||
self.data.qpos[:self.n_joints] = next_positions
|
||||
self.data.qvel[:self.n_joints] = 0
|
||||
|
||||
# Update feedback
|
||||
with self.feedback_lock:
|
||||
self.current_feedback = self.data.qpos[:self.n_joints].copy()
|
||||
|
||||
# Sync viewer
|
||||
if self.viewer:
|
||||
self.viewer.sync()
|
||||
|
||||
# Maintain real-time speed
|
||||
elapsed = time.time() - last_time
|
||||
sleep_time = self.time_interval - elapsed
|
||||
if sleep_time > 0:
|
||||
time.sleep(sleep_time)
|
||||
last_time = time.time()
|
||||
|
||||
def move_to_position(self, target, duration=1.0):
|
||||
"""
|
||||
Move to target position over specified duration
|
||||
|
||||
Args:
|
||||
target: Target joint positions
|
||||
duration: Time to complete movement (seconds)
|
||||
"""
|
||||
start_pos = self.get_feedback()
|
||||
end_pos = np.array(target[:self.n_joints])
|
||||
|
||||
# Apply limits
|
||||
for i in range(self.n_joints):
|
||||
end_pos[i] = np.clip(end_pos[i], self.joint_lower_limits[i], self.joint_upper_limits[i])
|
||||
|
||||
n_steps = int(duration / self.time_interval)
|
||||
|
||||
print(f" Moving over {duration}s ({n_steps} steps)")
|
||||
|
||||
for step in range(n_steps):
|
||||
alpha = (step + 1) / n_steps
|
||||
# Use easing for smoother motion
|
||||
ease_alpha = 1 - (1 - alpha) ** 2 # Quadratic ease-out
|
||||
current_target = start_pos + ease_alpha * (end_pos - start_pos)
|
||||
self.send_command(current_target)
|
||||
time.sleep(self.time_interval)
|
||||
|
||||
# Ensure exact target
|
||||
self.send_command(end_pos)
|
||||
time.sleep(0.1)
|
||||
|
||||
def wait_until_reached(self, tolerance=0.01, timeout=5.0):
|
||||
"""
|
||||
Wait until robot reaches target position
|
||||
|
||||
Args:
|
||||
tolerance: Position error tolerance (radians)
|
||||
timeout: Maximum wait time (seconds)
|
||||
"""
|
||||
start_time = time.time()
|
||||
|
||||
while time.time() - start_time < timeout:
|
||||
current = self.get_feedback()
|
||||
target = self.get_target()
|
||||
error = np.max(np.abs(target - current))
|
||||
|
||||
if error < tolerance:
|
||||
return True
|
||||
|
||||
time.sleep(0.01)
|
||||
|
||||
return False
|
||||
|
||||
def print_state(self):
|
||||
"""Print current robot state"""
|
||||
positions = self.get_feedback()
|
||||
target = self.get_target()
|
||||
print("Current positions:", [f"{p:.3f}" for p in positions[:4]], "...")
|
||||
print("Target positions: ", [f"{t:.3f}" for t in target[:4]], "...")
|
||||
|
||||
|
||||
# Demo
|
||||
def demo_position_control():
|
||||
"""Demonstrate pure position control"""
|
||||
|
||||
urdf_path = "/home/zl/Downloads/urdf_rm75/RM75-B.urdf"
|
||||
|
||||
if not Path(urdf_path).exists():
|
||||
print(f"Error: URDF not found at {urdf_path}")
|
||||
return
|
||||
|
||||
print("=" * 60)
|
||||
print("Pure Position Control Demo")
|
||||
print("=" * 60)
|
||||
|
||||
# Create controller
|
||||
robot = MuJoCoPositionController(urdf_path, smoothness=0.05, enable_viewer=True)
|
||||
robot.start()
|
||||
time.sleep(1)
|
||||
|
||||
print("\n[Test 1] Move joint 1 to 45 degrees")
|
||||
robot.send_command([0.785, 0, 0, 0, 0, 0, 0])
|
||||
robot.wait_until_reached()
|
||||
robot.print_state()
|
||||
time.sleep(0.5)
|
||||
|
||||
print("\n[Test 2] Move joint 2 to -30 degrees")
|
||||
robot.send_command([0, -0.524, 0, 0, 0, 0, 0])
|
||||
robot.wait_until_reached()
|
||||
robot.print_state()
|
||||
time.sleep(0.5)
|
||||
|
||||
print("\n[Test 3] Move multiple joints simultaneously")
|
||||
robot.send_command([0.5, -0.4, 0.3, 0.2, 0.1, 0, 0])
|
||||
robot.wait_until_reached()
|
||||
robot.print_state()
|
||||
time.sleep(0.5)
|
||||
|
||||
print("\n[Test 4] Return home")
|
||||
robot.send_command([0, 0, 0, 0, 0, 0, 0])
|
||||
robot.wait_until_reached()
|
||||
robot.print_state()
|
||||
|
||||
print("\n" + "=" * 60)
|
||||
print("✓ All tests passed! Robot is stable and controllable.")
|
||||
print("=" * 60)
|
||||
print("\nInteractive mode - close viewer to exit")
|
||||
|
||||
try:
|
||||
while robot.viewer and robot.viewer.is_running():
|
||||
time.sleep(0.1)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
|
||||
robot.stop()
|
||||
|
||||
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
demo_position_control()
|
||||
@@ -0,0 +1,3 @@
|
||||
conda install -c conda-forge osqp scipy tqdm matplotlib pandas "numpy<1.24" pinocchio -y
|
||||
pip install urdfpy mujoco "networkx>=2.8.4"
|
||||
pip install Robotic_Arm
|
||||
+1236
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,48 @@
|
||||
|
||||
|
||||
# conda activate coppeliasim
|
||||
# env fix, in terminal: fix_robotics_env.sh
|
||||
|
||||
from rm75_kinematics import rm75_kinematics
|
||||
|
||||
from math import pi
|
||||
import numpy as np
|
||||
|
||||
# pose expression of tool-tip in end-effector, x y z quatx quaty quatz quatw
|
||||
# load: kg, mass_center_x in ee frame: m, y, z, then last threes are for filling
|
||||
tools_in_ee = {
|
||||
'scissor': np.array([[0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0],[0.66, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
|
||||
'omnipic': np.array([[0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0],[0.43, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
|
||||
'minisci': np.array([[0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0],[0.46, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
|
||||
'no_tool': np.array([[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0],[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]],dtype=np.float64),
|
||||
}
|
||||
|
||||
# joint limit
|
||||
ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])/180*pi
|
||||
lb = -ub
|
||||
|
||||
tool_name = "scissor"
|
||||
|
||||
def main():
|
||||
"""Demonstrate pure position control"""
|
||||
|
||||
# Create controller
|
||||
|
||||
robot_kine = rm75_kinematics(urdf_path='urdf_rm75/RM75-SCI.urdf', mesh_dir='urdf_rm75',
|
||||
tcps=["scissor_tcp", "camera_tcp"], tools_in_ee=tools_in_ee, min_j=lb, max_j=ub)
|
||||
|
||||
ret_ik, q = robot_kine.get_ik_result(target_position=[0.2, -0.2 , 0.5 ], target_rpy=[0.2022060487764064, -0.0097962261845583, -0.6518417572686532],
|
||||
initial_guess=[0.1] * 7, tool=tool_name)
|
||||
|
||||
self_collision_sts = robot_kine.get_self_collision(q)
|
||||
|
||||
p = robot_kine.get_fk_result(joint_angles=q,tool=tool_name)
|
||||
|
||||
print(f'self_collision_sts: {self_collision_sts}')
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,90 @@
|
||||
|
||||
|
||||
# conda activate coppeliasim
|
||||
# env fix, in terminal: fix_robotics_env.sh
|
||||
|
||||
from rm75_kinematics import rm75_kinematics
|
||||
|
||||
from math import pi
|
||||
import numpy as np
|
||||
|
||||
# pose expression of tool-tip in end-effector, x y z quatx quaty quatz quatw
|
||||
# load: kg, mass_center_x in ee frame: m, y, z, then last threes are for filling
|
||||
tools_in_ee = {
|
||||
'scissor': np.array([[0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0],[0.66, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
|
||||
'omnipic': np.array([[0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0],[0.43, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
|
||||
'minisci': np.array([[0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0],[0.46, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0]],dtype=np.float64),
|
||||
'no_tool': np.array([[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0],[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]],dtype=np.float64),
|
||||
}
|
||||
|
||||
# joint limit
|
||||
ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])/180*pi
|
||||
lb = -ub
|
||||
|
||||
tool_name = "scissor"
|
||||
|
||||
def main():
|
||||
"""Demonstrate pure position control"""
|
||||
|
||||
# Create controller
|
||||
|
||||
robot_kine = rm75_kinematics(urdf_path='urdf_rm75/RM75-SCI.urdf', mesh_dir='urdf_rm75',
|
||||
tcps=["scissor_tcp", "camera_tcp"], tools_in_ee=tools_in_ee, min_j=lb, max_j=ub)
|
||||
|
||||
ret_ik, q = robot_kine.get_ik_result(target_position=[0.2, -0.2 , 0.5 ], target_rpy=[0.2022060487764064, -0.0097962261845583, -0.6518417572686532],
|
||||
initial_guess=[0.1] * 7, tool=tool_name)
|
||||
|
||||
self_collision_sts = robot_kine.get_self_collision(q)
|
||||
|
||||
p = robot_kine.get_fk_result(joint_angles=q,tool=tool_name)
|
||||
|
||||
print(f'self_collision_sts: {self_collision_sts}')
|
||||
|
||||
import numpy as np
|
||||
|
||||
ik_suc = 0
|
||||
|
||||
for i in range(100):
|
||||
joint_rand = np.random.uniform(ub, lb)
|
||||
|
||||
p_t = robot_kine.get_fk_result(joint_angles=joint_rand.tolist(), tool=tool_name)
|
||||
|
||||
joint_rand_init = np.random.uniform(ub, lb)
|
||||
|
||||
ret_ik, q = robot_kine.get_ik_result(target_position=p_t[0:3], target_rpy=p_t[3:6],
|
||||
initial_guess=joint_rand_init, tool=tool_name)
|
||||
|
||||
|
||||
|
||||
|
||||
p_fk = robot_kine.get_fk_result(joint_angles=q, tool=tool_name)
|
||||
|
||||
d_p_ik = cal_pose_deviation(pose1=p_t, pose2=p_fk)
|
||||
|
||||
coll_sts = robot_kine.get_self_collision(joint_angles=q)
|
||||
if ret_ik == True:
|
||||
|
||||
print(f'\n---- success, in the ik, j_t = {joint_rand}, q = {q}, p_t = {p_t}, d_p_ik = {d_p_ik}, self-collision_sts = {coll_sts}')
|
||||
|
||||
ik_suc += 1
|
||||
else:
|
||||
print(f'\n**** ik failed, in the ik, j_t = {joint_rand}, q={q}, p_t = {p_t}, d_p_ik = {d_p_ik}, self-collision_sts = {coll_sts}')
|
||||
|
||||
|
||||
print(f'ik_suc: {ik_suc}')
|
||||
|
||||
|
||||
def cal_pose_deviation(pose1, pose2):
|
||||
d_fk_p1 = np.array(pose1) - np.array(pose2)
|
||||
for j in [3, 4, 5]:
|
||||
while d_fk_p1[j] > pi:
|
||||
d_fk_p1[j] -= 2 * pi
|
||||
while d_fk_p1[j] < -pi:
|
||||
d_fk_p1[j] += 2 * pi
|
||||
d_fk = np.linalg.norm(d_fk_p1)
|
||||
return d_fk
|
||||
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,555 @@
|
||||
<?xml version='1.0' encoding='utf-8'?>
|
||||
<robot name="rm75_dual_arm">
|
||||
<!--Shared supporting base. Adjust the two mount joint origins to match the CAD mounting frames.-->
|
||||
<link name="dual_arm_base_link">
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/dual_arm_base.stl" scale="1 1 1" />
|
||||
</geometry>
|
||||
<material name="dual_arm_base_material">
|
||||
<color rgba="0.5 0.5 0.5 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/dual_arm_base.stl" scale="1 1 1" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="omnipic_base_link">
|
||||
<inertial>
|
||||
<origin xyz="0.00049987 5.2709E-05 0.060019" rpy="0 0 0" />
|
||||
<mass value="1.862" />
|
||||
<inertia ixx="0.0017232" ixy="-3.1058E-06" ixz="-3.7924E-05" iyy="0.0017051" iyz="1.3691E-06" izz="0.00090158" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/base_link.STL" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/base_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="omnipic_link_1">
|
||||
<inertial>
|
||||
<origin xyz="0.000241 -0.013273 -0.00995" rpy="0 0 0" />
|
||||
<mass value="1.574" />
|
||||
<inertia ixx="0.002487573" ixy="0.000009663" ixz="-0.000007909" iyy="0.002321038" iyz="0.000179393" izz="0.001450554" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_1.STL" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_1.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="omnipic_joint_1" type="revolute">
|
||||
<origin xyz="0 0 0.2405" rpy="0 0 0" />
|
||||
<parent link="omnipic_base_link" />
|
||||
<child link="omnipic_link_1" />
|
||||
<axis xyz="0 0 1" />
|
||||
<limit lower="-3.106" upper="3.106" effort="60" velocity="3.14" />
|
||||
</joint>
|
||||
<link name="omnipic_link_2">
|
||||
<inertial>
|
||||
<origin xyz="-0.000357 -0.106789 0.005329" rpy="0 0 0" />
|
||||
<mass value="1.217" />
|
||||
<inertia ixx="0.003494121" ixy="0.000002921" ixz="-0.000005613" iyy="0.000892721" iyz="-0.000583884" izz="0.003444080" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_2.STL" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_2.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="omnipic_joint_2" type="revolute">
|
||||
<origin xyz="0 0 0" rpy="-1.5708 0 0" />
|
||||
<parent link="omnipic_link_1" />
|
||||
<child link="omnipic_link_2" />
|
||||
<axis xyz="0 0 1" />
|
||||
<limit lower="-2.2689" upper="2.2689" effort="60" velocity="3.14" />
|
||||
</joint>
|
||||
<link name="omnipic_link_3">
|
||||
<inertial>
|
||||
<origin xyz="0.000003 -0.01398 -0.011324" rpy="0 0 0" />
|
||||
<mass value="1.11" />
|
||||
<inertia ixx="0.001836663" ixy="0.000002259" ixz="-0.000004216" iyy="0.001498875" iyz="0.000037167" izz="0.001062545" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_3.STL" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_3.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="omnipic_joint_3" type="revolute">
|
||||
<origin xyz="0 -0.256 0" rpy="1.5708 0 0" />
|
||||
<parent link="omnipic_link_2" />
|
||||
<child link="omnipic_link_3" />
|
||||
<axis xyz="0 0 1" />
|
||||
<limit lower="-3.106" upper="3.106" effort="30" velocity="3.14" />
|
||||
</joint>
|
||||
<link name="omnipic_link_4">
|
||||
<inertial>
|
||||
<origin xyz="-0.000005 -0.084658 0.004747" rpy="0 0 0" />
|
||||
<mass value="0.685" />
|
||||
<inertia ixx="0.001282444" ixy="-0.000000551" ixz="-0.000000630" iyy="0.000373013" iyz="-0.000232084" izz="0.001256177" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_4.STL" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_4.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="omnipic_joint_4" type="revolute">
|
||||
<origin xyz="0 0 0" rpy="-1.5708 0 0" />
|
||||
<parent link="omnipic_link_3" />
|
||||
<child link="omnipic_link_4" />
|
||||
<axis xyz="0 0 1" />
|
||||
<limit lower="-2.356" upper="2.356" effort="30" velocity="3.14" />
|
||||
</joint>
|
||||
<link name="omnipic_link_5">
|
||||
<inertial>
|
||||
<origin xyz="0.000078 -0.012937 -0.008781" rpy="0 0 0" />
|
||||
<mass value="0.619" />
|
||||
<inertia ixx="0.000627336" ixy="0.000001636" ixz="-0.000001345" iyy="0.000542455" iyz="0.000034970" izz="0.000370291" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_5.STL" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_5.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="omnipic_joint_5" type="revolute">
|
||||
<origin xyz="0 -0.21 0" rpy="1.5708 0 0" />
|
||||
<parent link="omnipic_link_4" />
|
||||
<child link="omnipic_link_5" />
|
||||
<axis xyz="0 0 1" />
|
||||
<limit lower="-3.106" upper="3.106" effort="10" velocity="3.14" />
|
||||
</joint>
|
||||
<link name="omnipic_link_6">
|
||||
<inertial>
|
||||
<origin xyz="-0.000014 -0.078524 0.002819" rpy="0 0 0" />
|
||||
<mass value="0.602" />
|
||||
<inertia ixx="0.000780774" ixy="-0.000000121" ixz="-0.000000469" iyy="0.000289973" iyz="-0.000120513" izz="0.000763955" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_6.STL" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_6.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="omnipic_joint_6" type="revolute">
|
||||
<origin xyz="0 0 0" rpy="-1.5708 0 0" />
|
||||
<parent link="omnipic_link_5" />
|
||||
<child link="omnipic_link_6" />
|
||||
<axis xyz="0 0 1" />
|
||||
<limit lower="-2.234" upper="2.234" effort="10" velocity="3.14" />
|
||||
</joint>
|
||||
<link name="omnipic_link_7">
|
||||
<inertial>
|
||||
<origin xyz="0.001094 -0.000077 -0.010119" rpy="0 0 0" />
|
||||
<mass value="0.107" />
|
||||
<inertia ixx="0.000044123" ixy="-0.000000064" ixz="0.0000003" iyy="0.000035078" iyz="-0.000000029" izz="0.000065445" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_7.STL" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_7.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="omnipic_joint_7" type="revolute">
|
||||
<origin xyz="0 -0.144 0" rpy="1.5708 0 0" />
|
||||
<parent link="omnipic_link_6" />
|
||||
<child link="omnipic_link_7" />
|
||||
<axis xyz="0 0 1" />
|
||||
<limit lower="-6.28" upper="6.28" effort="10" velocity="3.14" />
|
||||
</joint>
|
||||
<link name="omnipic_gripper_link">
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<mass value="0.1" />
|
||||
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/OmniPic.stl" scale="1 1 1" />
|
||||
</geometry>
|
||||
<material name="omnipic_OmniPic_material">
|
||||
<color rgba="0.7 0.7 0.7 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/OmniPic.stl" scale="1 1 1" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="omnipic_OmniPic_fixed_joint" type="fixed">
|
||||
<parent link="omnipic_link_7" />
|
||||
<child link="omnipic_gripper_link" />
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
</joint>
|
||||
<link name="omnipic_OmniPic_tcp" />
|
||||
<joint name="omnipic_OmniPic_tcp_fixed" type="fixed">
|
||||
<parent link="omnipic_gripper_link" />
|
||||
<child link="omnipic_OmniPic_tcp" />
|
||||
<origin xyz="0 0 0.14" rpy="0 0 0" />
|
||||
</joint>
|
||||
<!--Omnipic arm mount: edit xyz/rpy to match dual_arm_base.stl.-->
|
||||
<joint name="omnipic_base_mount_joint" type="fixed">
|
||||
<parent link="dual_arm_base_link" />
|
||||
<child link="omnipic_base_link" />
|
||||
<origin xyz="0.03 0 0" rpy="0 -1.57 3.141593" />
|
||||
</joint>
|
||||
<link name="scissor_base_link">
|
||||
<inertial>
|
||||
<origin xyz="0.00049987 5.2709E-05 0.060019" rpy="0 0 0" />
|
||||
<mass value="1.862" />
|
||||
<inertia ixx="0.0017232" ixy="-3.1058E-06" ixz="-3.7924E-05" iyy="0.0017051" iyz="1.3691E-06" izz="0.00090158" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/base_link.STL" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/base_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="scissor_link_1">
|
||||
<inertial>
|
||||
<origin xyz="0.000241 -0.013273 -0.00995" rpy="0 0 0" />
|
||||
<mass value="1.574" />
|
||||
<inertia ixx="0.002487573" ixy="0.000009663" ixz="-0.000007909" iyy="0.002321038" iyz="0.000179393" izz="0.001450554" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_1.STL" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_1.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="scissor_joint_1" type="revolute">
|
||||
<origin xyz="0 0 0.2405" rpy="0 0 0" />
|
||||
<parent link="scissor_base_link" />
|
||||
<child link="scissor_link_1" />
|
||||
<axis xyz="0 0 1" />
|
||||
<limit lower="-3.106" upper="3.106" effort="60" velocity="3.14" />
|
||||
</joint>
|
||||
<link name="scissor_link_2">
|
||||
<inertial>
|
||||
<origin xyz="-0.000357 -0.106789 0.005329" rpy="0 0 0" />
|
||||
<mass value="1.217" />
|
||||
<inertia ixx="0.003494121" ixy="0.000002921" ixz="-0.000005613" iyy="0.000892721" iyz="-0.000583884" izz="0.003444080" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_2.STL" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_2.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="scissor_joint_2" type="revolute">
|
||||
<origin xyz="0 0 0" rpy="-1.5708 0 0" />
|
||||
<parent link="scissor_link_1" />
|
||||
<child link="scissor_link_2" />
|
||||
<axis xyz="0 0 1" />
|
||||
<limit lower="-2.2689" upper="2.2689" effort="60" velocity="3.14" />
|
||||
</joint>
|
||||
<link name="scissor_link_3">
|
||||
<inertial>
|
||||
<origin xyz="0.000003 -0.01398 -0.011324" rpy="0 0 0" />
|
||||
<mass value="1.11" />
|
||||
<inertia ixx="0.001836663" ixy="0.000002259" ixz="-0.000004216" iyy="0.001498875" iyz="0.000037167" izz="0.001062545" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_3.STL" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_3.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="scissor_joint_3" type="revolute">
|
||||
<origin xyz="0 -0.256 0" rpy="1.5708 0 0" />
|
||||
<parent link="scissor_link_2" />
|
||||
<child link="scissor_link_3" />
|
||||
<axis xyz="0 0 1" />
|
||||
<limit lower="-3.106" upper="3.106" effort="30" velocity="3.14" />
|
||||
</joint>
|
||||
<link name="scissor_link_4">
|
||||
<inertial>
|
||||
<origin xyz="-0.000005 -0.084658 0.004747" rpy="0 0 0" />
|
||||
<mass value="0.685" />
|
||||
<inertia ixx="0.001282444" ixy="-0.000000551" ixz="-0.000000630" iyy="0.000373013" iyz="-0.000232084" izz="0.001256177" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_4.STL" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_4.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="scissor_joint_4" type="revolute">
|
||||
<origin xyz="0 0 0" rpy="-1.5708 0 0" />
|
||||
<parent link="scissor_link_3" />
|
||||
<child link="scissor_link_4" />
|
||||
<axis xyz="0 0 1" />
|
||||
<limit lower="-2.356" upper="2.356" effort="30" velocity="3.14" />
|
||||
</joint>
|
||||
<link name="scissor_link_5">
|
||||
<inertial>
|
||||
<origin xyz="0.000078 -0.012937 -0.008781" rpy="0 0 0" />
|
||||
<mass value="0.619" />
|
||||
<inertia ixx="0.000627336" ixy="0.000001636" ixz="-0.000001345" iyy="0.000542455" iyz="0.000034970" izz="0.000370291" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_5.STL" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_5.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="scissor_joint_5" type="revolute">
|
||||
<origin xyz="0 -0.21 0" rpy="1.5708 0 0" />
|
||||
<parent link="scissor_link_4" />
|
||||
<child link="scissor_link_5" />
|
||||
<axis xyz="0 0 1" />
|
||||
<limit lower="-3.106" upper="3.106" effort="10" velocity="3.14" />
|
||||
</joint>
|
||||
<link name="scissor_link_6">
|
||||
<inertial>
|
||||
<origin xyz="-0.000014 -0.078524 0.002819" rpy="0 0 0" />
|
||||
<mass value="0.602" />
|
||||
<inertia ixx="0.000780774" ixy="-0.000000121" ixz="-0.000000469" iyy="0.000289973" iyz="-0.000120513" izz="0.000763955" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_6.STL" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_6.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="scissor_joint_6" type="revolute">
|
||||
<origin xyz="0 0 0" rpy="-1.5708 0 0" />
|
||||
<parent link="scissor_link_5" />
|
||||
<child link="scissor_link_6" />
|
||||
<axis xyz="0 0 1" />
|
||||
<limit lower="-2.234" upper="2.234" effort="10" velocity="3.14" />
|
||||
</joint>
|
||||
<link name="scissor_link_7">
|
||||
<inertial>
|
||||
<origin xyz="0.001094 -0.000077 -0.010119" rpy="0 0 0" />
|
||||
<mass value="0.107" />
|
||||
<inertia ixx="0.000044123" ixy="-0.000000064" ixz="0.0000003" iyy="0.000035078" iyz="-0.000000029" izz="0.000065445" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_7.STL" />
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/link_7.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="scissor_joint_7" type="revolute">
|
||||
<origin xyz="0 -0.144 0" rpy="1.5708 0 0" />
|
||||
<parent link="scissor_link_6" />
|
||||
<child link="scissor_link_7" />
|
||||
<axis xyz="0 0 1" />
|
||||
<limit lower="-6.28" upper="6.28" effort="10" velocity="3.14" />
|
||||
</joint>
|
||||
<link name="scissor_scissor_link">
|
||||
<inertial>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<mass value="0.1" />
|
||||
<inertia ixx="0.0001" ixy="0" ixz="0" iyy="0.0001" iyz="0" izz="0.0001" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/scissor.stl" scale="1 1 1" />
|
||||
</geometry>
|
||||
<material name="scissor_scissor_material">
|
||||
<color rgba="0.7 0.7 0.7 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh filename="meshes/scissor.stl" scale="1 1 1" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="scissor_scissor_fixed_joint" type="fixed">
|
||||
<parent link="scissor_link_7" />
|
||||
<child link="scissor_scissor_link" />
|
||||
<origin xyz="0 0 0.165" rpy="0 0 0" />
|
||||
</joint>
|
||||
<link name="scissor_scissor_tcp" />
|
||||
<joint name="scissor_scissor_tcp_fixed" type="fixed">
|
||||
<parent link="scissor_scissor_link" />
|
||||
<child link="scissor_scissor_tcp" />
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
</joint>
|
||||
<link name="scissor_camera_tcp" />
|
||||
<joint name="scissor_camera_tcp_fixed" type="fixed">
|
||||
<parent link="scissor_scissor_link" />
|
||||
<child link="scissor_camera_tcp" />
|
||||
<origin xyz="0.042 0 -0.0755" rpy="0 0 1.57" />
|
||||
</joint>
|
||||
<!--Scissor arm mount: edit xyz/rpy to match dual_arm_base.stl.-->
|
||||
<joint name="scissor_base_mount_joint" type="fixed">
|
||||
<parent link="dual_arm_base_link" />
|
||||
<child link="scissor_base_link" />
|
||||
<origin xyz="-0.030 0 0" rpy="0 1.57 3.141593" />
|
||||
</joint>
|
||||
</robot>
|
||||
@@ -0,0 +1,9 @@
|
||||
Link Name,Center of Mass X,Center of Mass Y,Center of Mass Z,Center of Mass Roll,Center of Mass Pitch,Center of Mass Yaw,Mass,Moment Ixx,Moment Ixy,Moment Ixz,Moment Iyy,Moment Iyz,Moment Izz,Visual X,Visual Y,Visual Z,Visual Roll,Visual Pitch,Visual Yaw,Mesh Filename,Color Red,Color Green,Color Blue,Color Alpha,Collision X,Collision Y,Collision Z,Collision Roll,Collision Pitch,Collision Yaw,Collision Mesh Filename,Material Name,SW Components,Coordinate System,Axis Name,Joint Name,Joint Type,Joint Origin X,Joint Origin Y,Joint Origin Z,Joint Origin Roll,Joint Origin Pitch,Joint Origin Yaw,Parent,Joint Axis X,Joint Axis Y,Joint Axis Z,Limit Effort,Limit Velocity,Limit Lower,Limit Upper,Calibration rising,Calibration falling,Dynamics Damping,Dynamics Friction,Safety Soft Upper,Safety Soft Lower,Safety K Position,Safety K Velocity
|
||||
base_link,0.00049987,5.2709E-05,0.060019,0,0,0,0.83887,0.0017232,-3.1058E-06,-3.7924E-05,0.0017051,1.3691E-06,0.00090158,0,0,0,0,0,0,package://RM75-B/meshes/base_link.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/base_link.STL,,连杆1-1,base_link,,,,0,0,0,0,0,0,,0,0,0,,,,,,,,,,,,
|
||||
link_1,1.4803E-07,-0.021108,-0.025186,0,0,0,0.59354,0.0012661,6.0354E-09,-6.3788E-09,0.0011817,-0.00021121,0.00056132,0,0,0,0,0,0,package://RM75-B/meshes/link_1.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_1.STL,,连杆2-1,link_1,joint_1,joint_1,revolute,0,0,0.2405,0,0,0,base_link,0,0,1,60,3.14,-3.106,3.106,,,,,,,,
|
||||
link_2,4.2145E-07,-0.076129,0.011078,0,0,0,0.43285,0.0012584,1.4694E-09,-5.7413E-09,0.00031747,0.000279,0.0012225,0,0,0,0,0,0,package://RM75-B/meshes/link_2.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_2.STL,,连杆3-1,link_2,joint_2,joint_2,revolute,0,0,0,-1.5708,0,0,link_1,0,0,1,60,3.14,-2.2689,2.2689,,,,,,,,
|
||||
link_3,-3.2093E-07,-0.023545,-0.027347,0,0,0,0.43132,0.00079433,1.02E-09,1.3908E-08,0.00073037,-0.00014262,0.00031507,0,0,0,0,0,0,package://RM75-B/meshes/link_3.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_3.STL,,连杆4-1,link_3,joint_3,joint_3,revolute,0,-0.256,0,1.5708,0,0,link_2,0,0,1,30,3.14,-3.106,3.106,,,,,,,,
|
||||
link_4,5.0722E-06,-0.059593,0.010569,0,0,0,0.28963,0.00063737,7.0681E-08,3.8708E-08,0.00015648,0.00014461,0.00061418,0,0,0,0,0,0,package://RM75-B/meshes/link_4.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_4.STL,,连杆5-1,link_4,joint_4,joint_4,revolute,0,0,0,-1.5708,0,0,link_3,0,0,1,30,3.14,-2.356,2.356,,,,,,,,
|
||||
link_5,2.7551E-07,-0.018042,-0.02154,0,0,0,0.23942,0.00028595,1.9823E-09,-1.192E-09,0.00026273,-4.424E-05,0.0001199,0,0,0,0,0,0,package://RM75-B/meshes/link_5.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_5.STL,,连杆6-1,link_5,joint_5,joint_5,revolute,0,-0.21,0,1.5708,0,0,link_4,0,0,1,10,3.14,-3.106,3.106,,,,,,,,
|
||||
link_6,3.4947E-06,-0.059381,0.0073681,0,0,0,0.2188,0.00035054,3.4456E-08,1.7975E-08,0.00010493,7.8243E-05,0.00033448,0,0,0,0,0,0,package://RM75-B/meshes/link_6.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_6.STL,,连杆7-1,link_6,joint_6,joint_6,revolute,0,0,0,-1.5708,0,0,link_5,0,0,1,10,3.14,-2.234,2.234,,,,,,,,
|
||||
link_7,0.00081557,1.3323E-05,-0.012705,0,0,0,0.065037,2.1144E-05,2.2774E-08,2.5471E-08,1.8109E-05,1.019E-08,3.19E-05,0,0,0,0,0,0,package://RM75-B/meshes/link_7.STL,1,1,1,1,0,0,0,0,0,0,package://RM75-B/meshes/link_7.STL,,末端法兰件 方案一-1,link_7,joint_7,joint_7,revolute,0,-0.144,0,1.5708,0,0,link_6,0,0,1,10,3.14,-6.28,6.28,,,,,,,,
|
||||
|
@@ -0,0 +1,453 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com)
|
||||
Commit Version: 1.6.0-1-g15f4949 Build Version: 1.6.7594.29634
|
||||
For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
|
||||
<robot
|
||||
name="RM75-B">
|
||||
<link
|
||||
name="base_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.00049987 5.2709E-05 0.060019"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.862" />
|
||||
<inertia
|
||||
ixx="0.0017232"
|
||||
ixy="-3.1058E-06"
|
||||
ixz="-3.7924E-05"
|
||||
iyy="0.0017051"
|
||||
iyz="1.3691E-06"
|
||||
izz="0.00090158" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/base_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/base_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link
|
||||
name="link_1">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000241 -0.013273 -0.00995"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.574" />
|
||||
<inertia
|
||||
ixx="0.002487573"
|
||||
ixy="0.000009663"
|
||||
ixz="-0.000007909"
|
||||
iyy="0.002321038"
|
||||
iyz="0.000179393"
|
||||
izz="0.001450554" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_1.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_1.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_1"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0.2405"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="base_link" />
|
||||
<child
|
||||
link="link_1" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.106"
|
||||
upper="3.106"
|
||||
effort="60"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_2">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000357 -0.106789 0.005329"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.217" />
|
||||
<inertia
|
||||
ixx="0.003494121"
|
||||
ixy="0.000002921"
|
||||
ixz="-0.000005613"
|
||||
iyy="0.000892721"
|
||||
iyz="-0.000583884"
|
||||
izz="0.003444080" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_2.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_2.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_2"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="link_1" />
|
||||
<child
|
||||
link="link_2" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.2689"
|
||||
upper="2.2689"
|
||||
effort="60"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_3">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000003 -0.01398 -0.011324"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.11" />
|
||||
<inertia
|
||||
ixx="0.001836663"
|
||||
ixy="0.000002259"
|
||||
ixz="-0.000004216"
|
||||
iyy="0.001498875"
|
||||
iyz="0.000037167"
|
||||
izz="0.001062545" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_3.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_3.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_3"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.256 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_2" />
|
||||
<child
|
||||
link="link_3" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.106"
|
||||
upper="3.106"
|
||||
effort="30"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_4">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000005 -0.084658 0.004747"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.685" />
|
||||
<inertia
|
||||
ixx="0.001282444"
|
||||
ixy="-0.000000551"
|
||||
ixz="-0.000000630"
|
||||
iyy="0.000373013"
|
||||
iyz="-0.000232084"
|
||||
izz="0.001256177" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_4.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_4.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_4"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="link_3" />
|
||||
<child
|
||||
link="link_4" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.356"
|
||||
upper="2.356"
|
||||
effort="30"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_5">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000078 -0.012937 -0.008781"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.619" />
|
||||
<inertia
|
||||
ixx="0.000627336"
|
||||
ixy="0.000001636"
|
||||
ixz="-0.000001345"
|
||||
iyy="0.000542455"
|
||||
iyz="0.000034970"
|
||||
izz="0.000370291" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_5.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_5.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_5"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.21 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_4" />
|
||||
<child
|
||||
link="link_5" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.106"
|
||||
upper="3.106"
|
||||
effort="10"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_6">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000014 -0.078524 0.002819"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.602" />
|
||||
<inertia
|
||||
ixx="0.000780774"
|
||||
ixy="-0.000000121"
|
||||
ixz="-0.000000469"
|
||||
iyy="0.000289973"
|
||||
iyz="-0.000120513"
|
||||
izz="0.000763955" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_6.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_6.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_6"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="link_5" />
|
||||
<child
|
||||
link="link_6" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.234"
|
||||
upper="2.234"
|
||||
effort="10"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_7">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.001094 -0.000077 -0.010119"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.107" />
|
||||
<inertia
|
||||
ixx="0.000044123"
|
||||
ixy="-0.000000064"
|
||||
ixz="0.0000003"
|
||||
iyy="0.000035078"
|
||||
iyz="-0.000000029"
|
||||
izz="0.000065445" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_7.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_7.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_7"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.144 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_6" />
|
||||
<child
|
||||
link="link_7" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-6.28"
|
||||
upper="6.28"
|
||||
effort="10"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
</robot>
|
||||
@@ -0,0 +1,507 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com)
|
||||
Commit Version: 1.6.0-1-g15f4949 Build Version: 1.6.7594.29634
|
||||
For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
|
||||
<robot
|
||||
name="RM75-B">
|
||||
<link
|
||||
name="base_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.00049987 5.2709E-05 0.060019"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.862" />
|
||||
<inertia
|
||||
ixx="0.0017232"
|
||||
ixy="-3.1058E-06"
|
||||
ixz="-3.7924E-05"
|
||||
iyy="0.0017051"
|
||||
iyz="1.3691E-06"
|
||||
izz="0.00090158" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/base_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/base_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link
|
||||
name="link_1">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000241 -0.013273 -0.00995"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.574" />
|
||||
<inertia
|
||||
ixx="0.002487573"
|
||||
ixy="0.000009663"
|
||||
ixz="-0.000007909"
|
||||
iyy="0.002321038"
|
||||
iyz="0.000179393"
|
||||
izz="0.001450554" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_1.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_1.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_1"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0.2405"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="base_link" />
|
||||
<child
|
||||
link="link_1" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.106"
|
||||
upper="3.106"
|
||||
effort="60"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_2">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000357 -0.106789 0.005329"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.217" />
|
||||
<inertia
|
||||
ixx="0.003494121"
|
||||
ixy="0.000002921"
|
||||
ixz="-0.000005613"
|
||||
iyy="0.000892721"
|
||||
iyz="-0.000583884"
|
||||
izz="0.003444080" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_2.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_2.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_2"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="link_1" />
|
||||
<child
|
||||
link="link_2" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.2689"
|
||||
upper="2.2689"
|
||||
effort="60"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_3">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000003 -0.01398 -0.011324"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.11" />
|
||||
<inertia
|
||||
ixx="0.001836663"
|
||||
ixy="0.000002259"
|
||||
ixz="-0.000004216"
|
||||
iyy="0.001498875"
|
||||
iyz="0.000037167"
|
||||
izz="0.001062545" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_3.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_3.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_3"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.256 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_2" />
|
||||
<child
|
||||
link="link_3" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.106"
|
||||
upper="3.106"
|
||||
effort="30"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_4">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000005 -0.084658 0.004747"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.685" />
|
||||
<inertia
|
||||
ixx="0.001282444"
|
||||
ixy="-0.000000551"
|
||||
ixz="-0.000000630"
|
||||
iyy="0.000373013"
|
||||
iyz="-0.000232084"
|
||||
izz="0.001256177" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_4.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_4.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_4"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="link_3" />
|
||||
<child
|
||||
link="link_4" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.356"
|
||||
upper="2.356"
|
||||
effort="30"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_5">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000078 -0.012937 -0.008781"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.619" />
|
||||
<inertia
|
||||
ixx="0.000627336"
|
||||
ixy="0.000001636"
|
||||
ixz="-0.000001345"
|
||||
iyy="0.000542455"
|
||||
iyz="0.000034970"
|
||||
izz="0.000370291" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_5.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_5.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_5"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.21 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_4" />
|
||||
<child
|
||||
link="link_5" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.106"
|
||||
upper="3.106"
|
||||
effort="10"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_6">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000014 -0.078524 0.002819"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.602" />
|
||||
<inertia
|
||||
ixx="0.000780774"
|
||||
ixy="-0.000000121"
|
||||
ixz="-0.000000469"
|
||||
iyy="0.000289973"
|
||||
iyz="-0.000120513"
|
||||
izz="0.000763955" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_6.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_6.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_6"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="link_5" />
|
||||
<child
|
||||
link="link_6" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.234"
|
||||
upper="2.234"
|
||||
effort="10"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_7">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.001094 -0.000077 -0.010119"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.107" />
|
||||
<inertia
|
||||
ixx="0.000044123"
|
||||
ixy="-0.000000064"
|
||||
ixz="0.0000003"
|
||||
iyy="0.000035078"
|
||||
iyz="-0.000000029"
|
||||
izz="0.000065445" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_7.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_7.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_7"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.144 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_6" />
|
||||
<child
|
||||
link="link_7" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-6.28"
|
||||
upper="6.28"
|
||||
effort="10"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<!-- Scissor end-effector -->
|
||||
<link name="gripper_link">
|
||||
<inertial>
|
||||
<!-- Replace these values with CAD-derived mass properties -->
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<mass value="0.1" />
|
||||
<inertia
|
||||
ixx="0.0001"
|
||||
ixy="0"
|
||||
ixz="0"
|
||||
iyy="0.0001"
|
||||
iyz="0"
|
||||
izz="0.0001" />
|
||||
</inertial>
|
||||
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/OmniPic.stl"
|
||||
scale="1 1 1" />
|
||||
</geometry>
|
||||
<material name="OmniPic_material">
|
||||
<color rgba="0.7 0.7 0.7 1" />
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/OmniPic.stl"
|
||||
scale="1 1 1" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<!-- Rigidly attach the gripper to the final wrist link -->
|
||||
<joint name="OmniPic_fixed_joint" type="fixed">
|
||||
<parent link="link_7" />
|
||||
<child link="gripper_link" />
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
</joint>
|
||||
|
||||
|
||||
<link name="OmniPic_tcp"/>
|
||||
|
||||
<joint name="OmniPic_tcp_fixed" type="fixed">
|
||||
<parent link="gripper_link"/>
|
||||
<child link="OmniPic_tcp"/>
|
||||
<origin xyz="0 0 0.14" rpy="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
|
||||
</robot>
|
||||
@@ -0,0 +1,516 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com)
|
||||
Commit Version: 1.6.0-1-g15f4949 Build Version: 1.6.7594.29634
|
||||
For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
|
||||
<robot
|
||||
name="RM75-B">
|
||||
<link
|
||||
name="base_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.00049987 5.2709E-05 0.060019"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.862" />
|
||||
<inertia
|
||||
ixx="0.0017232"
|
||||
ixy="-3.1058E-06"
|
||||
ixz="-3.7924E-05"
|
||||
iyy="0.0017051"
|
||||
iyz="1.3691E-06"
|
||||
izz="0.00090158" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/base_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/base_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link
|
||||
name="link_1">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000241 -0.013273 -0.00995"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.574" />
|
||||
<inertia
|
||||
ixx="0.002487573"
|
||||
ixy="0.000009663"
|
||||
ixz="-0.000007909"
|
||||
iyy="0.002321038"
|
||||
iyz="0.000179393"
|
||||
izz="0.001450554" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_1.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_1.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_1"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0.2405"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="base_link" />
|
||||
<child
|
||||
link="link_1" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.106"
|
||||
upper="3.106"
|
||||
effort="60"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_2">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000357 -0.106789 0.005329"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.217" />
|
||||
<inertia
|
||||
ixx="0.003494121"
|
||||
ixy="0.000002921"
|
||||
ixz="-0.000005613"
|
||||
iyy="0.000892721"
|
||||
iyz="-0.000583884"
|
||||
izz="0.003444080" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_2.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_2.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_2"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="link_1" />
|
||||
<child
|
||||
link="link_2" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.2689"
|
||||
upper="2.2689"
|
||||
effort="60"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_3">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000003 -0.01398 -0.011324"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.11" />
|
||||
<inertia
|
||||
ixx="0.001836663"
|
||||
ixy="0.000002259"
|
||||
ixz="-0.000004216"
|
||||
iyy="0.001498875"
|
||||
iyz="0.000037167"
|
||||
izz="0.001062545" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_3.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_3.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_3"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.256 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_2" />
|
||||
<child
|
||||
link="link_3" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.106"
|
||||
upper="3.106"
|
||||
effort="30"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_4">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000005 -0.084658 0.004747"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.685" />
|
||||
<inertia
|
||||
ixx="0.001282444"
|
||||
ixy="-0.000000551"
|
||||
ixz="-0.000000630"
|
||||
iyy="0.000373013"
|
||||
iyz="-0.000232084"
|
||||
izz="0.001256177" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_4.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_4.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_4"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="link_3" />
|
||||
<child
|
||||
link="link_4" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.356"
|
||||
upper="2.356"
|
||||
effort="30"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_5">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000078 -0.012937 -0.008781"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.619" />
|
||||
<inertia
|
||||
ixx="0.000627336"
|
||||
ixy="0.000001636"
|
||||
ixz="-0.000001345"
|
||||
iyy="0.000542455"
|
||||
iyz="0.000034970"
|
||||
izz="0.000370291" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_5.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_5.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_5"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.21 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_4" />
|
||||
<child
|
||||
link="link_5" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.106"
|
||||
upper="3.106"
|
||||
effort="10"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_6">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000014 -0.078524 0.002819"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.602" />
|
||||
<inertia
|
||||
ixx="0.000780774"
|
||||
ixy="-0.000000121"
|
||||
ixz="-0.000000469"
|
||||
iyy="0.000289973"
|
||||
iyz="-0.000120513"
|
||||
izz="0.000763955" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_6.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_6.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_6"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="link_5" />
|
||||
<child
|
||||
link="link_6" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.234"
|
||||
upper="2.234"
|
||||
effort="10"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_7">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.001094 -0.000077 -0.010119"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.107" />
|
||||
<inertia
|
||||
ixx="0.000044123"
|
||||
ixy="-0.000000064"
|
||||
ixz="0.0000003"
|
||||
iyy="0.000035078"
|
||||
iyz="-0.000000029"
|
||||
izz="0.000065445" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_7.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_7.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_7"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.144 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_6" />
|
||||
<child
|
||||
link="link_7" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-6.28"
|
||||
upper="6.28"
|
||||
effort="10"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<!-- Scissor end-effector -->
|
||||
<link name="scissor_link">
|
||||
<inertial>
|
||||
<!-- Replace these values with CAD-derived mass properties -->
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<mass value="0.1" />
|
||||
<inertia
|
||||
ixx="0.0001"
|
||||
ixy="0"
|
||||
ixz="0"
|
||||
iyy="0.0001"
|
||||
iyz="0"
|
||||
izz="0.0001" />
|
||||
</inertial>
|
||||
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/scissor.stl"
|
||||
scale="1 1 1" />
|
||||
</geometry>
|
||||
<material name="scissor_material">
|
||||
<color rgba="0.7 0.7 0.7 1" />
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/scissor.stl"
|
||||
scale="1 1 1" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<!-- Rigidly attach the scissor to the final wrist link -->
|
||||
<joint name="scissor_fixed_joint" type="fixed">
|
||||
<parent link="link_7" />
|
||||
<child link="scissor_link" />
|
||||
<origin xyz="0 -0.12 0.03" rpy="1.5707963 0 0" />
|
||||
</joint>
|
||||
|
||||
|
||||
<link name="scissor_tcp"/>
|
||||
|
||||
<joint name="scissor_tcp_fixed" type="fixed">
|
||||
<parent link="scissor_link"/>
|
||||
<child link="scissor_tcp"/>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<link name="camera_tcp"/>
|
||||
|
||||
<joint name="camera_tcp_fixed" type="fixed">
|
||||
<parent link="scissor_link"/>
|
||||
<child link="camera_tcp"/>
|
||||
<origin xyz="0.042 0 -0.0755" rpy="0 0 1.5707963"/>
|
||||
</joint>
|
||||
|
||||
|
||||
|
||||
</robot>
|
||||
@@ -0,0 +1,516 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com)
|
||||
Commit Version: 1.6.0-1-g15f4949 Build Version: 1.6.7594.29634
|
||||
For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
|
||||
<robot
|
||||
name="RM75-B">
|
||||
<link
|
||||
name="base_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.00049987 5.2709E-05 0.060019"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.862" />
|
||||
<inertia
|
||||
ixx="0.0017232"
|
||||
ixy="-3.1058E-06"
|
||||
ixz="-3.7924E-05"
|
||||
iyy="0.0017051"
|
||||
iyz="1.3691E-06"
|
||||
izz="0.00090158" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/base_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/base_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link
|
||||
name="link_1">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000241 -0.013273 -0.00995"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.574" />
|
||||
<inertia
|
||||
ixx="0.002487573"
|
||||
ixy="0.000009663"
|
||||
ixz="-0.000007909"
|
||||
iyy="0.002321038"
|
||||
iyz="0.000179393"
|
||||
izz="0.001450554" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_1.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_1.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_1"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0.2405"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="base_link" />
|
||||
<child
|
||||
link="link_1" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.106"
|
||||
upper="3.106"
|
||||
effort="60"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_2">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000357 -0.106789 0.005329"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.217" />
|
||||
<inertia
|
||||
ixx="0.003494121"
|
||||
ixy="0.000002921"
|
||||
ixz="-0.000005613"
|
||||
iyy="0.000892721"
|
||||
iyz="-0.000583884"
|
||||
izz="0.003444080" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_2.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_2.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_2"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="link_1" />
|
||||
<child
|
||||
link="link_2" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.2689"
|
||||
upper="2.2689"
|
||||
effort="60"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_3">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000003 -0.01398 -0.011324"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.11" />
|
||||
<inertia
|
||||
ixx="0.001836663"
|
||||
ixy="0.000002259"
|
||||
ixz="-0.000004216"
|
||||
iyy="0.001498875"
|
||||
iyz="0.000037167"
|
||||
izz="0.001062545" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_3.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_3.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_3"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.256 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_2" />
|
||||
<child
|
||||
link="link_3" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.106"
|
||||
upper="3.106"
|
||||
effort="30"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_4">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000005 -0.084658 0.004747"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.685" />
|
||||
<inertia
|
||||
ixx="0.001282444"
|
||||
ixy="-0.000000551"
|
||||
ixz="-0.000000630"
|
||||
iyy="0.000373013"
|
||||
iyz="-0.000232084"
|
||||
izz="0.001256177" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_4.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_4.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_4"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="link_3" />
|
||||
<child
|
||||
link="link_4" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.356"
|
||||
upper="2.356"
|
||||
effort="30"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_5">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000078 -0.012937 -0.008781"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.619" />
|
||||
<inertia
|
||||
ixx="0.000627336"
|
||||
ixy="0.000001636"
|
||||
ixz="-0.000001345"
|
||||
iyy="0.000542455"
|
||||
iyz="0.000034970"
|
||||
izz="0.000370291" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_5.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_5.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_5"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.21 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_4" />
|
||||
<child
|
||||
link="link_5" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.106"
|
||||
upper="3.106"
|
||||
effort="10"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_6">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000014 -0.078524 0.002819"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.602" />
|
||||
<inertia
|
||||
ixx="0.000780774"
|
||||
ixy="-0.000000121"
|
||||
ixz="-0.000000469"
|
||||
iyy="0.000289973"
|
||||
iyz="-0.000120513"
|
||||
izz="0.000763955" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_6.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_6.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_6"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="link_5" />
|
||||
<child
|
||||
link="link_6" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.234"
|
||||
upper="2.234"
|
||||
effort="10"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_7">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.001094 -0.000077 -0.010119"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.107" />
|
||||
<inertia
|
||||
ixx="0.000044123"
|
||||
ixy="-0.000000064"
|
||||
ixz="0.0000003"
|
||||
iyy="0.000035078"
|
||||
iyz="-0.000000029"
|
||||
izz="0.000065445" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_7.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_7.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_7"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.144 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_6" />
|
||||
<child
|
||||
link="link_7" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-6.28"
|
||||
upper="6.28"
|
||||
effort="10"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<!-- Scissor end-effector -->
|
||||
<link name="scissor_link">
|
||||
<inertial>
|
||||
<!-- Replace these values with CAD-derived mass properties -->
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<mass value="0.1" />
|
||||
<inertia
|
||||
ixx="0.0001"
|
||||
ixy="0"
|
||||
ixz="0"
|
||||
iyy="0.0001"
|
||||
iyz="0"
|
||||
izz="0.0001" />
|
||||
</inertial>
|
||||
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/scissor.stl"
|
||||
scale="1 1 1" />
|
||||
</geometry>
|
||||
<material name="scissor_material">
|
||||
<color rgba="0.7 0.7 0.7 1" />
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/scissor.stl"
|
||||
scale="1 1 1" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<!-- Rigidly attach the scissor to the final wrist link -->
|
||||
<joint name="scissor_fixed_joint" type="fixed">
|
||||
<parent link="link_7" />
|
||||
<child link="scissor_link" />
|
||||
<origin xyz="-0.12 0 0.063" rpy="0 -1.5707963 0" />
|
||||
</joint>
|
||||
|
||||
|
||||
<link name="scissor_tcp"/>
|
||||
|
||||
<joint name="scissor_tcp_fixed" type="fixed">
|
||||
<parent link="scissor_link"/>
|
||||
<child link="scissor_tcp"/>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<link name="camera_tcp"/>
|
||||
|
||||
<joint name="camera_tcp_fixed" type="fixed">
|
||||
<parent link="scissor_link"/>
|
||||
<child link="camera_tcp"/>
|
||||
<origin xyz="0.042 0 -0.0755" rpy="0 0 1.57"/>
|
||||
</joint>
|
||||
|
||||
|
||||
|
||||
</robot>
|
||||
@@ -0,0 +1,516 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com)
|
||||
Commit Version: 1.6.0-1-g15f4949 Build Version: 1.6.7594.29634
|
||||
For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
|
||||
<robot
|
||||
name="RM75-B">
|
||||
<link
|
||||
name="base_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.00049987 5.2709E-05 0.060019"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.862" />
|
||||
<inertia
|
||||
ixx="0.0017232"
|
||||
ixy="-3.1058E-06"
|
||||
ixz="-3.7924E-05"
|
||||
iyy="0.0017051"
|
||||
iyz="1.3691E-06"
|
||||
izz="0.00090158" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/base_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/base_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link
|
||||
name="link_1">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000241 -0.013273 -0.00995"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.574" />
|
||||
<inertia
|
||||
ixx="0.002487573"
|
||||
ixy="0.000009663"
|
||||
ixz="-0.000007909"
|
||||
iyy="0.002321038"
|
||||
iyz="0.000179393"
|
||||
izz="0.001450554" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_1.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_1.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_1"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0.2405"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="base_link" />
|
||||
<child
|
||||
link="link_1" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.106"
|
||||
upper="3.106"
|
||||
effort="60"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_2">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000357 -0.106789 0.005329"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.217" />
|
||||
<inertia
|
||||
ixx="0.003494121"
|
||||
ixy="0.000002921"
|
||||
ixz="-0.000005613"
|
||||
iyy="0.000892721"
|
||||
iyz="-0.000583884"
|
||||
izz="0.003444080" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_2.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_2.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_2"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="link_1" />
|
||||
<child
|
||||
link="link_2" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.2689"
|
||||
upper="2.2689"
|
||||
effort="60"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_3">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000003 -0.01398 -0.011324"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.11" />
|
||||
<inertia
|
||||
ixx="0.001836663"
|
||||
ixy="0.000002259"
|
||||
ixz="-0.000004216"
|
||||
iyy="0.001498875"
|
||||
iyz="0.000037167"
|
||||
izz="0.001062545" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_3.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_3.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_3"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.256 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_2" />
|
||||
<child
|
||||
link="link_3" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.106"
|
||||
upper="3.106"
|
||||
effort="30"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_4">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000005 -0.084658 0.004747"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.685" />
|
||||
<inertia
|
||||
ixx="0.001282444"
|
||||
ixy="-0.000000551"
|
||||
ixz="-0.000000630"
|
||||
iyy="0.000373013"
|
||||
iyz="-0.000232084"
|
||||
izz="0.001256177" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_4.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_4.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_4"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="link_3" />
|
||||
<child
|
||||
link="link_4" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.356"
|
||||
upper="2.356"
|
||||
effort="30"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_5">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000078 -0.012937 -0.008781"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.619" />
|
||||
<inertia
|
||||
ixx="0.000627336"
|
||||
ixy="0.000001636"
|
||||
ixz="-0.000001345"
|
||||
iyy="0.000542455"
|
||||
iyz="0.000034970"
|
||||
izz="0.000370291" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_5.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_5.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_5"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.21 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_4" />
|
||||
<child
|
||||
link="link_5" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.106"
|
||||
upper="3.106"
|
||||
effort="10"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_6">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000014 -0.078524 0.002819"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.602" />
|
||||
<inertia
|
||||
ixx="0.000780774"
|
||||
ixy="-0.000000121"
|
||||
ixz="-0.000000469"
|
||||
iyy="0.000289973"
|
||||
iyz="-0.000120513"
|
||||
izz="0.000763955" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_6.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_6.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_6"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="link_5" />
|
||||
<child
|
||||
link="link_6" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.234"
|
||||
upper="2.234"
|
||||
effort="10"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_7">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.001094 -0.000077 -0.010119"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.107" />
|
||||
<inertia
|
||||
ixx="0.000044123"
|
||||
ixy="-0.000000064"
|
||||
ixz="0.0000003"
|
||||
iyy="0.000035078"
|
||||
iyz="-0.000000029"
|
||||
izz="0.000065445" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_7.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_7.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_7"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.144 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_6" />
|
||||
<child
|
||||
link="link_7" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-6.28"
|
||||
upper="6.28"
|
||||
effort="10"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<!-- Scissor end-effector -->
|
||||
<link name="scissor_link">
|
||||
<inertial>
|
||||
<!-- Replace these values with CAD-derived mass properties -->
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<mass value="0.1" />
|
||||
<inertia
|
||||
ixx="0.0001"
|
||||
ixy="0"
|
||||
ixz="0"
|
||||
iyy="0.0001"
|
||||
iyz="0"
|
||||
izz="0.0001" />
|
||||
</inertial>
|
||||
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/scissor.stl"
|
||||
scale="1 1 1" />
|
||||
</geometry>
|
||||
<material name="scissor_material">
|
||||
<color rgba="0.7 0.7 0.7 1" />
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/scissor.stl"
|
||||
scale="1 1 1" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<!-- Rigidly attach the scissor to the final wrist link -->
|
||||
<joint name="scissor_fixed_joint" type="fixed">
|
||||
<parent link="link_7" />
|
||||
<child link="scissor_link" />
|
||||
<origin xyz="0 0 0.165" rpy="0 0 0" />
|
||||
</joint>
|
||||
|
||||
|
||||
<link name="scissor_tcp"/>
|
||||
|
||||
<joint name="scissor_tcp_fixed" type="fixed">
|
||||
<parent link="scissor_link"/>
|
||||
<child link="scissor_tcp"/>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<link name="camera_tcp"/>
|
||||
|
||||
<joint name="camera_tcp_fixed" type="fixed">
|
||||
<parent link="scissor_link"/>
|
||||
<child link="camera_tcp"/>
|
||||
<origin xyz="0.042 0 -0.0755" rpy="0 0 1.57"/>
|
||||
</joint>
|
||||
|
||||
|
||||
|
||||
</robot>
|
||||
@@ -0,0 +1,516 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com)
|
||||
Commit Version: 1.6.0-1-g15f4949 Build Version: 1.6.7594.29634
|
||||
For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
|
||||
<robot
|
||||
name="RM75-B">
|
||||
<link
|
||||
name="base_link">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.00049987 5.2709E-05 0.060019"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.862" />
|
||||
<inertia
|
||||
ixx="0.0017232"
|
||||
ixy="-3.1058E-06"
|
||||
ixz="-3.7924E-05"
|
||||
iyy="0.0017051"
|
||||
iyz="1.3691E-06"
|
||||
izz="0.00090158" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/base_link.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/base_link.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link
|
||||
name="link_1">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000241 -0.013273 -0.00995"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.574" />
|
||||
<inertia
|
||||
ixx="0.002487573"
|
||||
ixy="0.000009663"
|
||||
ixz="-0.000007909"
|
||||
iyy="0.002321038"
|
||||
iyz="0.000179393"
|
||||
izz="0.001450554" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_1.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_1.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_1"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0.2405"
|
||||
rpy="0 0 0" />
|
||||
<parent
|
||||
link="base_link" />
|
||||
<child
|
||||
link="link_1" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.106"
|
||||
upper="3.106"
|
||||
effort="60"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_2">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000357 -0.106789 0.005329"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.217" />
|
||||
<inertia
|
||||
ixx="0.003494121"
|
||||
ixy="0.000002921"
|
||||
ixz="-0.000005613"
|
||||
iyy="0.000892721"
|
||||
iyz="-0.000583884"
|
||||
izz="0.003444080" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_2.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_2.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_2"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="link_1" />
|
||||
<child
|
||||
link="link_2" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.2689"
|
||||
upper="2.2689"
|
||||
effort="60"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_3">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000003 -0.01398 -0.011324"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="1.11" />
|
||||
<inertia
|
||||
ixx="0.001836663"
|
||||
ixy="0.000002259"
|
||||
ixz="-0.000004216"
|
||||
iyy="0.001498875"
|
||||
iyz="0.000037167"
|
||||
izz="0.001062545" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_3.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_3.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_3"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.256 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_2" />
|
||||
<child
|
||||
link="link_3" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.106"
|
||||
upper="3.106"
|
||||
effort="30"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_4">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000005 -0.084658 0.004747"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.685" />
|
||||
<inertia
|
||||
ixx="0.001282444"
|
||||
ixy="-0.000000551"
|
||||
ixz="-0.000000630"
|
||||
iyy="0.000373013"
|
||||
iyz="-0.000232084"
|
||||
izz="0.001256177" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_4.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_4.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_4"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="link_3" />
|
||||
<child
|
||||
link="link_4" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.356"
|
||||
upper="2.356"
|
||||
effort="30"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_5">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.000078 -0.012937 -0.008781"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.619" />
|
||||
<inertia
|
||||
ixx="0.000627336"
|
||||
ixy="0.000001636"
|
||||
ixz="-0.000001345"
|
||||
iyy="0.000542455"
|
||||
iyz="0.000034970"
|
||||
izz="0.000370291" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_5.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_5.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_5"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.21 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_4" />
|
||||
<child
|
||||
link="link_5" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-3.106"
|
||||
upper="3.106"
|
||||
effort="10"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_6">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="-0.000014 -0.078524 0.002819"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.602" />
|
||||
<inertia
|
||||
ixx="0.000780774"
|
||||
ixy="-0.000000121"
|
||||
ixz="-0.000000469"
|
||||
iyy="0.000289973"
|
||||
iyz="-0.000120513"
|
||||
izz="0.000763955" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_6.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_6.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_6"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="-1.5708 0 0" />
|
||||
<parent
|
||||
link="link_5" />
|
||||
<child
|
||||
link="link_6" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-2.234"
|
||||
upper="2.234"
|
||||
effort="10"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<link
|
||||
name="link_7">
|
||||
<inertial>
|
||||
<origin
|
||||
xyz="0.001094 -0.000077 -0.010119"
|
||||
rpy="0 0 0" />
|
||||
<mass
|
||||
value="0.107" />
|
||||
<inertia
|
||||
ixx="0.000044123"
|
||||
ixy="-0.000000064"
|
||||
ixz="0.0000003"
|
||||
iyy="0.000035078"
|
||||
iyz="-0.000000029"
|
||||
izz="0.000065445" />
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_7.STL" />
|
||||
</geometry>
|
||||
<material
|
||||
name="">
|
||||
<color
|
||||
rgba="1 1 1 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin
|
||||
xyz="0 0 0"
|
||||
rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/link_7.STL" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint
|
||||
name="joint_7"
|
||||
type="revolute">
|
||||
<origin
|
||||
xyz="0 -0.144 0"
|
||||
rpy="1.5708 0 0" />
|
||||
<parent
|
||||
link="link_6" />
|
||||
<child
|
||||
link="link_7" />
|
||||
<axis
|
||||
xyz="0 0 1" />
|
||||
<limit
|
||||
lower="-6.28"
|
||||
upper="6.28"
|
||||
effort="10"
|
||||
velocity="3.14" />
|
||||
</joint>
|
||||
<!-- Scissor end-effector -->
|
||||
<link name="scissor_link">
|
||||
<inertial>
|
||||
<!-- Replace these values with CAD-derived mass properties -->
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<mass value="0.1" />
|
||||
<inertia
|
||||
ixx="0.0001"
|
||||
ixy="0"
|
||||
ixz="0"
|
||||
iyy="0.0001"
|
||||
iyz="0"
|
||||
izz="0.0001" />
|
||||
</inertial>
|
||||
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/mini_scissor.stl"
|
||||
scale="1 1 1" />
|
||||
</geometry>
|
||||
<material name="scissor_material">
|
||||
<color rgba="0.7 0.7 0.7 1" />
|
||||
</material>
|
||||
</visual>
|
||||
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||
<geometry>
|
||||
<mesh
|
||||
filename="meshes/mini_scissor.stl"
|
||||
scale="1 1 1" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<!-- Rigidly attach the scissor to the final wrist link -->
|
||||
<joint name="scissor_fixed_joint" type="fixed">
|
||||
<parent link="link_7" />
|
||||
<child link="scissor_link" />
|
||||
<origin xyz="0 0 0.165" rpy="0 0 0" />
|
||||
</joint>
|
||||
|
||||
|
||||
<link name="scissor_tcp"/>
|
||||
|
||||
<joint name="scissor_tcp_fixed" type="fixed">
|
||||
<parent link="scissor_link"/>
|
||||
<child link="scissor_tcp"/>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<link name="camera_tcp"/>
|
||||
|
||||
<joint name="camera_tcp_fixed" type="fixed">
|
||||
<parent link="scissor_link"/>
|
||||
<child link="camera_tcp"/>
|
||||
<origin xyz="0.08 0 -0.135" rpy="0 0 1.57"/>
|
||||
</joint>
|
||||
|
||||
|
||||
|
||||
</robot>
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Reference in New Issue
Block a user