diff --git a/bt_sim_node.py b/bt_sim_node.py
index 2c551e2..8a291bd 100755
--- a/bt_sim_node.py
+++ b/bt_sim_node.py
@@ -6,6 +6,8 @@ import cv_bridge
import numpy as np
import rospy
+import control_msgs.msg
+import controller_manager_msgs.srv
import franka_gripper.msg
from geometry_msgs.msg import Pose
from sensor_msgs.msg import JointState, Image, CameraInfo
@@ -13,25 +15,25 @@ import std_srvs.srv
from robot_tools.controllers import CartesianPoseController
from robot_tools.ros.conversions import *
-from robot_tools.ros.tf import TransformTree
+from robot_tools.ros.tf import TF2Client
from simulation import Simulation
class BtSimNode:
def __init__(self, gui):
self.sim = Simulation(gui=gui, sleep=False)
- self.controller = CartesianPoseController(self.sim.arm)
self.controller_update_rate = 60
self.joint_state_publish_rate = 60
self.camera_publish_rate = 5
self.cv_bridge = cv_bridge.CvBridge()
- self.tf_tree = TransformTree()
+ self.tf_tree = TF2Client()
self.setup_robot_topics()
self.setup_camera_topics()
- self.setup_gripper_actions()
+ self.setup_controllers()
+ self.setup_gripper_interface()
self.broadcast_transforms()
rospy.Service("reset", std_srvs.srv.Trigger, self.reset)
@@ -40,10 +42,18 @@ class BtSimNode:
def setup_robot_topics(self):
self.joint_pub = rospy.Publisher("joint_states", JointState, queue_size=10)
- rospy.Subscriber("cmd", Pose, self.target_pose_cb)
+
+ def setup_controllers(self):
+ self.cartesian_pose_controller = CartesianPoseController(
+ self.sim.arm, stopped=False
+ )
+ rospy.Subscriber("command", Pose, self.target_pose_cb)
def target_pose_cb(self, msg):
- self.controller.set_target(from_pose_msg(msg))
+ self.cartesian_pose_controller.set_target(from_pose_msg(msg))
+
+ def setup_gripper_interface(self):
+ self.gripper_interface = GripperInterface(self.sim.gripper)
def setup_camera_topics(self):
self.cam_info_msg = to_camera_info_msg(self.sim.camera.intrinsic)
@@ -55,19 +65,6 @@ class BtSimNode:
)
self.depth_pub = rospy.Publisher("/cam/depth/image_raw", Image, queue_size=10)
- def setup_gripper_actions(self):
- self.move_server = actionlib.SimpleActionServer(
- "/franka_gripper/move",
- franka_gripper.msg.MoveAction,
- self.move,
- False,
- )
- self.move_server.start()
-
- def move(self, goal):
- self.sim.gripper.move(goal.width)
- self.move_server.set_succeeded()
-
def broadcast_transforms(self):
msgs = []
msg = geometry_msgs.msg.TransformStamped()
@@ -91,7 +88,7 @@ class BtSimNode:
self.reset_requested = True
rospy.sleep(1.0) # wait for the latest sim step to finish
self.sim.reset()
- self.controller.set_target(self.sim.arm.pose())
+ self.cartesian_pose_controller.set_target(self.sim.arm.pose())
self.step_cnt = 0
self.reset_requested = False
return std_srvs.srv.TriggerResponse(success=True)
@@ -107,7 +104,8 @@ class BtSimNode:
def handle_updates(self):
if self.step_cnt % int(self.sim.rate / self.controller_update_rate) == 0:
- self.controller.update()
+ self.cartesian_pose_controller.update()
+ self.gripper_interface.update(1.0 / 60.0)
if self.step_cnt % int(self.sim.rate / self.joint_state_publish_rate) == 0:
self.publish_joint_state()
if self.step_cnt % int(self.sim.rate / self.camera_publish_rate) == 0:
@@ -138,14 +136,86 @@ class BtSimNode:
self.depth_pub.publish(depth_msg)
-def main(args):
+class GripperInterface:
+ def __init__(self, gripper):
+ self.gripper = gripper
+ self.move_server = actionlib.SimpleActionServer(
+ "/franka_gripper/move",
+ franka_gripper.msg.MoveAction,
+ auto_start=False,
+ )
+ self.move_server.register_goal_callback(self.goal_cb)
+ self.move_server.start()
+
+ def goal_cb(self):
+ goal = self.move_server.accept_new_goal()
+ self.gripper.move(goal.width)
+ self.elapsed_time = 0.0
+
+ def update(self, dt):
+ if self.move_server.is_active():
+ self.elapsed_time += dt
+ if self.elapsed_time > 1.0:
+ self.move_server.set_succeeded()
+
+
+class JointTrajectoryController:
+ def __init__(self, arm, stopped=False):
+ self.arm = arm
+ self.action_server = actionlib.SimpleActionServer(
+ "position_joint_trajectory_controller/follow_joint_trajectory",
+ control_msgs.msg.FollowJointTrajectoryAction,
+ auto_start=False,
+ )
+ self.action_server.register_goal_callback(self.goal_cb)
+ self.action_server.start()
+
+ self.is_running = False
+ if not stopped:
+ self.start()
+
+ def goal_cb(self):
+ goal = self.action_server.accept_new_goal()
+ self.elapsed_time = 0.0
+ self.points = iter(goal.trajectory.points)
+ self.next_point = next(self.points)
+
+ def start(self):
+ self.is_running = True
+
+ def stop(self):
+ self.is_running = False
+
+ def update(self, dt):
+ if not (self.is_running and self.action_server.is_active()):
+ return
+
+ self.elapsed_time += dt
+ if self.elapsed_time > self.next_point.time_from_start.to_sec():
+ try:
+ self.next_point = next(self.points)
+ except StopIteration:
+ self.action_server.set_succeeded()
+ return
+
+ self.arm.set_desired_joint_positions(self.next_point.positions)
+
+
+def create_parser():
+ parser = argparse.ArgumentParser()
+ parser.add_argument("--gui", action="store_true")
+ return parser
+
+
+def main():
rospy.init_node("bt_sim")
+
+ parser = create_parser()
+ args, _ = parser.parse_known_args()
+
server = BtSimNode(args.gui)
server.run()
if __name__ == "__main__":
- parser = argparse.ArgumentParser()
- parser.add_argument("--gui", action="store_true")
- args, _ = parser.parse_known_args()
- main(args)
+ main()
diff --git a/launch/simulation.launch b/launch/simulation.launch
index 1444117..b61c993 100644
--- a/launch/simulation.launch
+++ b/launch/simulation.launch
@@ -11,6 +11,9 @@
+
+
+
diff --git a/policies.py b/policies.py
index f9ef74f..fdade02 100644
--- a/policies.py
+++ b/policies.py
@@ -10,8 +10,9 @@ import std_srvs.srv
from robot_tools.spatial import Rotation, Transform
from robot_tools.ros.conversions import *
+from robot_tools.ros.control import ControllerManagerClient
from robot_tools.ros.panda import PandaGripperClient
-from robot_tools.ros.tf import TransformTree
+from robot_tools.ros.tf import TF2Client
from robot_tools.perception import *
from vgn import vis
from vgn.detection import VGN, compute_grasps
@@ -34,7 +35,7 @@ class BaseController:
self.length = rospy.get_param("~length")
self.cv_bridge = cv_bridge.CvBridge()
- self.tf = TransformTree()
+ self.tf = TF2Client()
self.reset_client = rospy.ServiceProxy("/reset", std_srvs.srv.Trigger)
@@ -49,9 +50,12 @@ class BaseController:
self.base_frame_id = rospy.get_param("~base_frame_id")
self.ee_frame_id = rospy.get_param("~ee_frame_id")
self.ee_grasp_offset = Transform.from_list(rospy.get_param("~ee_grasp_offset"))
- self.target_pose_pub = rospy.Publisher("/cmd", Pose, queue_size=10)
+ self.target_pose_pub = rospy.Publisher("/command", Pose, queue_size=10)
self.gripper = PandaGripperClient()
+ def send_pose_command(self, target):
+ self.target_pose_pub.publish(to_pose_msg(target))
+
def setup_camera_connection(self):
self.cam_frame_id = rospy.get_param("~camera/frame_id")
info_topic = rospy.get_param("~camera/info_topic")
@@ -78,6 +82,7 @@ class BaseController:
self.execute_grasp()
def reset(self):
+ vis.clear()
req = std_srvs.srv.TriggerRequest()
self.reset_client(req)
rospy.sleep(1.0) # wait for states to be updated
@@ -104,13 +109,11 @@ class BaseController:
target = self.T_B_O * grasp.pose * self.ee_grasp_offset.inv()
self.gripper.move(0.08)
- rospy.sleep(1.0) # TODO properly implement the simulated move server
- self.target_pose_pub.publish(to_pose_msg(target))
- rospy.sleep(2.0)
+ self.send_pose_command(target)
+ rospy.sleep(3.0)
self.gripper.move(0.0)
- rospy.sleep(1.0) # TODO
target.translation[2] += 0.3
- self.target_pose_pub.publish(to_pose_msg(target))
+ self.send_pose_command(target)
rospy.sleep(2.0)
def predict_best_grasp(self):
@@ -176,8 +179,30 @@ class FixedTrajectoryBaseline(BaseController):
self.center
+ np.r_[self.radius * np.cos(t), self.radius * np.sin(t), 0.0]
)
- self.target_pose_pub.publish(to_pose_msg(self.target))
+ self.send_pose_command(self.target)
+
+
+class Map:
+ def __init__(self):
+ pass
+
+ def update(self):
+ pass
class MultiViewPicking(BaseController):
- pass
+ def __init__(self):
+ super().__init__()
+ self.rate = 5
+ self.grid = np.zeros((40, 40, 40))
+
+ def reset(self):
+ super().reset()
+ self.tic = rospy.Time.now()
+ timeout = rospy.Duration(0.1)
+ x0 = self.tf.lookup(self.base_frame_id, self.ee_frame_id, self.tic, timeout)
+ self.center = np.r_[x0.translation[0] + self.radius, x0.translation[1:]]
+ self.target = x0
+
+ def update(self):
+ pass
diff --git a/simulation.py b/simulation.py
index da979e5..ed61f21 100644
--- a/simulation.py
+++ b/simulation.py
@@ -27,7 +27,7 @@ class Simulation(BtManipulationSim):
ori = Rotation.from_rotvec(np.array([0, 0, np.pi / 2])).as_quat()
p.loadURDF("table/table.urdf", baseOrientation=ori, useFixedBase=True)
self.length = 0.3
- self.origin = [-0.2, -0.5 * self.length, 0.5]
+ self.origin = [-0.3, -0.5 * self.length, 0.5]
def add_robot(self):
self.T_W_B = Transform(Rotation.identity(), np.r_[-0.6, 0.0, 0.5])
@@ -39,7 +39,8 @@ class Simulation(BtManipulationSim):
self.remove_all_objects()
self.set_initial_arm_configuration()
urdfs = np.random.choice(self.urdfs, 4)
- self.add_random_arrangement(urdfs, np.r_[self.origin[:2], 0.625], self.length)
+ origin = np.r_[self.origin[:2], 0.625]
+ self.add_random_arrangement(urdfs, origin, self.length, 0.8)
def set_initial_arm_configuration(self):
q = self.arm.configurations["ready"]