Simulation & Cartesian position controller

.gitignore

@@ -127,3 +127,6 @@ dmypy.json
 
 # Pyre type checker
 .pyre/
+
+# VSCode
+.vscode/

controllers.py

@@ -0,0 +1,19 @@
+import numpy as np
+
+
+class CartesianPoseController:
+    def __init__(self, model):
+        self.model = model
+
+    def set_target(self, pose):
+        self.target = pose.translation
+
+    def update(self, q, dq):
+        t = self.model.pose().translation
+        J = self.model.jacobian(q)
+        J_pinv = np.linalg.pinv(J)
+
+        err = 2.0 * (self.target - t)
+        cmd = np.dot(J_pinv, err)
+
+        return cmd

launch/panda_visualization.launch

@@ -0,0 +1,9 @@
+<?xml version="1.0" ?>
+<launch>
+  <param name="robot_description" command="$(find xacro)/xacro --inorder $(find franka_description)/robots/panda_arm.urdf.xacro" />
+
+  <node pkg="tf2_ros" type="static_transform_publisher" name="T_world_robot" args="0.03226862 -0.06137175  0.04107702 0.00099995 0. 0.39898185 0.91695828 panda_link8 camera_depth_optical_frame" />
+
+  <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
+  <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d /home/michel/catkin_ws/src/active_grasp/launch/panda_visualization.rviz" />
+</launch>

launch/panda_visualization.rviz

@@ -0,0 +1,189 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+      Splitter Ratio: 0.5
+    Tree Height: 549
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        panda_link0:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link7:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link8:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz/InteractiveMarkers
+      Enable Transparency: true
+      Enabled: true
+      Name: Target
+      Show Axes: false
+      Show Descriptions: true
+      Show Visual Aids: false
+      Update Topic: /target/update
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: panda_link0
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 1.841537594795227
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: 0.06870806962251663
+        Y: 0.16815021634101868
+        Z: 0.3779522478580475
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.29539811611175537
+      Target Frame: <Fixed Frame>
+      Yaw: 5.128592014312744
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 846
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: true
+  Width: 1200
+  X: 646
+  Y: 193

requirements.txt

@@ -0,0 +1,3 @@
+numpy
+scipy
+pybullet

run_demo.py

@@ -0,0 +1,27 @@
+import rospy
+
+from controllers import *
+from simulation import *
+from utils import *
+
+
+# parameters
+gui = True
+dt = 1.0 / 60.0
+
+rospy.init_node("demo")
+
+env = SimPandaEnv(gui)
+model = env.arm
+controller = CartesianPoseController(model)
+
+init_ee_pose = env.arm.pose()
+marker = InteractiveMarkerWrapper("target", "panda_link0", init_ee_pose)
+
+# run the control loop
+while True:
+    controller.set_target(marker.get_pose())
+    q, dq = env.arm.get_state()
+    cmd = controller.update(q, dq)
+    env.arm.set_desired_joint_velocities(cmd)
+    env.forward(dt)

simulation.py

@@ -0,0 +1,127 @@
+import time
+
+import numpy as np
+import pybullet as p
+import pybullet_data
+
+from sensor_msgs.msg import JointState
+
+from utils import *
+
+
+class PandaArm(object):
+    def __init__(self):
+        self.num_dof = 7
+        self.ee_link_id = 7
+        self.ee_frame_id = "panda_link7"
+
+        self.named_joint_values = {"ready": [0.0, -0.79, 0.0, -2.356, 0.0, 1.57, 0.79]}
+
+        self.upper_limits = [-7] * self.num_dof
+        self.lower_limits = [7] * self.num_dof
+        self.ranges = [7] * self.num_dof
+
+    def load(self, pose):
+        self.T_world_base = pose
+        self.uid = p.loadURDF(
+            "assets/urdfs/panda/panda_arm_hand.urdf",
+            basePosition=pose.translation,
+            baseOrientation=pose.rotation.as_quat(),
+            useFixedBase=True,
+        )
+        for i in range(self.num_dof):
+            p.resetJointState(self.uid, i, self.named_joint_values["ready"][i])
+
+    def get_state(self):
+        joint_states = p.getJointStates(self.uid, range(p.getNumJoints(self.uid)))[:7]
+        positions = [state[0] for state in joint_states]
+        velocities = [state[1] for state in joint_states]
+        return positions, velocities
+
+    def set_desired_joint_positions(self, positions):
+        for i, position in enumerate(positions):
+            p.setJointMotorControl2(self.uid, i, p.POSITION_CONTROL, position)
+
+    def set_desired_joint_velocities(self, velocities):
+        for i, velocity in enumerate(velocities):
+            p.setJointMotorControl2(
+                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):
+        for i in [9, 10]:
+            p.setJointMotorControl2(
+                self.uid,
+                i,
+                p.POSITION_CONTROL,
+                0.5 * width,
+                force=10,
+            )
+
+    def read(self):
+        return p.getJointState(self.uid, 9)[0] + p.getJointState(self.uid, 10)[0]
+
+
+class Camera(object):
+    pass
+
+
+class SimPandaEnv(object):
+    def __init__(self, gui, dt=1.0 / 240.0, publish_state=True):
+        self.gui = gui
+        self.dt = dt
+        p.connect(p.GUI if gui else p.DIRECT)
+        p.setAdditionalSearchPath(pybullet_data.getDataPath())
+        p.setTimeStep(dt)
+        p.setGravity(0.0, 0.0, -9.8)
+
+        p.loadURDF("plane.urdf")
+        p.loadURDF(
+            "table/table.urdf",
+            baseOrientation=Rotation.from_rotvec(np.array([0, 0, np.pi / 2])).as_quat(),
+            useFixedBase=True,
+        )
+        p.loadURDF("cube_small.urdf", [0.0, 0.0, 0.8])
+
+        self.arm = PandaArm()
+        self.arm.load(Transform(Rotation.identity(), np.r_[-0.8, 0.0, 0.4]))
+        self.gripper = PandaGripper()
+        self.gripper.uid = self.arm.uid
+
+        self.camera = None
+
+        if publish_state:
+            self.state_pub = rospy.Publisher("/joint_states", JointState, queue_size=10)
+            rospy.Timer(rospy.Duration(1.0 / 30), self._publish_state)
+
+    def forward(self, dt):
+        steps = int(dt / self.dt)
+        for _ in range(steps):
+            self.step()
+
+    def step(self):
+        p.stepSimulation()
+        time.sleep(self.dt)
+
+    def _publish_state(self, event):
+        positions, velocities = self.arm.get_state()
+        msg = JointState()
+        msg.header.stamp = rospy.Time.now()
+        msg.name = ["panda_joint{}".format(i) for i in range(1, 8)]
+        msg.position = positions
+        msg.velocity = velocities
+        self.state_pub.publish(msg)

