def add_yaml(self, yaml):
"""
Adds some fixed collision objects that are defined in the yaml description
:param yaml: yaml description of the task
:type: Task
"""
self.add_ground()
if yaml is None:
self.add_cam_mast(0.92, 0.92)
else:
x = yaml.mast_of_cam.base_pose.linear.x
y = yaml.mast_of_cam.base_pose.linear.y
self.add_cam_mast(x, y)
if len(yaml.relative_puzzle_part_target_poses) == 0:
return
pose = PoseStamped()
pose.header.frame_id = "/odom_combined"
pose.pose = deepcopy(yaml.puzzle_fixture)
#TODO: WTF?
#if you change the Point shit here, also change it in map.py!
# pose.pose.position = add_point(pose.pose.position, Point(0.115 if yaml.puzzle_fixture.position.x < 0 else -0.18, 0.165 if yaml.puzzle_fixture.position.y < 0 else 0.05, 0))
pose.pose.position = get_puzzle_fixture_center(yaml.puzzle_fixture)
box = self.make_box("puzzle", pose, [0.32, 0.32, 0.1])
print("puzzle_fixture collision object = " + str(box))
self.safe_objects.append(box.id)
self.add_object(box)
def add_yaml(self, yaml):
"""
Adds some fixed collision objects that are defined in the yaml description
:param yaml: yaml description of the task
:type: Task
"""
self.add_ground()
if yaml is None:
self.add_cam_mast(0.92, 0.92)
else:
x = yaml.mast_of_cam.base_pose.linear.x
y = yaml.mast_of_cam.base_pose.linear.y
self.add_cam_mast(x, y)
if len(yaml.relative_puzzle_part_target_poses) == 0:
return
pose = PoseStamped()
pose.header.frame_id = "/odom_combined"
pose.pose = deepcopy(yaml.puzzle_fixture)
#TODO: WTF?
#if you change the Point shit here, also change it in map.py!
# pose.pose.position = add_point(pose.pose.position, Point(0.115 if yaml.puzzle_fixture.position.x < 0 else -0.18, 0.165 if yaml.puzzle_fixture.position.y < 0 else 0.05, 0))
pose.pose.position = get_puzzle_fixture_center(yaml.puzzle_fixture)
box = self.make_box("puzzle", pose, [0.32, 0.32, 0.1])
print("puzzle_fixture collision object = "+str(box))
self.safe_objects.append(box.id)
self.add_object(box)
def goto_shelf2(self):
shelf = PoseStamped()
header = Header()
header.frame_id = 'map'
shelf.header = header
shelf.pose.position = Point(-1.0, self.dist_to_shelfs, 0.0)
shelf.pose.orientation = Quaternion(0.0, 0.0, 0.0, 1.0)
self(shelf)
def relative_pose(self, position, orientation):
shelf = PoseStamped()
header = Header()
header.frame_id = 'base_footprint'
shelf.header = header
shelf.pose.position = Point(*position)
shelf.pose.orientation = Quaternion(*orientation)
self(shelf)
def goto_shelf4(self):
shelf = PoseStamped()
header = Header()
header.frame_id = 'map'
shelf.header = header
shelf.pose.position = Point(-2.9, self.dist_to_shelfs, 0.000)
shelf.pose.orientation = Quaternion(0.0, 0.0, 0.0, 1.0)
# shelf.pose.orientation = Quaternion(*quaternion_from_euler(0,0,pi*.99))
self(shelf)
def goto_shelf1(self):
# self.client.cancel_all_goals()
shelf = PoseStamped()
header = Header()
header.frame_id = 'map'
shelf.header = header
shelf.pose.position = Point(-0.0, self.dist_to_shelfs, 0.0)
shelf.pose.orientation = Quaternion(0.0, 0.0, 0.0, 1.0)
self(shelf)
def np_array_to_pose_stampeds(self, nparray, orientation, header):
pose_list = []
for p in nparray:
pose = PoseStamped()
pose.pose.position = Point(*p)
pose.pose.orientation = orientation
pose.header = header
pose_list.append(pose)
return pose_list
def odomCallback(odomData):
global lastOdom,tfl,lastMapPose
lastOdom = odomData
temp = PoseStampedMsg()
temp.pose = lastOdom.pose.pose
temp.header = lastOdom.header
try:
#now lastmappose has map coords
tfl.transformPose("map",temp,lastMapPose)
except tf.TransformException:
rospy.roserror("Transform Error")
def transform_to(self, pose_target, target_frame="/odom_combined"):
'''
Transforms the pose_target into the target_frame.
:param pose_target: object to transform as PoseStamped/PointStamped/Vector3Stamped/CollisionObject/PointCloud2
:param target_frame: goal frame id
:return: transformed object
'''
if pose_target.header.frame_id == target_frame:
return pose_target
odom_pose = None
i = 0
while odom_pose is None and i < 10:
try:
#now = rospy.Time.now()
#self.__listener.waitForTransform(target_frame, pose_target.header.frame_id, now, rospy.Duration(4))
if type(pose_target) is CollisionObject:
i = 0
new_co = deepcopy(pose_target)
for cop in pose_target.primitive_poses:
p = PoseStamped()
p.header = pose_target.header
p.pose.orientation = cop.orientation
p.pose.position = cop.position
p = self.transform_to(p, target_frame)
if p is None:
return None
new_co.primitive_poses[i].position = p.pose.position
new_co.primitive_poses[i].orientation = p.pose.orientation
i += 1
new_co.header.frame_id = target_frame
return new_co
if type(pose_target) is PoseStamped:
odom_pose = self.__listener.transformPose(target_frame, pose_target)
break
if type(pose_target) is Vector3Stamped:
odom_pose = self.__listener.transformVector3(target_frame, pose_target)
break
if type(pose_target) is PointStamped:
odom_pose = self.__listener.transformPoint(target_frame, pose_target)
break
if type(pose_target) is PointCloud2:
odom_pose = self.__listener.transformPointCloud(target_frame, pose_target)
break
except Exception, e:
print "tf error:::", e
rospy.sleep(0.5)
i += 1
rospy.logdebug("tf fail nr. " + str(i))
def goal_pose_to_cmd(self, goal_pose, arm):
moving_arm_cmd = ArmCommand()
moving_arm_cmd.type = ArmCommand.CARTESIAN_GOAL
moving_arm_cmd.goal_pose = goal_pose
asdf = SemanticFloat64()
asdf.semantics = 'l_rot_error'
asdf.value = 0.0174
moving_arm_cmd.convergence_thresholds.append(asdf)
asdf = SemanticFloat64()
asdf.semantics = 'l_trans_error'
asdf.value = 0.001
moving_arm_cmd.convergence_thresholds.append(asdf)
nonmoving_arm_cmd = ArmCommand()
right_goal = PoseStamped()
right_goal.header.frame_id = 'right_gripper_tool_frame'
right_goal.pose.orientation.w = 1
nonmoving_arm_cmd.type = ArmCommand.CARTESIAN_GOAL
nonmoving_arm_cmd.goal_pose = self.transformPose(
'base_footprint', right_goal)
cmd = WholeBodyCommand()
cmd.type = WholeBodyCommand.STANDARD_CONTROLLER
if arm == WiggleGoal.LEFT_ARM:
cmd.left_ee = moving_arm_cmd
cmd.right_ee = nonmoving_arm_cmd
else:
cmd.left_ee = nonmoving_arm_cmd
cmd.right_ee = moving_arm_cmd
return cmd
def get_place_position_for_puzzle(destination, orientation):
rospy.logdebug("get_place_position_for_puzzle")
place_poses = []
pitch = pi / 2
if orientation_to_vector(
rotate_quaternion(deepcopy(orientation), 0, pitch, 0)).z > 0:
pitch = -pitch
place_pose_1 = PoseStamped()
place_pose_1.header.frame_id = "/odom_combined"
place_pose_1.pose.position = deepcopy(destination.point)
place_pose_1.pose.orientation = rotate_quaternion(deepcopy(orientation), 0,
pitch, 0)
place_poses.append(place_pose_1)
#roll = add_point(deepcopy(destination.point),Point(-1, 0, 0))
place_pose_2 = deepcopy(place_pose_1)
place_pose_2.pose.orientation = rotate_quaternion(deepcopy(orientation), 0,
pitch, pi / 2)
place_poses.append(place_pose_2)
#roll = add_point(deepcopy(destination.point),Point(0, 1, 0))
place_pose_3 = deepcopy(place_pose_1)
place_pose_3.pose.orientation = rotate_quaternion(deepcopy(orientation), 0,
pitch, pi)
place_poses.append(place_pose_3)
#roll = add_point(deepcopy(destination.point),Point(0, -1, 0))
place_pose_4 = deepcopy(place_pose_1)
place_pose_4.pose.orientation = rotate_quaternion(deepcopy(orientation), 0,
pitch, pi * 1.5)
place_poses.append(place_pose_4)
return place_poses
def marker_callback(self, data):
if data.event_type == InteractiveMarkerFeedback.MOUSE_UP:
rospy.loginfo("Current Marker Pose for %s:", self.mesh_resource)
self.tool_marker.header = data.header
self.tool_marker.pose = data.pose
self.marker_pub.publish(self.tool_marker)
rospy.loginfo("\n%s", PoseStamped(data.header, data.pose))
self.server.applyChanges()
def test1_1(self):
p = PoseStamped()
p.header.frame_id = "/odom_combined"
p.pose.position = Point(1,0,0)
p.pose.orientation = euler_to_quaternion(0,pi,0)
p2 = Point(0.7815,0,0)
p1 = get_fingertip(p)
self.assertTrue(abs(p1.point.x - p2.x) < 0.0001)
self.assertTrue(abs(p1.point.y - p2.y) < 0.0001)
self.assertTrue(abs(p1.point.z - p2.z) < 0.0001)
def add_ground(self, height=-0.01):
"""
Adds a big flat box to the planning scene.
:param height: z value
:type: float
"""
pose = PoseStamped()
pose.header.frame_id = "/odom_combined"
pose.pose.position = Point(0, 0, height)
pose.pose.orientation = Quaternion(0, 0, 0, 1)
box = self.make_box("ground" + str(height), pose, [3, 3, 0.01])
self.safe_objects.append(box.id)
self.add_object(box)
def __init__(self):
self.n = int
self.n = 0
""" GETTING PARAMS """
self.pose = rospy.get_param('~pose', 'amcl_pose')
self.path = rospy.get_param('~path', 'path')
self.bag_file_name = rospy.get_param(
'~bag_file_name',
'/home/arturo/rosbag_files/test_interfaces/arturo_voice_1.bag')
"""MEMBERS"""
self.path = Path()
self.stamped_pose = PoseStamped()
""" SUBSCRIBERS """
self.amcl_pose_subscriber = rospy.Subscriber(
self.pose, PoseWithCovarianceStamped, self.amcl_pose_callback)
""" PUBLISHERS """
self.path_publisher = rospy.Publisher(self.path, Path, queue_size=1)
def make_scan_pose(point, distance, angle, frame="/odom_combined", n=8):
"""
Calculates "n" positions around and pointing to "point" with "distance" in an "angle"
:param point: Point the positions will be pointing to
:type: Point
:param distance: distance from the point to tcp origin
:type: float
:param angle: pitch of the gripper, 0 = horizontally, pi/2 = downwards
:type: float
:param frame: the frame that the positions will have
:type: str
:param n: number of positions
:type: float
:return: list of scan poses
:type: [PoseStamped
"""
#TODO: assumes odom_combined
look_positions = []
alpha = pi/2 - angle
r = sin(alpha) * distance
h = cos(alpha) * distance
h_vector = Point()
h_vector.z = h
muh = add_point(point, h_vector)
for i in range(0, n):
a = 2 * pi * ((i + 0.0) / (n + 0.0))
b = a - (pi / 2)
look_point = Point(cos(a), sin(a), 0)
look_point = set_vector_length(r, look_point)
look_point = add_point(look_point, muh)
roll_point = Point(cos(b), sin(b), 0)
roll_point = add_point(roll_point, point)
look_pose = PoseStamped()
look_pose.header.frame_id = frame
look_pose.pose.orientation = three_points_to_quaternion(look_point, point, roll_point)
look_pose.pose.position = look_point
look_positions.append(look_pose)
look_positions.sort(key=lambda x: magnitude(x.pose.position))
return look_positions
def addConveyorBelt(self, msg):
print "addConveyorBelt callback"
# print msg
# zu 1
drop_point = msg.conveyor_belt.drop_center_point
print drop_point
# zu 2
conveyor_belt_width = msg.conveyor_belt.drop_deviation.y * 2
print conveyor_belt_width
# zu 3
conveyor_belt_height = drop_point.z
print conveyor_belt_height
# zu 4
conveyor_belt_length = msg.conveyor_belt.move_direction_and_length.y
print conveyor_belt_length
pose = PoseStamped()
pose.header.frame_id = "/odom_combined"
# h w d
# pose.pose.position = Point(drop_point.x,
# (drop_point.y - abs(conveyor_belt_length / 2)),
# drop_point.z - abs(conveyor_belt_height / 2))
# pose.pose.orientation = Quaternion(0, 0, 0, 1)
l = math.sqrt(msg.conveyor_belt.move_direction_and_length.x *
msg.conveyor_belt.move_direction_and_length.x +
conveyor_belt_length * conveyor_belt_length)
alpha = math.pi / 2 - math.asin(conveyor_belt_length / l)
wx = conveyor_belt_width * math.cos(alpha)
wy = conveyor_belt_width * math.sin(alpha)
w = wx + wy
pose.pose.position = Point(
drop_point.x, (drop_point.y - abs(conveyor_belt_length / 2)),
drop_point.z - abs(conveyor_belt_height / 2))
pose.pose.orientation = Quaternion(0, 0, 0, 1)
print pose
cb_size = [w * 2, l, conveyor_belt_height]
co = utils.manipulation.get_planning_scene().make_box(
"conveyor_belt", pose, cb_size)
utils.manipulation.get_planning_scene().add_object(co)
def make_grasp_pose(depth, gripper_origin, roll, frame_id):
"""
Calculates a Pose pointing from gripper_origin to direction.
:param depth: desired distance between gripper_origin and direction
:type: float
:param gripper_origin: origin of the gripper
:type: float
:param roll: point for roll
:type: Point
:param frame_id: frame_id
:type: str
:return: grasp pose
:type: PoseStamped
"""
grasp = PoseStamped()
grasp.header.frame_id = frame_id
grasp.pose.orientation = three_points_to_quaternion(gripper_origin, geometry_msgs.msg.Point(0, 0, 0), roll)
grasp.pose.position = multiply_point(depth, gripper_origin)
return grasp
def add_cam_mast(self, x, y):
"""
Adds a Cylinder where the cam mast is.
:param x: x coordinate
:type: float
:param y: y coordinate
:type: float
"""
pose = PoseStamped()
pose.header.frame_id = "/odom_combined"
pose.pose.position = Point(x, y, 0.55)
pose.pose.orientation = Quaternion(0, 0, 0, 1)
c1 = self.make_cylinder("cm1", pose, [1.1, 0.05])
self.safe_objects.append(c1.id)
self.add_object(c1)
pose.pose.position = Point(x, y, 1.0)
pose.pose.orientation = Quaternion(0, 0, 0, 1)
c1 = self.make_box("mast_cams", pose, [0.3, 0.3, 0.3])
self.safe_objects.append(c1.id)
self.add_object(c1)
def test2_1(self):
co = CollisionObject()
co.operation = CollisionObject.ADD
co.id = "muh"
co.header.frame_id = "/odom_combined"
cylinder = SolidPrimitive()
cylinder.type = SolidPrimitive.CYLINDER
cylinder.dimensions.append(0.3)
cylinder.dimensions.append(0.03)
co.primitives = [cylinder]
co.primitive_poses = [Pose()]
co.primitive_poses[0].position = Point(1.2185, 0,0)
co.primitive_poses[0].orientation = Quaternion(0,0,0,1)
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions.append(0.1)
box.dimensions.append(0.1)
box.dimensions.append(0.1)
co.primitives.append(box)
co.primitive_poses.append(Pose())
co.primitive_poses[1].position = Point(1.1185, 0,0)
co.primitive_poses[1].orientation = Quaternion(0,0,0,1)
co.primitives.append(box)
co.primitive_poses.append(Pose())
co.primitive_poses[2].position = Point(0, 0,0)
co.primitive_poses[2].orientation = Quaternion(0,0,0,1)
p = PoseStamped()
p.header.frame_id = "/odom_combined"
p.pose.position = Point(1,0,0)
p.pose.orientation = euler_to_quaternion(0,0,0)
self.assertEqual(get_grasped_part(co, get_fingertip(p))[1], 0)
if __name__ == '__main__':
rospy.init_node('brain')
move_base = MoveBase(True)
move_base.client.cancel_all_goals()
tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer)
goals = []
while not rospy.is_shutdown():
cmd = raw_input('[add|clear|replay|q] - ')
if cmd == 'add':
transform = tfBuffer.lookup_transform('map', 'base_footprint',
rospy.Time())
goal_pose = PoseStamped()
goal_pose.header.frame_id = transform.header.frame_id
goal_pose.pose.position.x = transform.transform.translation.x
goal_pose.pose.position.y = transform.transform.translation.y
goal_pose.pose.position.z = transform.transform.translation.z
goal_pose.pose.orientation = transform.transform.rotation
goals.append(goal_pose)
elif cmd == 'clear':
goals = []
elif cmd == 'replay':
for goal in goals:
rospy.loginfo('moving to: \n{}'.format(goal))
move_base.move_to(goal)
transform = tfBuffer.lookup_transform('map', 'base_footprint',
rospy.Time())
goal_diff(goal, transform)
def make6DofMarker(self, interaction_mode, root_link, tip_link):
def normed_q(x, y, z, w):
return np.array([x, y, z, w]) / np.linalg.norm([x, y, z, w])
int_marker = InteractiveMarker()
p = PoseStamped()
p.header.frame_id = tip_link
p.pose.orientation.w = 1
int_marker.header.frame_id = tip_link
int_marker.pose.orientation.w = 1
int_marker.pose = self.transformPose(root_link, p).pose
int_marker.header.frame_id = root_link
int_marker.scale = .2
int_marker.name = "eef_{}_to_{}".format(root_link, tip_link)
# insert a box
self.makeBoxControl(int_marker)
int_marker.controls[0].interaction_mode = interaction_mode
if interaction_mode != InteractiveMarkerControl.NONE:
control_modes_dict = {
InteractiveMarkerControl.MOVE_3D: "MOVE_3D",
InteractiveMarkerControl.ROTATE_3D: "ROTATE_3D",
InteractiveMarkerControl.MOVE_ROTATE_3D: "MOVE_ROTATE_3D"
}
int_marker.name += "_" + control_modes_dict[interaction_mode]
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 0, 0, 1)
control.name = "rotate_x"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(0, 0, 0, 1)
control.name = "move_x"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 1, 0, 1))
control.name = "rotate_z"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 1, 0, 1))
control.name = "move_z"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 0, 1, 1))
control.name = "rotate_y"
control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(control)
control = InteractiveMarkerControl()
control.orientation = Quaternion(*normed_q(0, 0, 1, 1))
control.name = "move_y"
control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(control)
return int_marker
'ur5_wrist_1_joint',
'ur5_wrist_2_joint',
'ur5_wrist_3_joint',
]
joint_state.position = [
0.0174419, -1.33713, 1.47922, -1.05844, -0.00214559, -0.660072
]
# 0.0138173009806,
# -0.948155685642,
# 1.64415424887,
# 0.833014565285,
# -0.00345735390984,
# -1.52754142152, ]
test.send_joint_goal(joint_state)
goal = PoseStamped()
goal.header.frame_id = 'gripper_tool_frame'
goal.pose.position.x = 0.1
goal.pose.position.z = -0.2
goal.pose.orientation.w = 1.0
test.send_cart_goal(goal)
goal = PoseStamped()
goal.header.frame_id = 'gripper_tool_frame'
goal.pose.position = Point(0, 0, 0)
goal.pose.orientation = Quaternion(0.0, 0.0, 0.70710678, 0.70710678)
test.send_cart_goal(goal)
goal = PoseStamped()
goal.header.frame_id = 'gripper_tool_frame'
goal.pose.position = Point(0, -0.1, 0)
def getPoseStamped(self, header=None):
if header is None:
header = Header(0, rospy.rostime.get_rostime(), "world")
return PoseStamped(header, self.getPose())
