Compare commits

3 Commits
Author SHA1 Message Date
LiuzhengSJ ceb80a8b17 the scissor stl file is added. 2026-07-27 20:34:02 +01:00
LiuzhengSJ d1080638c1 Merge remote-tracking branch 'origin/class_version' into class_version
# Conflicts:
#	kine_ctrl/workspace_comfortable/workspace_cal.py
2026-07-27 15:15:11 +01:00
LiuzhengSJ d0ca4d6115 update teh contour plot method 2026-07-24 10:46:09 +01:00
8 changed files with 630 additions and 11 deletions
+6 -3
View File
@@ -100,9 +100,12 @@ def main():
if d_p_ik < 0.01:
result[0][1] += 1
# robot_mjk.send_command(q)
# robot_mjk.wait_until_reached()
# robot_mjk.print_state()
while 1:
time.sleep(1)
robot_mjk.send_command(q)
robot_mjk.wait_until_reached()
robot_mjk.print_state()
else:
fk_qp_p2 = robot_kine_qp.forward_kinematics(q, tool=tool_name)
d_p_ik = cal_pose_deviation(pose1=t_p, pose2=fk_qp_p2)
+496
View File
@@ -0,0 +1,496 @@
<?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/sci.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/sci.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" rpy="3.14159 0 0" />
</joint>
</robot>
Binary file not shown.
@@ -8,7 +8,7 @@ import pandas as pd
# --------------------------------------------------
# 1. Load the data
# --------------------------------------------------
file_name = "rm75b_comfort_workspace_collision_minisci.csv"
file_name = "rm75b_comfort_workspace_v_minis_collision.csv"
csv_path = Path(file_name)
# The file has no column names, so header=None is important.
@@ -16,13 +16,21 @@ df_csv = pd.read_csv(
csv_path,
)
rate_res = df_csv.iloc[:, :4]
rate_res_sort = rate_res.sort_values('z').reset_index(drop=True)
try:
rate_res_sort = rate_res.sort_values('z').reset_index(drop=True)
except:
rate_res_sort = rate_res
DECIMALS = 4
x_unique = np.round(rate_res_sort['x'], DECIMALS).unique()
y_unique = np.round(rate_res_sort['y'], DECIMALS).unique()
z_unique = np.round(rate_res_sort['z'], DECIMALS).unique()
try:
x_unique = np.round(rate_res_sort['x'], DECIMALS).unique()
y_unique = np.round(rate_res_sort['y'], DECIMALS).unique()
z_unique = np.round(rate_res_sort['z'], DECIMALS).unique()
except:
x_unique = np.round(rate_res_sort.iloc[:,0], DECIMALS).unique()
y_unique = np.round(rate_res_sort.iloc[:, 1], DECIMALS).unique()
z_unique = np.round(rate_res_sort.iloc[:, 2], DECIMALS).unique()
nx, ny, nz = len(x_unique), len(y_unique), len(z_unique)
@@ -0,0 +1,109 @@
from pathlib import Path
import matplotlib.pyplot as plt
import numpy as np
import pandas as pd
# --------------------------------------------------
# 1. Load the data
# --------------------------------------------------
file_name = "workspace minisci collision.csv"
csv_path = Path(file_name)
# The file has no column names, so header=None is important.
df_csv = pd.read_csv(
csv_path,
header=None, # the file has no header row
names=['x', 'y', 'z', 'ik_success_rate'] # assign names
)
rate_res = df_csv.iloc[:, :4]
try:
rate_res_sort = rate_res.sort_values('z').reset_index(drop=True)
except:
rate_res_sort = rate_res
DECIMALS = 4
try:
x_unique = np.round(rate_res_sort['x'], DECIMALS).unique()
y_unique = np.round(rate_res_sort['y'], DECIMALS).unique()
z_unique = np.round(rate_res_sort['z'], DECIMALS).unique()
except:
x_unique = np.round(rate_res_sort.iloc[:,0], DECIMALS).unique()
y_unique = np.round(rate_res_sort.iloc[:, 1], DECIMALS).unique()
z_unique = np.round(rate_res_sort.iloc[:, 2], DECIMALS).unique()
nx, ny, nz = len(x_unique), len(y_unique), len(z_unique)
ik_rates = rate_res_sort.to_numpy()
# --------------------------------------------------
# 2. Create an output directory
# --------------------------------------------------
output_dir = Path(file_name.split(".")[0])
output_dir.mkdir(exist_ok=True)
# --------------------------------------------------
# 3. Use the same colour scale for every z-plane
# --------------------------------------------------
df = rate_res_sort
value_min = df["ik_success_rate"].min()
value_max = df["ik_success_rate"].max()
# More levels give a smoother-looking contour plot.
levels = np.linspace(value_min, value_max, 51)
# --------------------------------------------------
# 4. Draw one contour plot for each z-plane
# --------------------------------------------------
for z_value, plane in df.groupby("z", sort=True):
# Rows become y-coordinates, columns become x-coordinates.
grid = plane.pivot(index="y", columns="x", values="ik_success_rate")
x = grid.columns.to_numpy()
y = grid.index.to_numpy()
ik_grid = grid.to_numpy()
X, Y = np.meshgrid(x, y)
fig, ax = plt.subplots(figsize=(7, 6))
contour = ax.contourf(
X,
Y,
ik_grid,
levels=levels,
cmap="viridis",
extend="both",
)
highlight_levels = [0.6, 0.7]
# Only plot if the levels are within the data range (optional)
if value_min <= 0.6 <= value_max or value_min <= 0.7 <= value_max:
lines = ax.contour(X, Y, ik_grid, levels=highlight_levels,
colors='red', linewidths=2, linestyles='solid')
# Optionally label the lines
ax.clabel(lines, inline=True, fontsize=10, fmt='%1.1f')
colorbar = fig.colorbar(contour, ax=ax)
colorbar.set_label("IK rate")
ax.set_title(f"IK rate at z = {z_value:.2f}")
ax.set_xlabel("x")
ax.set_ylabel("y")
ax.set_aspect("equal")
fig.tight_layout()
output_path = output_dir / f"ik_contour_z_{z_value:.2f}.png"
fig.savefig(output_path, dpi=200, bbox_inches="tight")
plt.close(fig)
print(f"Plots saved to: {output_dir.resolve()}")
Binary file not shown.

Before

Width:  |  Height:  |  Size: 138 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 138 KiB

@@ -57,6 +57,11 @@ GRID_RESOLUTION = 0.05 # 5 cm. Use 0.02 for finer but slower.
num_orientations = 120
tool_name = "v_minis"
output_csv = "rm75b_comfort_workspace" + tool_name + ".csv"
# Comfort thresholds
MIN_JOINT_MARGIN = 0.05 # 15% away from joint limits
MAX_CONDITION_NUMBER = 150.0
@@ -89,7 +94,6 @@ tools_in_ee = {
ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])/180*pi
lb = -ub
tool_name = "v_minis"
URDF_PATH = str(parent_dir) + '/urdf_rm75/RM75-B.urdf'
MESH_DIR = str(Path(URDF_PATH).parent)
@@ -733,7 +737,6 @@ def plot_comfortable_only(df):
if __name__ == "__main__":
df = evaluate_workspace()
output_csv = "rm75b_comfort_workspace_v_minis_collision.csv"
df.to_csv(output_csv, index=False)
print(f"\nSaved result to: {output_csv}")