KDL based robot model

controllers.py

@@ -4,16 +4,17 @@ import numpy as np
 class CartesianPoseController:
     def __init__(self, model):
         self.model = model
+        # self.x_d = x0
 
     def set_target(self, pose):
-        self.target = pose.translation
+        self.x_d = pose.translation
 
     def update(self, q, dq):
-        t = self.model.pose().translation
+        t = self.model.pose(q).translation
-        J = self.model.jacobian(q)
+        J = self.model.jacobian(q)[:3, :]
         J_pinv = np.linalg.pinv(J)
 
-        err = 2.0 * (self.target - t)
+        err = 2.0 * (self.x_d - t)
         cmd = np.dot(J_pinv, err)
 
         return cmd

model.py

@@ -0,0 +1,38 @@
+import numpy as np
+import kdl_parser_py.urdf as kdl_parser
+import PyKDL as kdl
+
+import utils
+
+
+class Model(object):
+    def __init__(self, root_frame_id, tip_frame_id):
+        _, tree = kdl_parser.treeFromFile("assets/urdfs/panda/panda_arm_hand.urdf")
+        chain = tree.getChain(root_frame_id, tip_frame_id)
+        self.nr_joints = chain.getNrOfJoints()
+        self.fk_pos_solver = kdl.ChainFkSolverPos_recursive(chain)
+        self.fk_vel_solver = kdl.ChainFkSolverVel_recursive(chain)
+        self.jac_solver = kdl.ChainJntToJacSolver(chain)
+        return
+
+    def pose(self, q):
+        jnt_array = utils.to_kdl_jnt_array(q)
+        frame = kdl.Frame()
+        self.fk_pos_solver.JntToCart(jnt_array, frame)
+        return utils.Transform.from_kdl(frame)
+
+    def velocities(self, q, dq):
+        jnt_array_vel = kdl.JntArrayVel(
+            utils.to_kdl_jnt_array(q), utils.to_kdl_jnt_array(dq)
+        )
+        twist = kdl.FrameVel()
+        self.fk_vel_solver.JntToCart(jnt_array_vel, twist)
+        d = twist.deriv()
+        linear, angular = np.r_[d[0], d[1], d[2]], np.r_[d[3], d[4], d[5]]
+        return linear, angular
+
+    def jacobian(self, q):
+        jnt_array = utils.to_kdl_jnt_array(q)
+        J = kdl.Jacobian(self.nr_joints)
+        self.jac_solver.JntToJac(jnt_array, J)
+        return utils.kdl_to_mat(J)

robot_sim.py

@@ -48,18 +48,6 @@ class PandaArm(object):
                 self.uid, i, p.VELOCITY_CONTROL, targetVelocity=velocity
             )
 
-    def pose(self):
-        result = p.getLinkState(self.uid, self.ee_link_id, computeForwardKinematics=1)
-        _, _, _, _, frame_pos, frame_quat = result
-        T_world_ee = Transform(Rotation.from_quat(frame_quat), np.array(frame_pos))
-        return self.T_world_base.inverse() * T_world_ee
-
-    def jacobian(self, q):
-        q = np.r_[q, 0.0, 0.0].tolist()
-        q_dot, q_ddot = np.zeros(9).tolist(), np.zeros(9).tolist()
-        linear, _ = p.calculateJacobian(self.uid, 7, [0.0, 0.0, 0.0], q, q_dot, q_ddot)
-        return np.asarray(linear)[:, :7]
-
 
 class PandaGripper(object):
     def move(self, width):

run_demo.py

@@ -1,7 +1,8 @@
 import rospy
 
 from controllers import *
-from simulation import *
+from model import *
+from robot_sim import *
 from utils import *
 
 
@@ -12,11 +13,11 @@ dt = 1.0 / 60.0
 rospy.init_node("demo")
 
 env = SimPandaEnv(gui)
-model = env.arm
+model = Model("panda_link0", "panda_link8")
 controller = CartesianPoseController(model)
 
-init_ee_pose = env.arm.pose()
+q, dq = env.arm.get_state()
-marker = InteractiveMarkerWrapper("target", "panda_link0", init_ee_pose)
+marker = InteractiveMarkerWrapper("target", "panda_link0", model.pose(q))
 
 # run the control loop
 while True:
@@ -24,4 +25,5 @@ while True:
     q, dq = env.arm.get_state()
     cmd = controller.update(q, dq)
     env.arm.set_desired_joint_velocities(cmd)
+
     env.forward(dt)

utils.py

@@ -1,5 +1,6 @@
 import numpy as np
 from scipy.spatial.transform import Rotation
+import PyKDL as kdl
 
 from interactive_markers.interactive_marker_server import *
 from interactive_markers.menu_handler import *
@@ -109,3 +110,32 @@ class Transform(object):
         rotation = Rotation.from_quat(list[:4])
         translation = list[4:]
         return cls(rotation, translation)
+
+    @classmethod
+    def from_kdl(cls, f):
+        rotation = Rotation.from_matrix(
+            np.array(
+                [
+                    [f.M[0, 0], f.M[0, 1], f.M[0, 2]],
+                    [f.M[1, 0], f.M[1, 1], f.M[1, 2]],
+                    [f.M[2, 0], f.M[2, 1], f.M[2, 2]],
+                ]
+            )
+        )
+        translation = np.r_[f.p[0], f.p[1], f.p[2]]
+        return cls(rotation, translation)
+
+
+def to_kdl_jnt_array(q):
+    jnt_array = kdl.JntArray(len(q))
+    for i, q_i in enumerate(q):
+        jnt_array[i] = q_i
+    return jnt_array
+
+
+def kdl_to_mat(m):
+    mat = np.zeros((m.rows(), m.columns()))
+    for i in range(m.rows()):
+        for j in range(m.columns()):
+            mat[i, j] = m[i, j]
+    return mat