def __init__(self):
self.pub_tf = rospy.Publisher("/tf", tf2_msgs.msg.TFMessage, queue_size=1)
stefan = SceneObject()
while not rospy.is_shutdown():
rospy.sleep(0.1)
t = geometry_msgs.msg.TransformStamped()
t.header.frame_id = "base"
for key, value in stefan.list.items():
t.header.stamp = rospy.Time.now()
t.child_frame_id = key + "_frame"
t.transform.translation = value.pose.position
t.transform.rotation = value.pose.orientation
tfm = tf2_msgs.msg.TFMessage([t])
self.pub_tf.publish(tfm)
def __init__(self):
### MoveIt!
moveit_commander.roscpp_initialize(sys.argv)
#rospy.init_node('move_group_planner',
# anonymous=True)
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.group = moveit_commander.MoveGroupCommander("panda_arms")
self.group_left = moveit_commander.MoveGroupCommander("panda_left")
# self.object_group = moveit_commander.MoveGroupCommander("stefan")
self.hand_left = moveit_commander.MoveGroupCommander("hand_left")
self.hand_right = moveit_commander.MoveGroupCommander("hand_right")
self.display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
# We can get the name of the reference frame for this robot:
self.planning_frame = self.group.get_planning_frame()
print("============ Reference frame: %s" % self.planning_frame)
# We can also print the name of the end-effector link for this group:
self.eef_link = self.group.get_end_effector_link()
print("============ End effector: %s" % self.eef_link)
# We can get a list of all the groups in the robot:
self.group_names = self.robot.get_group_names()
print("============ Robot Groups:", self.robot.get_group_names())
rospy.sleep(1)
self.stefan_dir = "/home/jiyeong/STEFAN/stl/"
self.stefan = SceneObject()
self.scene.remove_attached_object(self.group.get_end_effector_link())
rospy.sleep(1)
self.scene.remove_world_object()
for key, value in self.stefan.list.items():
self.scene.add_mesh(key, value, self.stefan_dir + key + ".stl")
rospy.sleep(1)
### Franka Collision
self.set_collision_behavior = rospy.ServiceProxy(
'franka_control/set_force_torque_collision_behavior',
franka_control.srv.SetForceTorqueCollisionBehavior)
#self.set_collision_behavior.wait_for_service()
self.active_controllers = []
def __init__(self):
### MoveIt!
moveit_commander.roscpp_initialize(sys.argv)
br = tf.TransformBroadcaster()
#rospy.init_node('move_group_planner',
# anonymous=True)
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.plan_scene = moveit_commander.PlanningScene()
self.group_1st = moveit_commander.MoveGroupCommander("panda_1")
self.group_2nd = moveit_commander.MoveGroupCommander("panda_2")
self.group_3rd = moveit_commander.MoveGroupCommander("panda_3")
self.group_list = [self.group_1st, self.group_2nd, self.group_3rd]
self.group_chain = moveit_commander.MoveGroupCommander("panda_closed_chain")
self.group_chairup = moveit_commander.MoveGroupCommander("panda_chair_up")
self.hand_1st = moveit_commander.MoveGroupCommander("hand_1")
self.hand_2nd = moveit_commander.MoveGroupCommander("hand_2")
self.hand_3rd = moveit_commander.MoveGroupCommander("hand_3")
self.hand_list = [self.hand_1st, self.hand_2nd, self.hand_3rd]
self.display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
# We can get the name of the reference frame for this robot:
self.planning_frame = self.group_1st.get_planning_frame()
print ("============ Reference frame: %s" % self.planning_frame)
# We can also print the name of the end-effector link for this group:
self.eef_link = self.group_1st.get_end_effector_link()
print ("============ End effector: %s" % self.eef_link)
# We can get the name of the reference frame for this robot:
self.planning_frame = self.group_3rd.get_planning_frame()
print ("============ Reference frame: %s" % self.planning_frame)
# We can also print the name of the end-effector link for this group:
self.eef_link = self.group_3rd.get_end_effector_link()
print ("============ End effector: %s" % self.eef_link)
# We can get a list of all the groups in the robot:
self.group_names = self.robot.get_group_names()
print ("============ Robot Groups:", self.robot.get_group_names())
rospy.sleep(1)
self.stefan = SceneObject()
# self.scene.remove_attached_object(self.group_1st.get_end_effector_link())
# self.scene.remove_attached_object(self.group_3rd.get_end_effector_link())
# self.scene.remove_world_object()
### Franka Collision
self.set_collision_behavior = rospy.ServiceProxy(
'franka_control/set_force_torque_collision_behavior',
franka_control.srv.SetForceTorqueCollisionBehavior)
#self.set_collision_behavior.wait_for_service()
self.active_controllers = []
self.listener = tf.TransformListener()
self.tr = TransformerROS()
box_pose = geometry_msgs.msg.PoseStamped()
box_pose.header.frame_id = "base"
box_pose.pose.orientation.w = 1.0
box_pose.pose.position.x = 0.8
box_pose.pose.position.z = 0.675
box_pose2 = geometry_msgs.msg.PoseStamped()
box_pose2.header.frame_id = "base"
box_pose2.pose.orientation.w = 1.0
box_pose2.pose.position.x = 0.8
box_pose2.pose.position.y = -0.25
box_pose2.pose.position.z = 1.0
self.scene.add_box("box", box_pose, size = (1.2, 0.5, 0.15))