From 88ba0b959a074202ebc07ea016503e0e9895b5aa Mon Sep 17 00:00:00 2001
From: Xinsong Lin <zhongjiezhex12@me.com>
Date: Tue, 12 Mar 2024 17:23:14 -0700
Subject: [PATCH 1/3] Update planner.py

planner init bugs
---
 mplib/planner.py | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/mplib/planner.py b/mplib/planner.py
index f44fbe15..91bc66ca 100644
--- a/mplib/planner.py
+++ b/mplib/planner.py
@@ -68,7 +68,7 @@ def __init__(
             [0, 0, -9.81],
             user_link_names,
             user_joint_names,
-            convex=True,
+            convex=kwargs.get("convex", False),
             verbose=False,
         )
         self.pinocchio_model = self.robot.get_pinocchio_model()
@@ -82,10 +82,6 @@ def __init__(
             kwargs.get("normal_object_names", []),
         )
 
-        if srdf == "":
-            self.generate_collision_pair()
-            self.robot.update_SRDF(self.srdf)
-
         self.joint_name_2_idx = {}
         for i, joint in enumerate(self.user_joint_names):
             self.joint_name_2_idx[joint] = i
@@ -93,6 +89,10 @@ def __init__(
         for i, link in enumerate(self.user_link_names):
             self.link_name_2_idx[link] = i
 
+        if srdf == "":
+            self.generate_collision_pair()
+            self.robot.update_SRDF(self.srdf)
+        
         assert (
             move_group in self.user_link_names
         ), f"end-effector not found as one of the links in {self.user_link_names}"

From 2760cdc0ff388e8f2ad6d98301356d84bd46796c Mon Sep 17 00:00:00 2001
From: Xinsong Lin <zhongjiezhex12@me.com>
Date: Tue, 12 Mar 2024 17:39:31 -0700
Subject: [PATCH 2/3] more bugs -_-

---
 mplib/planner.py | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/mplib/planner.py b/mplib/planner.py
index 91bc66ca..3b1e3d49 100644
--- a/mplib/planner.py
+++ b/mplib/planner.py
@@ -63,8 +63,8 @@ def __init__(
         urdf = self.replace_package_keyword(package_keyword_replacement)
 
         self.robot = ArticulatedModel(
-            urdf,
-            srdf,
+            self.urdf,
+            self.srdf,
             [0, 0, -9.81],
             user_link_names,
             user_joint_names,
@@ -674,6 +674,8 @@ def plan_qpos_to_pose(
                 if current_qpos[i] > self.joint_limits[i][1]:
                     current_qpos[i] = self.joint_limits[i][1] - 1e-3
 
+        current_qpos = self.pad_qpos(current_qpos)
+        
         if wrt_world:
             goal_pose = self.transform_goal_to_wrt_base(goal_pose)
 

From 012de618d4f3dc700baa0172fe238853cb8b3b72 Mon Sep 17 00:00:00 2001
From: Xinsong Lin <zhongjiezhex12@me.com>
Date: Tue, 12 Mar 2024 17:52:04 -0700
Subject: [PATCH 3/3] change all srdf to self.srdf

---
 mplib/planner.py | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/mplib/planner.py b/mplib/planner.py
index 3b1e3d49..d54aaecd 100644
--- a/mplib/planner.py
+++ b/mplib/planner.py
@@ -55,7 +55,8 @@ def __init__(
         if joint_acc_limits is None:
             joint_acc_limits = []
         self.urdf = urdf
-        if srdf == "" and os.path.exists(urdf.replace(".urdf", ".srdf")):
+        self.srdf = srdf
+        if self.srdf == "" and os.path.exists(urdf.replace(".urdf", ".srdf")):
             self.srdf = urdf.replace(".urdf", ".srdf")
             print(f"No SRDF file provided but found {self.srdf}")
 
@@ -89,7 +90,7 @@ def __init__(
         for i, link in enumerate(self.user_link_names):
             self.link_name_2_idx[link] = i
 
-        if srdf == "":
+        if self.srdf == "":
             self.generate_collision_pair()
             self.robot.update_SRDF(self.srdf)