hofee/nbv_sim
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Simulate joint trajectory and move gripper interfaces

Browse Source
This commit is contained in:
Michel Breyer 2021-06-02 10:54:52 +02:00
parent 0545dbcc06
commit e04361dd8c
4 changed files with 137 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

122
bt_sim_node.py
View File

@ -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()

3
launch/simulation.launch
View File

@ -11,6 +11,9 @@
<node pkg="active_grasp" type="bt_sim_node.py" name="simulation" args="--gui" output="screen" />
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
<!-- Launch MoveIt -->
<!-- <include file="$(find panda_moveit_config)/launch/move_group.launch" /> -->
<!-- Visualize the robot. -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find active_grasp)/launch/panda_visualization.rviz" if="$(arg launch_rviz)" />
</launch>

43
policies.py
View File

@ -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):
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

5
simulation.py
View File

@ -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"]