diff --git a/.gitignore b/.gitignore index b6e4761..7be1192 100644 --- a/.gitignore +++ b/.gitignore @@ -127,3 +127,6 @@ dmypy.json # Pyre type checker .pyre/ + +# VSCode +.vscode/ diff --git a/controllers.py b/controllers.py new file mode 100644 index 0000000..4d705df --- /dev/null +++ b/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 diff --git a/launch/panda_visualization.launch b/launch/panda_visualization.launch new file mode 100644 index 0000000..dc9e3df --- /dev/null +++ b/launch/panda_visualization.launch @@ -0,0 +1,9 @@ + + + + + + + + + diff --git a/launch/panda_visualization.rviz b/launch/panda_visualization.rviz new file mode 100644 index 0000000..8268569 --- /dev/null +++ b/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: + 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: + 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 diff --git a/requirements.txt b/requirements.txt new file mode 100644 index 0000000..4c14051 --- /dev/null +++ b/requirements.txt @@ -0,0 +1,3 @@ +numpy +scipy +pybullet diff --git a/run_demo.py b/run_demo.py new file mode 100644 index 0000000..4dcb90a --- /dev/null +++ b/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) diff --git a/simulation.py b/simulation.py new file mode 100644 index 0000000..2ffaf9f --- /dev/null +++ b/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) diff --git a/utils.py b/utils.py new file mode 100644 index 0000000..d599456 --- /dev/null +++ b/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)