diff --git a/CMakeLists.txt b/CMakeLists.txt index 0099564..08d1a45 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,4 @@ cmake_minimum_required(VERSION 3.1) -project(vgn) +project(active_grasp) find_package(catkin REQUIRED) diff --git a/controllers.py b/controllers.py deleted file mode 100644 index 18de546..0000000 --- a/controllers.py +++ /dev/null @@ -1,29 +0,0 @@ -import numpy as np - - -class CartesianPoseController: - def __init__(self, robot, model, x0, rate): - self.robot = robot - self.model = model - self.rate = rate - self.x_d = x0 - self.kp = np.ones(6) * 4.0 - - def set_target(self, pose): - self.x_d = pose - - def update(self): - q, _ = self.robot.get_state() - - x = self.model.pose(q) - x_d = self.x_d - - err = np.zeros(6) - err[:3] = x_d.translation - x.translation - err[3:] = (x_d.rotation * x.rotation.inv()).as_rotvec() - - J = self.model.jacobian(q) - J_pinv = np.linalg.pinv(J) - cmd = np.dot(J_pinv, self.kp * err) - - self.robot.set_desired_joint_velocities(cmd) diff --git a/launch/panda_visualization.launch b/launch/panda_visualization.launch index 74d7957..d694dc6 100644 --- a/launch/panda_visualization.launch +++ b/launch/panda_visualization.launch @@ -1,7 +1,6 @@ - diff --git a/launch/panda_visualization.rviz b/launch/panda_visualization.rviz index 5c6909c..29fdccd 100644 --- a/launch/panda_visualization.rviz +++ b/launch/panda_visualization.rviz @@ -5,8 +5,6 @@ Panels: Property Tree Widget: Expanded: - /Global Options1 - - /RobotModel1/Status1 - - /TF1/Frames1 Splitter Ratio: 0.5 Tree Height: 549 - Class: rviz/Selection @@ -130,10 +128,34 @@ Visualization Manager: Value: true Visual Enabled: true - Class: rviz/TF - Enabled: false + Enabled: true Frame Timeout: 15 Frames: All Enabled: false + panda_hand: + Value: true + panda_leftfinger: + Value: false + panda_link0: + Value: true + panda_link1: + Value: false + panda_link2: + Value: false + panda_link3: + Value: false + panda_link4: + Value: false + panda_link5: + Value: false + panda_link6: + Value: false + panda_link7: + Value: false + panda_link8: + Value: false + panda_rightfinger: + Value: false Marker Alpha: 1 Marker Scale: 0.5 Name: TF @@ -141,17 +163,21 @@ Visualization Manager: Show Axes: true Show Names: true Tree: - {} + panda_link0: + panda_link1: + panda_link2: + panda_link3: + panda_link4: + panda_link5: + panda_link6: + panda_link7: + panda_link8: + panda_hand: + panda_leftfinger: + {} + panda_rightfinger: + {} Update Interval: 0 - Value: false - - 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: @@ -181,7 +207,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 1.2702323198318481 + Distance: 1.3619381189346313 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -189,17 +215,17 @@ Visualization Manager: Value: false Field of View: 0.7853981852531433 Focal Point: - X: 0.06870806962251663 - Y: 0.16815021634101868 - Z: 0.3779522478580475 + X: 0.14782209694385529 + Y: 0.10340728610754013 + Z: 0.3605891168117523 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.38539746403694153 + Pitch: 0.21979668736457825 Target Frame: - Yaw: 5.683584213256836 + Yaw: 5.333564758300781 Saved: ~ Window Geometry: Displays: @@ -217,5 +243,5 @@ Window Geometry: Views: collapsed: true Width: 1200 - X: 1045 - Y: 249 + X: 659 + Y: 27 diff --git a/model.py b/model.py deleted file mode 100644 index 283f560..0000000 --- a/model.py +++ /dev/null @@ -1,38 +0,0 @@ -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) diff --git a/policies.py b/policies.py new file mode 100644 index 0000000..d4e441b --- /dev/null +++ b/policies.py @@ -0,0 +1,46 @@ +from geometry_msgs.msg import Pose +import numpy as np +import rospy +import scipy.interpolate + +from robot_tools.ros import * + + +def get_policy(name): + if name == "fixed": + return FixedTrajectory() + + +class BasePolicy: + def __init__(self): + self.tf_tree = TransformTree() + self.target_pose_pub = rospy.Publisher("/target", Pose, queue_size=10) + rospy.sleep(1.0) + + +class FixedTrajectory(BasePolicy): + def __init__(self): + super().__init__() + self.duration = 4.0 + self.radius = 0.1 + self.m = scipy.interpolate.interp1d([0, self.duration], [np.pi, 3.0 * np.pi]) + + def start(self): + self.tic = rospy.Time.now() + timeout = rospy.Duration(0.1) + x0 = self.tf_tree.lookup("panda_link0", "panda_hand", self.tic, timeout) + self.origin = np.r_[x0.translation[0] + self.radius, x0.translation[1:]] + self.target = x0 + + def update(self): + elapsed_time = (rospy.Time.now() - self.tic).to_sec() + + if elapsed_time > self.duration: + return True + + t = self.m(elapsed_time) + self.target.translation = ( + self.origin + np.r_[self.radius * np.cos(t), self.radius * np.sin(t), 0.0] + ) + self.target_pose_pub.publish(to_pose_msg(self.target)) + return False diff --git a/requirements.txt b/requirements.txt index 4c14051..e69de29 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,3 +0,0 @@ -numpy -scipy -pybullet diff --git a/robot_sim.py b/robot_sim.py deleted file mode 100644 index b522f81..0000000 --- a/robot_sim.py +++ /dev/null @@ -1,161 +0,0 @@ -import time - -import numpy as np -import pybullet as p -import pybullet_data - -from sensor_msgs.msg import JointState - -from model import * -from utils import * - - -class PandaArm: - def __init__(self): - self.nr_dof = 7 - - self.named_joint_values = {"ready": [0.0, -0.79, 0.0, -2.356, 0.0, 1.57, 0.79]} - - self.upper_limits = [-7] * self.nr_dof - self.lower_limits = [7] * self.nr_dof - self.ranges = [7] * self.nr_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, q_i in enumerate(self.named_joint_values["ready"]): - p.resetJointState(self.uid, i, q_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 - ) - - -class PandaGripper: - 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: - def __init__(self, uid, link_id, width, height, hfov, near, far): - self.uid = uid - self.link_id = link_id - - f = width / (2.0 * np.tan(hfov / 2.0)) - cx, cy = width / 2.0, height / 2.0 - - self.width = width - self.height = height - self.K = [f, 0.0, cx, 0.0, f, cy, 0.0, 0.0, 1.0] - self.near = near - self.far = far - - self.proj_mat = p.computeProjectionMatrixFOV( - np.rad2deg(hfov), width / height, near, far - ) - - def update(self): - result = p.getLinkState(self.uid, self.link_id, computeForwardKinematics=1) - t_world_cam = result[4] - R_world_cam = Rotation.from_quat(result[5]) - eye = t_world_cam - target = R_world_cam.apply(np.r_[0.0, 0.0, 1.0]) + t_world_cam - up = R_world_cam.apply(np.r_[0.0, -1.0, 0.0]) - view_mat = p.computeViewMatrix(eye, target, up) - - result = p.getCameraImage( - self.width, - self.height, - view_mat, - self.proj_mat, - renderer=p.ER_BULLET_HARDWARE_OPENGL, - ) - color_img, z_buffer = result[2][:, :, :3], result[3] - - color_img = color_img.astype(np.uint8) - depth_img = ( - self.far * self.near / (self.far - (self.far - self.near) * z_buffer) - ).astype(np.float32) - - return color_img, depth_img - - -class SimPandaEnv(object): - def __init__(self, gui, publish_state=True): - self.gui = gui - self.rate = 240 - p.connect(p.GUI if gui else p.DIRECT) - p.setAdditionalSearchPath(pybullet_data.getDataPath()) - p.setTimeStep(1.0 / self.rate) - p.setGravity(0.0, 0.0, -9.8) - self.step_cnt = 0 - - 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.model = Model("panda_link0", "panda_ee") - self.camera = Camera(self.arm.uid, 12, 320, 240, 1.047, 0.2, 2.0) - - 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.rate) - for _ in range(steps): - self._step() - - def _step(self): - if self.step_cnt % int(self.rate / self.controller.rate) == 0: - self.controller.update() - p.stepSimulation() - time.sleep(1.0 / self.rate) - - def _publish_state(self, event): - positions, velocities = self.arm.get_state() - width = self.gripper.read() - msg = JointState() - msg.header.stamp = rospy.Time.now() - msg.name = ["panda_joint{}".format(i) for i in range(1, 8)] + [ - "panda_finger_joint1", - "panda_finger_joint2", - ] - msg.position = np.r_[positions, 0.5 * width, 0.5 * width] - msg.velocity = velocities - self.state_pub.publish(msg) diff --git a/run.py b/run.py new file mode 100644 index 0000000..ce44641 --- /dev/null +++ b/run.py @@ -0,0 +1,31 @@ +import argparse + +from geometry_msgs.msg import Pose +import numpy as np +import rospy +from std_srvs.srv import Trigger + +from policies import get_policy + + +def main(args): + rospy.init_node("panda_grasp") + + policy = get_policy(args.policy) + + r = rospy.Rate(args.rate) + done = False + policy.start() + while not done: + done = policy.update() + r.sleep() + + # TODO execute grasp + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("--policy", type=str, choices=["fixed"]) + parser.add_argument("--rate", type=int, default=10) + args = parser.parse_args() + main(args) diff --git a/run_demo.py b/run_demo.py deleted file mode 100644 index 0d890d3..0000000 --- a/run_demo.py +++ /dev/null @@ -1,23 +0,0 @@ -import rospy - -from controllers import * -from robot_sim import * -from utils import * - - -gui = True - -rospy.init_node("demo") - -env = SimPandaEnv(gui) - -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) diff --git a/sim.py b/sim.py new file mode 100644 index 0000000..150ee89 --- /dev/null +++ b/sim.py @@ -0,0 +1,69 @@ +import argparse + +import numpy as np +import rospy + +from geometry_msgs.msg import Pose +from sensor_msgs.msg import JointState + +from robot_tools.btsim import BtPandaEnv +from robot_tools.controllers import CartesianPoseController +from robot_tools.ros import * + +CONTROLLER_UPDATE_RATE = 60 +JOINT_STATE_PUBLISHER_RATE = 60 + + +class BtSimNode: + def __init__(self, gui): + self.sim = BtPandaEnv(gui=gui, sleep=False) + self.controller = CartesianPoseController(self.sim.arm) + + self.joint_state_pub = rospy.Publisher( + "/joint_states", JointState, queue_size=10 + ) + rospy.Subscriber("/target", Pose, self._target_pose_cb) + + def run(self): + rate = rospy.Rate(self.sim.rate) + self.step_cnt = 0 + while not rospy.is_shutdown(): + self._handle_updates() + self.sim.step() + self.step_cnt = (self.step_cnt + 1) % self.sim.rate + rate.sleep() + + def _handle_updates(self): + if self.step_cnt % int(self.sim.rate / CONTROLLER_UPDATE_RATE) == 0: + self.controller.update() + if self.step_cnt % int(self.sim.rate / JOINT_STATE_PUBLISHER_RATE) == 0: + self._publish_joint_state() + + def _publish_joint_state(self): + q, dq = self.sim.arm.get_state() + width = self.sim.gripper.read() + msg = JointState() + msg.header.stamp = rospy.Time.now() + msg.name = ["panda_joint{}".format(i) for i in range(1, 8)] + [ + "panda_finger_joint1", + "panda_finger_joint2", + ] + msg.position = np.r_[q, 0.5 * width, 0.5 * width] + msg.velocity = dq + self.joint_state_pub.publish(msg) + + def _target_pose_cb(self, msg): + self.controller.set_target(from_pose_msg(msg)) + + +def main(args): + rospy.init_node("bt_sim") + server = BtSimNode(args.gui) + server.run() + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("--gui", type=str, default=True) + args = parser.parse_args() + main(args) diff --git a/utils.py b/utils.py deleted file mode 100644 index 82b76b9..0000000 --- a/utils.py +++ /dev/null @@ -1,214 +0,0 @@ -import numpy as np -from scipy.spatial.transform import Rotation -import PyKDL as kdl - -import geometry_msgs.msg -from interactive_markers.interactive_marker_server import * -from interactive_markers.menu_handler import * -import std_msgs.msg -from visualization_msgs.msg import * - - -class InteractiveMarkerWrapper(object): - def __init__(self, name, frame_id, x0): - self.pose = x0 - - server = InteractiveMarkerServer(topic_ns=name) - - int_marker = InteractiveMarker() - int_marker.header.frame_id = frame_id - int_marker.name = name - int_marker.scale = 0.2 - int_marker.pose = to_pose_msg(x0) - - # Attach visible sphere - marker = Marker() - marker.type = Marker.SPHERE - marker.scale = to_vector3_msg([0.05, 0.05, 0.05]) - marker.color = to_color_msg([0.0, 0.5, 0.5, 0.6]) - - ctrl = InteractiveMarkerControl() - ctrl.always_visible = True - ctrl.markers.append(marker) - int_marker.controls.append(ctrl) - - # Attach rotation controls - ctrl = InteractiveMarkerControl() - ctrl.name = "rotate_x" - ctrl.orientation = to_quat_msg(Rotation.from_quat([1, 0, 0, 1])) - ctrl.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS - int_marker.controls.append(ctrl) - - ctrl = InteractiveMarkerControl() - ctrl.name = "rotate_y" - ctrl.orientation = to_quat_msg(Rotation.from_quat([0, 1, 0, 1])) - ctrl.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS - int_marker.controls.append(ctrl) - - ctrl = InteractiveMarkerControl() - ctrl.name = "rotate_z" - ctrl.orientation = to_quat_msg(Rotation.from_quat([0, 0, 1, 1])) - ctrl.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS - int_marker.controls.append(ctrl) - - # Attach translation controls - ctrl = InteractiveMarkerControl() - ctrl.name = "move_x" - ctrl.orientation = to_quat_msg(Rotation.from_quat([1, 0, 0, 1])) - ctrl.interaction_mode = InteractiveMarkerControl.MOVE_AXIS - int_marker.controls.append(ctrl) - - ctrl = InteractiveMarkerControl() - ctrl.name = "move_y" - ctrl.orientation = to_quat_msg(Rotation.from_quat([0, 1, 0, 1])) - ctrl.interaction_mode = InteractiveMarkerControl.MOVE_AXIS - int_marker.controls.append(ctrl) - - ctrl = InteractiveMarkerControl() - ctrl.name = "move_z" - ctrl.orientation = to_quat_msg(Rotation.from_quat([0, 0, 1, 1])) - ctrl.interaction_mode = InteractiveMarkerControl.MOVE_AXIS - int_marker.controls.append(ctrl) - - server.insert(int_marker, self.cb) - server.applyChanges() - - def cb(self, feedback): - self.pose = from_pose_msg(feedback.pose) - - -class Transform(object): - def __init__(self, rotation, translation): - 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) - - @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) - - -# KDL Conversions - - -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 - - -# ROS Conversions - - -def to_color_msg(color): - msg = std_msgs.msg.ColorRGBA() - msg.r = color[0] - msg.g = color[1] - msg.b = color[2] - msg.a = color[3] - return msg - - -def to_point_msg(point): - msg = geometry_msgs.msg.Point() - msg.x = point[0] - msg.y = point[1] - msg.z = point[2] - return msg - - -def from_point_msg(msg): - return np.r_[msg.x, msg.y, msg.z] - - -def to_vector3_msg(vector3): - msg = geometry_msgs.msg.Vector3() - msg.x = vector3[0] - msg.y = vector3[1] - msg.z = vector3[2] - return msg - - -def from_vector3_msg(msg): - return np.r_[msg.x, msg.y, msg.z] - - -def to_quat_msg(orientation): - quat = orientation.as_quat() - msg = geometry_msgs.msg.Quaternion() - msg.x = quat[0] - msg.y = quat[1] - msg.z = quat[2] - msg.w = quat[3] - return msg - - -def from_quat_msg(msg): - return Rotation.from_quat([msg.x, msg.y, msg.z, msg.w]) - - -def to_pose_msg(transform): - msg = geometry_msgs.msg.Pose() - msg.position = to_point_msg(transform.translation) - msg.orientation = to_quat_msg(transform.rotation) - return msg - - -def from_pose_msg(msg): - position = from_point_msg(msg.position) - orientation = from_quat_msg(msg.orientation) - return Transform(orientation, position) \ No newline at end of file