Compare commits
3
Commits
5cac8c5a73
...
ceb80a8b17
| Author | SHA1 | Date | |
|---|---|---|---|
 LiuzhengSJ
|
ceb80a8b17 | ||
|
LiuzhengSJ
|
d1080638c1 | ||
|
LiuzhengSJ
|
d0ca4d6115 |
+6
-3
@@ -100,9 +100,12 @@ def main():
|
|||||||
if d_p_ik < 0.01:
|
if d_p_ik < 0.01:
|
||||||
result[0][1] += 1
|
result[0][1] += 1
|
||||||
|
|
||||||
# robot_mjk.send_command(q)
|
while 1:
|
||||||
# robot_mjk.wait_until_reached()
|
time.sleep(1)
|
||||||
# robot_mjk.print_state()
|
|
||||||
|
robot_mjk.send_command(q)
|
||||||
|
robot_mjk.wait_until_reached()
|
||||||
|
robot_mjk.print_state()
|
||||||
else:
|
else:
|
||||||
fk_qp_p2 = robot_kine_qp.forward_kinematics(q, tool=tool_name)
|
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)
|
d_p_ik = cal_pose_deviation(pose1=t_p, pose2=fk_qp_p2)
|
||||||
|
|||||||
@@ -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
|
# 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)
|
csv_path = Path(file_name)
|
||||||
|
|
||||||
# The file has no column names, so header=None is important.
|
# The file has no column names, so header=None is important.
|
||||||
@@ -16,13 +16,21 @@ df_csv = pd.read_csv(
|
|||||||
csv_path,
|
csv_path,
|
||||||
)
|
)
|
||||||
rate_res = df_csv.iloc[:, :4]
|
rate_res = df_csv.iloc[:, :4]
|
||||||
|
try:
|
||||||
rate_res_sort = rate_res.sort_values('z').reset_index(drop=True)
|
rate_res_sort = rate_res.sort_values('z').reset_index(drop=True)
|
||||||
|
except:
|
||||||
|
rate_res_sort = rate_res
|
||||||
|
|
||||||
DECIMALS = 4
|
DECIMALS = 4
|
||||||
|
|
||||||
|
try:
|
||||||
x_unique = np.round(rate_res_sort['x'], DECIMALS).unique()
|
x_unique = np.round(rate_res_sort['x'], DECIMALS).unique()
|
||||||
y_unique = np.round(rate_res_sort['y'], DECIMALS).unique()
|
y_unique = np.round(rate_res_sort['y'], DECIMALS).unique()
|
||||||
z_unique = np.round(rate_res_sort['z'], 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)
|
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
|
num_orientations = 120
|
||||||
|
|
||||||
|
tool_name = "v_minis"
|
||||||
|
|
||||||
|
output_csv = "rm75b_comfort_workspace" + tool_name + ".csv"
|
||||||
|
|
||||||
|
|
||||||
# Comfort thresholds
|
# Comfort thresholds
|
||||||
MIN_JOINT_MARGIN = 0.05 # 15% away from joint limits
|
MIN_JOINT_MARGIN = 0.05 # 15% away from joint limits
|
||||||
MAX_CONDITION_NUMBER = 150.0
|
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
|
ub = np.array([179.0, 129.0, 179.0, 134, 179.0, 127.0, 359.0])/180*pi
|
||||||
lb = -ub
|
lb = -ub
|
||||||
|
|
||||||
tool_name = "v_minis"
|
|
||||||
|
|
||||||
URDF_PATH = str(parent_dir) + '/urdf_rm75/RM75-B.urdf'
|
URDF_PATH = str(parent_dir) + '/urdf_rm75/RM75-B.urdf'
|
||||||
MESH_DIR = str(Path(URDF_PATH).parent)
|
MESH_DIR = str(Path(URDF_PATH).parent)
|
||||||
@@ -733,7 +737,6 @@ def plot_comfortable_only(df):
|
|||||||
if __name__ == "__main__":
|
if __name__ == "__main__":
|
||||||
df = evaluate_workspace()
|
df = evaluate_workspace()
|
||||||
|
|
||||||
output_csv = "rm75b_comfort_workspace_v_minis_collision.csv"
|
|
||||||
df.to_csv(output_csv, index=False)
|
df.to_csv(output_csv, index=False)
|
||||||
|
|
||||||
print(f"\nSaved result to: {output_csv}")
|
print(f"\nSaved result to: {output_csv}")
|
||||||
|
|||||||
Reference in New Issue
Block a user