utils.py

@@ -0,0 +1,111 @@
+import numpy as np
+from scipy.spatial.transform import Rotation
+
+from interactive_markers.interactive_marker_server import *
+from interactive_markers.menu_handler import *
+from visualization_msgs.msg import *
+
+
+class InteractiveMarkerWrapper(object):
+    def __init__(self, name, frame_id, pose):
+        server = InteractiveMarkerServer(topic_ns=name)
+
+        target_marker = InteractiveMarker()
+        target_marker.header.frame_id = frame_id
+        target_marker.name = name
+        target_marker.scale = 0.2
+        target_marker.pose.position.x = pose.translation[0]
+        target_marker.pose.position.y = pose.translation[1]
+        target_marker.pose.position.z = pose.translation[2]
+
+        marker = Marker()
+        marker.type = Marker.SPHERE
+        marker.scale.x = 0.05
+        marker.scale.y = 0.05
+        marker.scale.z = 0.05
+        marker.color.r = 0.0
+        marker.color.g = 0.5
+        marker.color.b = 0.5
+        marker.color.a = 0.6
+        ctrl = InteractiveMarkerControl()
+        ctrl.always_visible = True
+        ctrl.markers.append(marker)
+        target_marker.controls.append(ctrl)
+
+        ctrl = InteractiveMarkerControl()
+        ctrl.name = "move_x"
+        ctrl.orientation.w = 1.0
+        ctrl.orientation.x = 1.0
+        ctrl.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
+        target_marker.controls.append(ctrl)
+
+        ctrl = InteractiveMarkerControl()
+        ctrl.name = "move_y"
+        ctrl.orientation.w = 1.0
+        ctrl.orientation.y = 1.0
+        ctrl.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
+        target_marker.controls.append(ctrl)
+
+        ctrl = InteractiveMarkerControl()
+        ctrl.name = "move_z"
+        ctrl.orientation.w = 1.0
+        ctrl.orientation.z = 1.0
+        ctrl.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
+        target_marker.controls.append(ctrl)
+
+        server.insert(target_marker, self.cb)
+        server.applyChanges()
+
+        self.pose = pose
+
+    def cb(self, feedback):
+        pos = feedback.pose.position
+        self.pose = Transform(Rotation.identity(), np.r_[pos.x, pos.y, pos.z])
+
+    def get_pose(self):
+        return self.pose
+
+
+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)
+
+    def as_matrix(self):
+        return np.vstack(
+            (np.c_[self.rotation.as_matrix(), self.translation], [0.0, 0.0, 0.0, 1.0])
+        )
+
+    def to_list(self):
+        return np.r_[self.rotation.as_quat(), self.translation]
+
+    def __mul__(self, other):
+        rotation = self.rotation * other.rotation
+        translation = self.rotation.apply(other.translation) + self.translation
+        return self.__class__(rotation, translation)
+
+    def inverse(self):
+        rotation = self.rotation.inv()
+        translation = -rotation.apply(self.translation)
+        return self.__class__(rotation, translation)
+
+    @classmethod
+    def identity(cls):
+        rotation = Rotation.from_quat([0.0, 0.0, 0.0, 1.0])
+        translation = np.array([0.0, 0.0, 0.0])
+        return cls(rotation, translation)
+
+    @classmethod
+    def from_matrix(cls, m):
+        rotation = Rotation.from_matrix(m[:3, :3])
+        translation = m[:3, 3]
+        return cls(rotation, translation)
+
+    @classmethod
+    def from_list(cls, list):
+        rotation = Rotation.from_quat(list[:4])
+        translation = list[4:]
+        return cls(rotation, translation)