def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
rospy.Subscriber('camera/depth_registered/points', PointCloud2, point_callback)
rospy.Subscriber('darknet_ros/bounding_boxes', BoundingBoxes, bbox_callback)
freq = 30
rate = rospy.Rate(freq)
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory,queue_size=20)
self.ii = 0
# self.listener = tf.TransformListener()
self.goal_x = 0
self.goal_y = 0
self.goal_z = 0
self.goal_ori_x = 0
self.goal_ori_y = 0
self.goal_ori_z = 0
self.goal_ori_w = 0
self.marker = []
self.position_list = []
self.orientation_list = []
self.m_idd = 0
self.m_pose_x = []
self.m_pose_y = []
self.m_pose_z = []
self.m_ori_w = []
self.m_ori_x = []
self.m_ori_y = []
self.m_ori_z = []
self.br = tf.TransformBroadcaster()
self.target_ar_id = 9
self.calculed_tomato = Pose()
self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty)
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(5)
self.robot_arm.set_num_planning_attempts(5)
self.display_trajectory_publisher = display_trajectory_publisher
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
self.clear_octomap()
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
self.jaco_arm = MoveGroupCommander("Arm")
self.hand = MoveGroupCommander("Hand")
self.pose_pub = rospy.Publisher("hand_pose",
PoseStamped,
queue_size=100)
self.pick_command = rospy.Publisher("pick_command",
Bool,
queue_size=100)
rospy.Subscriber("pick_pose", PoseStamped, self.pick)
self.jaco_arm.allow_replanning(True)
self.jaco_arm.set_planning_time(5)
self.jaco_arm.set_goal_tolerance(0.02)
self.jaco_arm.set_goal_orientation_tolerance(0.1)
self.place_pose = PoseStamped()
self.place_pose.header.frame_id = 'arm_stand'
self.place_pose.pose.position.x = 0.4
self.place_pose.pose.position.y = 0.4
self.place_pose.pose.position.z = -0.4
self.place_pose.pose.orientation = Quaternion(0.606301648371,
0.599731279995,
0.381153346104,
0.356991358063)
self.orient = [
2.042967990797618, -0.03399658412747265, 1.5807131823846676
]
self.result = False
def move_arm():
# plan to a random location
group = moveit_commander.MoveGroupCommander("arm")
robot = RobotCommander()
a = robot.arm
a.set_start_state(RobotState())
p = a.plan([0, 0])
print "Solution:"
print p
a.execute(p)
rospy.sleep(10)
a.set_start_state(RobotState())
p = a.plan([1.57, -1.57])
print "Solution:"
print p
a.execute(p)
rospy.sleep(10)
a.set_start_state(RobotState())
p = a.plan([3.14, 3.14])
print "Solution:"
print p
a.execute(p)
rospy.sleep(10)
moveit_commander.roscpp_shutdown()
print "Finishing"
def __init__(self):
self._scene = PlanningSceneInterface()
# clear the scene
self._scene.remove_world_object()
self.robot = RobotCommander()
# pause to wait for rviz to load
rospy.sleep(4)
floor_pose = [0, 0, -0.12, 0, 0, 0, 1]
floor_dimensions = [4, 4, 0.02]
box1_pose = [0.70, 0.70, 0.25, 0, 0, 0, 1]
box1_dimensions = [0.5, 0.5, 0.5]
box2_pose = [0.5, -0.5, 0.60, 0, 0, 0, 1]
box2_dimensions = [0.25, 0.25, 0.25]
box3_pose = [0.5, -0.5, 0.24, 0, 0, 0, 1]
box3_dimensions = [0.48, 0.48, 0.48]
box4_pose = [-0.8, 0.7, 0.5, 0, 0, 0, 1]
box4_dimensions = [0.4, 0.4, 0.4]
self.add_box_object("floor", floor_dimensions, floor_pose)
self.add_box_object("box1", box1_dimensions, box1_pose)
self.add_box_object("box2", box2_dimensions, box2_pose)
self.add_box_object("box3", box3_dimensions, box3_pose)
self.add_box_object("box4", box4_dimensions, box4_pose)
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
#robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(5)
self.robot_arm.set_num_planning_attempts(5)
self.robot_arm.remember_joint_values("zeros", None)
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
#self.robot_arm.allow_replanning(True)
#scene = moveit_commander.PlanningSceneInterface()
#robot = moveit_commander.RobotCommander()
#rospy.sleep(2)
#add Table
p = PoseStamped()
p.header.frame_id = FIXED_FRAME
p.pose.position.x = 0.0
p.pose.position.y = 0.2
p.pose.position.z = 0.1
self.scene.add_box("table", p, (0.3, 0.3, 0.3))
def main(argv):
rospy.init_node("Table_mesh_inserter")
scene = PlanningSceneInterface()
robot = RobotCommander()
while (scene._pub_co.get_num_connections() < 1):
pass
quaternion = tf.transformations.quaternion_from_euler(0.0, 0.0, 0.0)
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0.3250
p.pose.position.y = 0.4250
p.pose.position.z = -0.540
p.pose.orientation.x = quaternion[0]
p.pose.orientation.y = quaternion[1]
p.pose.orientation.z = quaternion[2]
p.pose.orientation.w = quaternion[3]
scene.add_box("table", p, (1.2, 1.2, 1.05))
p.pose.position.x = 0.0
p.pose.position.y = 0.0
p.pose.position.z = -0.0075
scene.add_box("plate", p, (0.25, 0.25, 0.015))
while not rospy.is_shutdown():
pass
def commander():
global past_reads
rospy.init_node('rubik_camera', anonymous=True)
rospy.Subscriber("/usb_cam/image_raw", Image, callback)
pub = rospy.Publisher("cube_configuration", String, queue_size=1)
# Gets a handle on the robot that MoveIt is controlling
robot = RobotCommander()
rospy.sleep(1)
# Picks a group called "manipulator" to work with #
arm = robot.get_group("manipulator")
arm.set_max_velocity_scaling_factor(1.0)
arm.set_goal_position_tolerance(0.0001)
arm.set_goal_orientation_tolerance(0.005)
go_home(arm)
pick_rubik(arm, robot, "right")
go_home(arm)
for i in range(5):
read(arm, i)
go_home(arm)
place_rubik(arm, robot, "right")
go_home(arm)
pick_rubik(arm, robot, "left")
go_home(arm)
for i in range(5):
read(arm, i)
go_home(arm)
place_rubik(arm, robot, "left")
go_home(arm)
print(str(past_reads))
pub.publish(reads_to_cube())
rospy.sleep(1)
def _init_planning(self):
"""
Initialises the needed connections for the planning.
"""
self.group_id = self._annotations["group_id"]
self.planners = self._annotations["planners"]
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
self.group = MoveGroupCommander(self.group_id)
self._marker_pub = rospy.Publisher('/visualization_marker_array',
MarkerArray,
queue_size=10,
latch=True)
self._planning_time_sub = rospy.Subscriber(
'/move_group/result', MoveGroupActionResult,
self._check_computation_time)
rospy.sleep(1)
self.group.set_num_planning_attempts(
self._annotations["planning_attempts"])
self.group.set_goal_tolerance(self._annotations["goal_tolerance"])
self.group.set_planning_time(self._annotations["planning_time"])
self.group.allow_replanning(self._annotations["allow_replanning"])
self._comp_time = []
self.planner_data = []
def callback(states):
scene = PlanningSceneInterface()
robot = RobotCommander()
arm = MoveGroupCommander("manipulator")
arm.set_planner_id("RRTConnectkConfigDefault")
arm.set_num_planning_attempts(20)
arm.allow_replanning(True)
pose = copy.deepcopy(states.pose[-1])
temp = tf.transformations.euler_from_quaternion(
(pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w))
quaternion = tf.transformations.quaternion_from_euler(
math.pi, temp[1], temp[2])
pose.position.z += 0.22
pose.orientation.x = quaternion[0]
pose.orientation.y = quaternion[1]
pose.orientation.z = quaternion[2]
pose.orientation.w = quaternion[3]
print pose
arm.set_pose_target(pose)
move_plan = arm.plan()
i = 0
while (not move_plan.joint_trajectory.joint_names):
move_plan = arm.plan()
i += 1
if (i == 5): break
state = arm.execute(move_plan)
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move',anonymous=True)
self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty)
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
#robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(5)
self.robot_arm.set_num_planning_attempts(5)
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
init_table_goal = self.robot_arm.get_current_joint_values()
init_table_goal[0] = 0.2
init_table_goal[1] = -1.983025375996725
init_table_goal[2] = -2.4233086744891565
init_table_goal[3] = 0.9490636587142944
init_table_goal[4] = 1.4068996906280518
init_table_goal[5] = -3.060608450566427
self.robot_arm.go(init_table_goal, wait=True)
rospy.sleep(0.5)
self.clear_octomap()
print("====== move plan go to init table goal ======")
def __init__(self):
self._scene = PlanningSceneInterface()
self.robot = RobotCommander()
# pause to wait for rviz to load
rospy.sleep(4)
self.first_stamp = None
self.cloud_pub = rospy.Publisher('/camera/depth_registered/points',
PointCloud2,
queue_size=20,
latch=True)
self.cloud_sub = rospy.Subscriber(
'/camera/depth_registered/points_old', PointCloud2, self.msg_cb)
# clear the scene
self._scene.remove_world_object()
# add floor object
floor_pose = [0, 0, -0.12, 0, 0, 0, 1]
floor_dimensions = [4, 4, 0.02]
self.add_box_object("floor", floor_dimensions, floor_pose)
# add collision objects
self._kinect_pose_1 = [0.0, 0.815, 0.665, 0, -0.707, 0.707, 0]
# stream the kinect tf
kinect_cloud_topic = "kinect2_rgb_optical_frame"
thread.start_new_thread(self.spin, (kinect_cloud_topic, ))
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
self.goal_x = 0
self.goal_y = 0
self.goal_z = 0
#rospy.loginfo("Waiting for ar_pose_marker topic...")
#rospy.wait_for_message('ar_pose_marker', AlvarMarkers)
#rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.ar_callback)
#rospy.loginfo("Maker messages detected. Starting followers...")
self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty)
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
#robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(5)
self.robot_arm.set_num_planning_attempts(5)
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
self.ur5 = MoveGroupCommander("manipulator")
self.gripper = MoveGroupCommander("end_effector")
print("======================================================")
print(self.robot.get_group_names())
print(self.robot.get_link_names())
print(self.robot.get_joint_names())
print(self.robot.get_planning_frame())
print(self.ur5.get_end_effector_link())
print("======================================================")
self.ur5.set_max_velocity_scaling_factor(0.2)
self.ur5.set_max_acceleration_scaling_factor(0.2)
self.ur5.set_end_effector_link("fts_toolside")
self.ur5.set_planning_time(10.0)
#self.ur5.set_planner_id("RRTkConfigDefault")
self.gripper_ac = RobotiqActionClient('icl_phri_gripper/gripper_controller')
self.gripper_ac.wait_for_server()
self.gripper_ac.initiate()
self.gripper_ac.send_goal(0.08)
self.gripper_ac.wait_for_result()
self.sub = rospy.Subscriber('/target_position', Int32MultiArray, self.pickplace_cb)
def get_group_for_joint(joint_names, robot=None, smallest=False, groups=None):
"""Given list of joints find the most appropriate group.
INPUT
link_ids [List of Strings]
robot [RobotCommander]
smallest [Bool]
OUTPUT
group [String]
"""
if not robot:
robot = RobotCommander()
if not groups:
groups = robot.get_group_names()
group = None
n = np.Inf if smallest else 0
for g in groups:
names = robot.get_joint_names(g)
if all_in(joint_names, names):
if smallest and len(names) < n:
n = len(names)
group = g
elif len(names) > n:
n = len(names)
group = g
return group
def __init__(self):
allowed_planning_time = 0.0
ARM_AND_LINEAR_GROUP_NAME = "arm_and_linear"
ARM_GROUP_NAME = "arm_torso"
GRIPPER_GROUP = "gripper"
REFERENCE_FRAME = "base_link"
PLANNER = "RRTConnectkConfigDefault"
#PLANNER = "RRTstarkConfigDefault"
MOVE_GROUP_SERVICE = "/move_group/plan_execution/set_parameters"
result = False
upright_constraints = None
robot = None
arm = None
arm_and_linear = None
database = None
scene = None
grasp_generator = None
place_generator = None
marker_publisher = None
self.robot = RobotCommander()
self.arm = self.robot.get_group("arm_torso")
self.gripper = self.robot.get_group(GRIPPER_GROUP)
self.scene = PlanningSceneInterface()
self.pickup_object = RecognizedObject("part")
self.table = EntityInfo()
def __init__(self):
# Retrieve params:
self._table_object_name = rospy.get_param('~table_object_name',
'Grasp_Table')
self._grasp_object_name = rospy.get_param('~grasp_object_name',
'Grasp_Object')
self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01)
self._arm_group = rospy.get_param('~manipulator', 'manipulator')
self._gripper_group = rospy.get_param('~gripper', 'gripper')
self._approach_retreat_desired_dist = rospy.get_param(
'~approach_retreat_desired_dist', 0.2)
self._approach_retreat_min_dist = rospy.get_param(
'~approach_retreat_min_dist', 0.1)
# Create (debugging) publishers:
self._grasps_pub = rospy.Publisher('grasps',
PoseArray,
queue_size=1,
latch=True)
self._places_pub = rospy.Publisher('places',
PoseArray,
queue_size=1,
latch=True)
# Create planning scene and robot commander:
self._scene = PlanningSceneInterface()
self._robot = RobotCommander()
# Retrieve groups (arm and gripper):
self._arm = self._robot.get_group(self._arm_group)
self._gripper = self._robot.get_group(self._gripper_group)
rospy.sleep(1.0)
# self.setup()
# Clean the scene:
self._scene.remove_world_object(self._table_object_name)
self._scene.remove_world_object(self._grasp_object_name)
# Add table and Coke can objects to the planning scene:
self._pose_table = self._add_table(self._table_object_name)
rospy.loginfo("added table")
rospy.sleep(2.0)
self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name)
rospy.loginfo("added grasping box")
rospy.sleep(1.0)
# Define target place pose:
self._pose_place = Pose()
self._pose_place.position.x = self._pose_coke_can.position.x
self._pose_place.position.y = self._pose_coke_can.position.y - 0.06
self._pose_place.position.z = self._pose_coke_can.position.z
self._pose_place.orientation = Quaternion(
*quaternion_from_euler(0.0, 0.0, 0.0))
def __init__(self):
roscpp_initialize([])
rospy.sleep(8)
# TO-DO: wait for moveit.
self.listener = tf.TransformListener()
self.robot = RobotCommander()
self.init_planner()
self.gripper_dimensions = [0.02, 0.02, 0.02]
def init_arm(self, num_planning_attempts=20):
self.arm = MoveGroupCommander("arm")
self.gripper = MoveGroupCommander("gripper")
self.robot = RobotCommander()
self.arm.set_num_planning_attempts(num_planning_attempts)
self.arm.set_goal_tolerance(0.2)
self.init_services()
self.init_action_servers()
def get_current_state():
# Prepare a new state object for validity check
rs = RobotState()
robot = RobotCommander()
robot_state = robot.get_current_state()
rs.joint_state.name = robot_state.joint_state.name
rs.joint_state.position = list(robot_state.joint_state.position) # filler for rest of the joint angles not found in waypoint
return rs
def initialize_environment():
# Initialize environment
rospy.init_node('Baxter_Env')
robot = RobotCommander()
scene = PlanningSceneInterface()
scene._scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=0)
rospy.sleep(2)
return scene, robot
def setUp(self):
self.robot = RobotCommander()
self.bounds = list()
for name in self.robot.get_joint_names():
joint = self.robot.get_joint(name)
bounds = joint.bounds()
if bounds:
self.bounds.append((name, bounds))
def __init__(self):
self.scene = PlanningSceneInterface()
self.robot = RobotCommander()
self.p = PoseStamped()
self.scene_srv = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene)
rospy.loginfo("Connecting to clear octomap service...")
self.clear_octomap_srv = rospy.ServiceProxy('/clear_octomap', Empty)
self.clear_octomap_srv.wait_for_service()
rospy.loginfo("Connected!")
def init_scene(self):
world_objects = [
"table", "tui", "monitor", "overHead", "wall", "farWall",
"frontWall", "backWall", "blockProtector", "rearCamera"
]
self.robot = RobotCommander()
self.scene = PlanningSceneInterface()
for obj in world_objects:
self.scene.remove_world_object(obj)
rospy.sleep(0.5)
self.tuiPose = PoseStamped()
self.tuiPose.header.frame_id = self.robot.get_planning_frame()
self.tuiPose.pose.position = Point(0.0056, -0.343, -0.51)
self.tuiDimension = (0.9906, 0.8382, 0.8836)
self.overHeadPose = PoseStamped()
self.overHeadPose.header.frame_id = self.robot.get_planning_frame()
self.overHeadPose.pose.position = Point(0.0056, 0.0, 0.97)
self.overHeadDimension = (0.9906, 0.8382, 0.05)
self.blockProtectorPose = PoseStamped()
self.blockProtectorPose.header.frame_id = self.robot.get_planning_frame(
)
self.blockProtectorPose.pose.position = Point(
0.0056, -0.343, -0.51 + self.cruising_height)
self.blockProtectorDimension = (0.9906, 0.8382, 0.8636)
self.wallPose = PoseStamped()
self.wallPose.header.frame_id = self.robot.get_planning_frame()
self.wallPose.pose.position = Point(-0.858, -0.343, -0.3048)
self.wallDimension = (0.6096, 2, 1.35)
self.farWallPose = PoseStamped()
self.farWallPose.header.frame_id = self.robot.get_planning_frame()
self.farWallPose.pose.position = Point(0.9, -0.343, -0.3048)
self.farWallDimension = (0.6096, 2, 3.35)
self.frontWallPose = PoseStamped()
self.frontWallPose.header.frame_id = self.robot.get_planning_frame()
self.frontWallPose.pose.position = Point(0.0056, -0.85, -0.51)
self.frontWallDimension = (1, 0.15, 4)
self.backWallPose = PoseStamped()
self.backWallPose.header.frame_id = self.robot.get_planning_frame()
self.backWallPose.pose.position = Point(0.0056, 0.55, -0.51)
self.backWallDimension = (1, 0.005, 4)
self.rearCameraPose = PoseStamped()
self.rearCameraPose.header.frame_id = self.robot.get_planning_frame()
self.rearCameraPose.pose.position = Point(0.65, 0.45, -0.51)
self.rearCameraDimension = (0.5, 0.5, 2)
rospy.sleep(0.5)
self.scene.add_box("tui", self.tuiPose, self.tuiDimension)
self.scene.add_box("wall", self.wallPose, self.wallDimension)
self.scene.add_box("farWall", self.farWallPose, self.farWallDimension)
self.scene.add_box("overHead", self.overHeadPose,
self.overHeadDimension)
self.scene.add_box("backWall", self.backWallPose,
self.backWallDimension)
self.scene.add_box("frontWall", self.frontWallPose,
self.frontWallDimension)
self.scene.add_box("rearCamera", self.rearCameraPose,
self.rearCameraDimension)
def __init__(self,
scene=None,
manip_name="manipulator",
eef_name="endeffector"):
self.robot = RobotCommander()
self.man = MoveGroupCommander(manip_name)
self.eef = MoveGroupCommander(eef_name)
self.scene = scene
self.target_poses = []
self.picked = []
def setUpClass(self):
rospy.init_node('test_moveit_vs060')
self.robot = RobotCommander()
self._mvgroup = MoveGroupCommander(self._MOVEGROUP_MAIN)
# Temporary workaround of planner's issue similar to https://github.com/tork-a/rtmros_nextage/issues/170
self._mvgroup.set_planner_id(self._KINEMATICSOLVER_SAFE)
self._movegroups = sorted(['manipulator', 'manipulator_flange'])
self._joints_movegroup_main = sorted(
['j1', 'j2', 'j3', 'j4', 'j5', 'flange'])
def __init__(self):
self._gdict = {}
self._group_name = ""
self._prev_group_name = ""
self._planning_scene_interface = PlanningSceneInterface()
self._robot = RobotCommander()
self._last_plan = None
self._db_host = None
self._db_port = 33829
self._trace = False
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
self.detected = {}
self.detected_names = rospy.get_param(
'/darknet_ros/yolo_model/detection_classes/names')
self.object_pose_sub = rospy.Subscriber(
'/cluster_decomposer/centroid_pose_array', PoseArray,
self.collectJsk)
self.listener = tf.TransformListener()
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
self.ii = 0
global goal_x
global goal_y
global goal_z
global ori_w
global ori_x
global ori_y
global ori_z
self.distance = 0
self.tomato = []
self.position_list = []
self.orientation_list = []
self.tomato_pose = Pose()
self.goalPoseFromTomato = Pose()
self.br = tf.TransformBroadcaster()
self.calculated_tomato = Pose()
self.calculated_tomato_coor = Pose()
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(10)
self.robot_arm.set_num_planning_attempts(10)
self.display_trajectory_publisher = display_trajectory_publisher
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
def print_joint_bounds(robot=None):
if not robot:
robot = RobotCommander()
print 'NAME'.center(16), 'MIN'.center(10), 'MAX'.center(10)
for name in robot.get_joint_names():
joint = robot.get_joint(name)
bounds = joint.bounds()
if bounds:
print '%15s % 10.3f % 10.3f' % (name, bounds[0], bounds[1])
def __init__(self, group_name='arm_torso'):
self.rate = rospy.Rate(10)
self.robot = RobotCommander()
rospy.sleep(1)
self.move_group = MoveGroupCommander(group_name)
self.velocity = Twist()
self.velocity_publisher = rospy.Publisher(
'/mobile_base_controller/cmd_vel', Twist, queue_size=10)
def init_arm(self, num_planning_attempts=700):
rospy.set_param(
"/move_group/trajectory_execution/allowed_start_tolerance",
self.START_TOLERANCE)
self.arm = MoveGroupCommander("arm")
self.gripper = MoveGroupCommander("gripper")
self.robot = RobotCommander()
self.arm.set_num_planning_attempts(num_planning_attempts)
self.arm.set_goal_position_tolerance(self.GOAL_TOLERANCE)
self.arm.set_goal_orientation_tolerance(0.02)
self.init_services()
self.init_action_servers()
