Small change to the controller

controllers.py

@@ -2,12 +2,12 @@ import numpy as np
 
 
 class CartesianPoseController:
-    def __init__(self, robot, model, rate):
+    def __init__(self, robot, model, x0, rate):
         self.robot = robot
         self.model = model
         self.rate = rate
-        self.x_d = None
-        self.kp = np.ones(6) * 5.0
+        self.x_d = x0
+        self.kp = np.ones(6) * 4.0
 
     def set_target(self, pose):
         self.x_d = pose

run_demo.py

@@ -10,12 +10,14 @@ gui = True
 rospy.init_node("demo")
 
 env = SimPandaEnv(gui)
-env.controller = CartesianPoseController(env.arm, env.model, 60)
 
 q, dq = env.arm.get_state()
 x0 = env.model.pose(q)
+
+env.controller = CartesianPoseController(env.arm, env.model, x0, 60)
 marker = InteractiveMarkerWrapper("target", "panda_link0", x0)
 
 while True:
     env.controller.set_target(marker.pose)
+    env.camera.update()
     env.forward(0.1)

utils.py

@@ -79,9 +79,6 @@ class InteractiveMarkerWrapper(object):
 
 class Transform(object):
     def __init__(self, rotation, translation):
-        assert isinstance(rotation, Rotation)
-        assert isinstance(translation, (np.ndarray, list))
-
         self.rotation = rotation
         self.translation = np.asarray(translation, np.double)