def setup_class(cls): c30 = np.sqrt(3) / 2.0 s30 = -0.5 c60 = -s30 s60 = -c30 # create pose1 from transformation matrix m1 = np.eye(4) m1[0:3, 3] = np.asarray([0.8660254037844386, 0.5, 0.0]) m1[0:2, 0:2] = np.asarray([[c30, -s30], [s30, c30]]) cls.pose1 = Pose.from_transformation_matrix(m1) # create pose2 from transformation matrix m2 = np.eye(4) m2[0:3, 3] = np.asarray([0.25, -0.43301270189221935, 0.0]) m2[0:2, 0:2] = np.asarray([[c60, -s60], [s60, c60]]) cls.pose2 = Pose.from_transformation_matrix(m2) # create pose3 from transformation matrix cls.m3 = np.eye(4) cls.m3[0:3, 3] = np.asarray([0.8660254037844386, 0.0, 0.0]) cls.m3[0:2, 0:2] = np.asarray([[0.0, 1.0], [-1.0, 0.0]]) cls.pose3 = Pose.from_transformation_matrix(cls.m3) # create pose4 from transformation matrix translation = np.asarray([0.22419, -0.78420, 0.945699]) quaternion = Quaternion(x=0.6087561845779419, y=0.7779673933982849, z=0.047310106456279755, w=0.1481364518404007) cls.pose4 = Pose(translation, quaternion)
def project_pixel_on_plane(pixel_point: np.ndarray, plane_params: np.ndarray, camera_pose: Pose, camera_matrix: np.ndarray): """ Find pose of specifc image pixel Assumption is that the image content lies on a plane Parameters ---------- pixel_point : np.ndarray pixel point [x, y] plane_params : np.ndarray plane in general form ax + by + cz + d = 0 as numpy array [a,b,c,d] camera_pose : xamla_motion.data_types.Pose pose of camera in world coordinates camera_matrix : np.ndarray 3x3 camera matrix see opencv for more information """ cam_pose = camera_pose.transformation_matrix() # create ray in camera space htp = np.array([pixel_point[0], pixel_point[1], 1]) ray = np.matmul(np.linalg.inv(camera_matrix), htp) # transform form camera to world axis ray_world = np.matmul(cam_pose[0:3, 0:3], ray) eye = cam_pose[0:3, 3] at = eye + ray_world # compute intersection ray and plane t, hit = x3d.intersect_ray_plane(eye, at, plane_params) return hit
def setup_class(cls): cls.client = WorldViewClient() joint_set = JointSet('1,2,3') cls.joint_values_1 = JointValues(joint_set, 0.0) cls.joint_values_2 = JointValues(joint_set, [1, 2, 3]) cls.pose_1 = Pose.identity() cls.pose_2 = Pose.identity().translate([0.1, 0.2, 1.8]) cls.pose_3 = Pose.identity().translate([0.1, -0.2, 1.8]) cls.cartesian_path_1 = CartesianPath([cls.pose_1, cls.pose_2]) cls.cartesian_path_2 = CartesianPath([cls.pose_2, cls.pose_1]) collision_box_1 = CollisionPrimitive.create_box( 0.05, 0.05, 0.05, cls.pose_1 ) collision_box_2 = CollisionPrimitive.create_box( 0.05, 0.05, 0.05, cls.pose_2 ) collision_box_3 = CollisionPrimitive.create_box( 0.05, 0.05, 0.05, cls.pose_3 ) cls.collision_object_1 = CollisionObject([collision_box_1]) cls.collision_object_2 = CollisionObject( [collision_box_2, collision_box_3]) cls.folder_path = 'test/my_test' cls.joint_values_path = 'test_joint_values' cls.pose_path = 'test_pose' cls.cartesian_path = 'test_cartesian_path' cls.collision_object_path = 'test_collision_object'
def main(): # create move group instance move_group = MoveGroup() # get default endeffector of the movegroup end_effector = move_group.get_end_effector() # create cartesian path and equivalent joint path t1 = [0.502522, 0.2580, 0.3670] q1 = Quaternion(w=0.304389, x=0.5272, y=0.68704, z=0.39666) t2 = [0.23795, 0.46845, 0.44505] q2 = Quaternion(w=0.212097, x=0.470916, y=0.720915, z=0.462096) t3 = [0.6089578, 0.3406782, 0.208865] q3 = Quaternion(w=0.231852, x=0.33222, y=0.746109, z=0.528387) pose_1 = Pose(t1, q1) pose_2 = Pose(t2, q2) pose_3 = Pose(t3, q3) cartesian_path = CartesianPath([pose_1, pose_2, pose_3]) plan = end_effector.move_cartesian(cartesian_path, collision_check=True).plan() stepped_client = plan.execute_supervised() robot_chat = RobotChatClient() robot_chat_stepped_motion = RobotChatSteppedMotion(robot_chat, stepped_client, move_group.name) loop = asyncio.get_event_loop() register_asyncio_shutdown_handler(loop) try: loop.run_until_complete( robot_chat_stepped_motion.handle_stepwise_motions()) finally: loop.close()
def send_set_point(self, setPoint: Pose): """ Jogging to Pose Parameters ---------- setPoint : Pose The Pose to which the end effector should jog to """ pose_msg = setPoint.to_posestamped_msg() self._set_point_pub.publish(pose_msg)
async def imitate_move(end_effector, move_group): input( "Press enter to set the current position as reference start point, and start to listen to poses from client" ) start = end_effector.get_current_pose() with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s: s.bind((HOST, PORT)) s.listen() while True: conn = None try: conn, addr = s.accept() with conn: while True: data = conn.recv(1024) if not data: break pose_list = eval(data.decode("utf-8")) # print(pose_list) path = [] for pose in pose_list: coordinate = pose[:3] # The following line should be changed according to current pose and the frame of robot base coordinate[0], coordinate[1], coordinate[ 2] = coordinate[2], coordinate[0], coordinate[ 1] coordinate = np.array( coordinate) + start.translation quat_element = pose[3:7] # The following line should be changed to match the previous line of coordinate quat = Quaternion(quat_element[0], quat_element[3], quat_element[1], quat_element[2]) quat = quat * start.quaternion pose = Pose(coordinate, quat) path.append(pose) cartesian_path = CartesianPath(path) joint_path = end_effector.inverse_kinematics_many( cartesian_path, False).path move_joints = move_group.move_joints(joint_path) move_joints = move_joints.with_velocity_scaling(0.1) move_joints_plan = move_joints.plan() await move_joints_plan.execute_async() except KeyboardInterrupt: if conn: conn.close() break
def plane_parameters_from_pose(pose: Pose): """ Compute plane parameters in general form ax + by + cz + d = 0 Parameters ---------- pose : xamla_motion.data_types.Pose pose which plane in extracted z is normal axis """ pm = pose.transformation_matrix() z = normalize(pm[0:3, 2]) d = np.matmul(z, pm[0:3, 3]) return np.asarray([z[0], z[1], z[2], -d])
async def __call__(self, crop_boxes: List[CropBox], transform: Pose, shutter_speed_in_ms: int = 50): # TODO: Add timer # with Timer('computeCropBoxStatistics', self.logger): goal = CropBoxStatisticsGoal() goal.crop_boxes = crop_boxes goal.cloud_transform = transform.to_pose_msg() goal.shutter_speed_in_ms = float(shutter_speed_in_ms) responds = await self.client(goal) result = responds.result() if result.success == False: raise rospy.TransportException('Failed to get images') return result.number_of_points
def main(pose: Pose, xSize: int, ySize: int, xStepSize: float, yStepSize: float) -> List[Pose]: """ This function uses a pose to calculate a grid of poses Parameters ---------- pose : Pose Defines the position and orientation of the grid xSize : int Number of elements of the grid in x-direction ySize : int Number of elements of the grid in y-direction xStepSize : float The distance between the poses in x-direction yStepSize : float The distance between the poses in y-direction Returns ------ List[Pose] A list of generated poses """ # The point defines the rotation and is a corner of the grid rotation = pose.quaternion # Calculate the delta of each step in x- and y-direction # For that, we scale the unit vector pointing in x-direction/y-direction # and then rotate it by the rotation of the pose delta_x = rotation.rotate(np.array([xStepSize, 0.0, 0.0])) delta_y = rotation.rotate(np.array([0.0, yStepSize, 0.0])) poses = [] # type: List[Pose] for y_index in range(ySize): for x_index in range(ySize): translation = pose.translation + x_index * delta_x + y_index * delta_y poses.append(Pose(translation, rotation)) return poses
def test_cached_trajectories(tc_go_to: TaskTrajectoryCache, tc_come_back: TaskTrajectoryCache): world_view_client = WorldViewClient() move_group = example_utils.get_move_group() end_effector = move_group.get_end_effector() world_view_folder = "example_08_trajectory_cache" add_generated_folder(world_view_client, world_view_folder) # Get start and end from world view start_jv = world_view_client.get_joint_values("start_jv", world_view_folder) start_pose = end_effector.compute_pose(start_jv) end_pose = world_view_client.get_pose("end_pose", world_view_folder) # Get a SampleBox, which contains a Volume defined by translations and rotations end_box = get_sample_box(end_pose) # Show the grid in world view sample_positions = end_box.sample_positions for i in range(len(sample_positions[0])): translation = [ sample_positions[0][i], sample_positions[1][i], sample_positions[2][i] ] pose = Pose.from_transformation_matrix(np.eye(4)) pose = pose.translate(translation) pose = pose.rotate(end_box.quaternions[0]) # poses contains a list of poses with same translation, different rotation world_view_client.add_pose(element_name="pose_{}".format(i), folder_path=world_view_folder + "/generated", pose=pose) # Move to start_jv loop.run_until_complete(move_group.move_joints_collision_free(start_jv)) if tc_go_to is not None and tc_come_back is not None: world_view_client.add_pose(element_name="target_pose", folder_path=world_view_folder + "/generated", pose=end_pose) input( "Please move 'target_pose' in world view in folder 'example_08_trajectory_cache/generated', then press enter." ) target_pose = world_view_client.get_pose( element_name="target_pose", folder_path=world_view_folder + "/generated") try: # This movement uses the trajectory which ends nearest to target_pose print("Move to nearest pose") execution = move_with_trajectory_cache( cache=tc_go_to, end_effector=end_effector, start_joint_values=start_jv, target_pose=target_pose, max_position_diff_radius=0.05) loop.run_until_complete(execution) # This movement uses the trajectory which begins nearest to target_pose print("Return home") execution = move_with_trajectory_cache( cache=tc_come_back, end_effector=end_effector, start_joint_values=None, target_pose=start_pose, max_position_diff_radius=0.05) loop.run_until_complete(execution) except RuntimeError as e: print(e) print( "The chosen pose must lie in the radius of one of the poses in the used SampleVolume " ) else: print("Could not open serialized data. ") input("Press enter to clean up") world_view_client.remove_element("generated", world_view_folder)
def main(): # create move group instance move_group = MoveGroup() # get default endeffector of the movegroup end_effector = move_group.get_end_effector() # create cartesian path and equivalent joint path t1 = [0.502522, 0.2580, 0.3670] q1 = Quaternion(w=0.304389, x=0.5272, y=0.68704, z=0.39666) t2 = [0.23795, 0.46845, 0.44505] q2 = Quaternion(w=0.212097, x=0.470916, y=0.720915, z=0.462096) t3 = [0.6089578, 0.3406782, 0.208865] q3 = Quaternion(w=0.231852, x=0.33222, y=0.746109, z=0.528387) pose_1 = Pose(t1, q1) pose_2 = Pose(t2, q2) pose_3 = Pose(t3, q3) cartesian_path = CartesianPath([pose_1, pose_2, pose_3]) joint_path = end_effector.inverse_kinematics_many(cartesian_path, False).path loop = asyncio.get_event_loop() register_asyncio_shutdown_handler(loop) async def example_moves(): print('test MoveGroup class') print('---------------- move joints -------------------') move_joints = move_group.move_joints(joint_path) move_joints = move_joints.with_velocity_scaling(0.1) move_joints_plan = move_joints.plan() await move_joints_plan.execute_async() print('---------------- move joints supervised -------------------') move_joints = move_joints.with_velocity_scaling(0.5) stepped_motion_client = move_joints.plan().execute_supervised() await run_supervised(stepped_motion_client) print('---------------- move joints collision free -------------------') move_joints_cf = MoveJointsCollisionFreeOperation( move_joints.to_args()) await move_joints_cf.plan().execute_async() print('test EndEffector class') print('---------------- move cartesian -------------------') move_cartesian = end_effector.move_cartesian(cartesian_path) move_cartesian = move_cartesian.with_velocity_scaling(0.4) move_cartesian_plan = move_cartesian.plan() await move_cartesian_plan.execute_async() print('---------------- move cartesian supervised -------------------') move_cartesian = move_cartesian.with_velocity_scaling(0.8) stepped_motion_client = move_cartesian.plan().execute_supervised() await run_supervised(stepped_motion_client) print('---------------- move cartesian collision free -------------------') move_cartesian_cf = MoveCartesianCollisionFreeOperation( move_cartesian.to_args() ) await move_cartesian_cf.plan().execute_async() try: loop.run_until_complete(example_moves()) finally: loop.close()
def runTooltipCalib(self): print('Please move robot to capture pose. Then press \'Enter\' and wait.') sys.stdin.read(1) sys.stdin.read(1) if not os.path.isdir(self.storage_folder) : os.makedirs(self.storage_folder) image_left = None image_right = None if not self.offline : # capture images of calibration target and write them into storage folder images = self.capture_client(self.exposure_time) image_left = cv.cvtColor(images[self.left_serial], cv.COLOR_GRAY2RGB) image_right = cv.cvtColor(images[self.right_serial], cv.COLOR_GRAY2RGB) cv.imwrite(os.path.join(self.storage_folder, 'left.png'), image_left) cv.imwrite(os.path.join(self.storage_folder, 'right.png'), image_right) else : # offline version: read images of calibration target image_left = cv.imread(os.path.join(self.storage_folder, 'left.png')) image_right = cv.imread(os.path.join(self.storage_folder, 'right.png')) current_posture_1 = None current_pose_1 = None if not self.offline : # store robot poses into storage folder current_posture_1 = self.move_group.get_current_joint_positions() current_pose_1 = self.move_group.motion_service.query_pose(self.move_group.name, current_posture_1, self.link_name) with open(os.path.join(self.storage_folder, 'posture_1.p'), 'wb') as f: pickle.dump(current_posture_1, f) with open(os.path.join(self.storage_folder, 'pose_1.p'), 'wb') as f: pickle.dump(current_pose_1, f) else : # offline version: read posture and poses with open(os.path.join(self.storage_folder, 'posture_1.p'), 'rb') as f: current_posture_1 = pickle.load(f) with open(os.path.join(self.storage_folder, 'pose_1.p'), 'rb') as f: current_pose_1 = pickle.load(f) self.pattern_localizer.setStereoCalibration(self.stereocalib) # self.stereocalib_sensorhead_cams camPoseFinalList, circleGridPointsLeft, circleGridPointsRight, pointsInCamCoords = self.pattern_localizer.calcCamPoseViaPlaneFit(image_left, image_right, "left", False, True) if (circleGridPointsLeft == None or circleGridPointsRight == None) : print("Not all 4 patterns have been found in both camera images.") print("-> One of the patterns may lie too near to the image boundary, such that it is cropped after image rectification.") print("-> The images might be too dark, i.e. exposure time might be too low, ...") print("-> Run pattern search in debug mode to analyze problem:") camPoseFinalList, circleGridPointsLeft, circleGridPointsRight, pointsInCamCoords = self.pattern_localizer.calcCamPoseViaPlaneFit(image_left, image_right, "left", True, True) return False elif (len(circleGridPointsLeft) != 4 or len(circleGridPointsRight) != 4) : print("Not all 4 patterns have been found in both camera images.") print("-> One of the patterns may lie too near to the image boundary, such that it is cropped after image rectification.") print("-> The images might be too dark, i.e. exposure time might be too low, ...") print("-> Run pattern search in debug mode to analyze problem:") camPoseFinalList, circleGridPointsLeft, circleGridPointsRight, pointsInCamCoords = self.pattern_localizer.calcCamPoseViaPlaneFit(image_left, image_right, "left", True, True) return False Hc1 = camPoseFinalList["1"] Hc2 = camPoseFinalList["3"] Hc3 = camPoseFinalList["5"] Hc4 = camPoseFinalList["7"] print("camPoseFinalList[\"1\"]:") print(Hc1) print("camPoseFinalList[\"3\"]:") print(Hc2) print("camPoseFinalList[\"5\"]:") print(Hc3) print("camPoseFinalList[\"7\"]:") print(Hc4) print("\n") Hg_Pose = deepcopy(current_pose_1) H_Pose = deepcopy(self.hand_eye) # hand_eye is torso_eye (= torso_to_cam) here! H = deepcopy(H_Pose.transformation_matrix()) Hg = deepcopy(Hg_Pose.transformation_matrix()) print("Hg (torso pose):") print(Hg) print("H (torso_to_cam):") print(H) print("\n") t_mat1 = np.matmul(Hg, np.matmul(H, Hc1)) t_mat2 = np.matmul(Hg, np.matmul(H, Hc2)) t_mat3 = np.matmul(Hg, np.matmul(H, Hc3)) t_mat4 = np.matmul(Hg, np.matmul(H, Hc4)) print("Pose of pattern with id 1 in base coordinates:") print(t_mat1) print("Pose of pattern with id 3 in base coordinates:") print(t_mat2) print("Pose of pattern with id 5 in base coordinates:") print(t_mat3) print("Pose of pattern with id 7 in base coordinates:") print(t_mat4) print("\n") t1 = np.zeros(shape=2, dtype=np.float64) t2 = np.zeros(shape=2, dtype=np.float64) t3 = np.zeros(shape=2, dtype=np.float64) t4 = np.zeros(shape=2, dtype=np.float64) t1[0] = t_mat1[0][3] t1[1] = t_mat1[1][3] t2[0] = t_mat2[0][3] t2[1] = t_mat2[1][3] t3[0] = t_mat3[0][3] t3[1] = t_mat3[1][3] t4[0] = t_mat4[0][3] t4[1] = t_mat4[1][3] def calc_intersection(p1,p2,q1,q2) : x1 = deepcopy(p1[0]) y1 = deepcopy(p1[1]) x2 = deepcopy(p2[0]) y2 = deepcopy(p2[1]) a1 = deepcopy(q1[0]) b1 = deepcopy(q1[1]) a2 = deepcopy(q2[0]) b2 = deepcopy(q2[1]) zaehler = -y1 + b1 - (((a1-x1)/(x2-x1)) * (y2-y1)) nenner = (((a2-a1)/(x2-x1)) * (y2-y1)) - (b2-b1) quotient = zaehler / nenner s1 = a1 + quotient * (a2-a1) s2 = b1 + quotient * (b2-b1) s = np.zeros(shape=2, dtype=np.float64) s[0] = s1 s[1] = s2 return s intersection = calc_intersection(t1,t3,t2,t4) print("=> Position of cross lines:") cross_pos = np.zeros(shape=3, dtype=np.float64) cross_pos[0] = intersection[0] cross_pos[1] = intersection[1] cross_pos[2] = 0.25 * (t_mat1[2][3] + t_mat2[2][3] + t_mat3[2][3] + t_mat4[2][3]) print(cross_pos) print("\n") print('Please move tooltip straight down to cross lines. Then press \'Enter\'.') sys.stdin.read(1) current_posture_2 = None current_pose_2 = None if not self.offline : # store robot pose into storage folder current_posture_2 = self.move_group.get_current_joint_positions() current_pose_2 = self.move_group.motion_service.query_pose(self.move_group.name, current_posture_2, self.flange_link_name) with open(os.path.join(self.storage_folder, 'posture_2.p'), 'wb') as f: pickle.dump(current_posture_2, f) with open(os.path.join(self.storage_folder, 'pose_2.p'), 'wb') as f: pickle.dump(current_pose_2, f) else : # offline version: read posture and poses with open(os.path.join(self.storage_folder, 'posture_2.p'), 'rb') as f: current_posture_2 = pickle.load(f) with open(os.path.join(self.storage_folder, 'pose_2.p'), 'rb') as f: current_pose_2 = pickle.load(f) Hg = deepcopy(current_pose_2.transformation_matrix()) cross_pose = deepcopy(current_pose_2.transformation_matrix()) cross_pose[0][3] = cross_pos[0] cross_pose[1][3] = cross_pos[1] cross_pose[2][3] = cross_pos[2] print("cross_pose:") print(cross_pose) cross_pose_worldview = Pose.from_transformation_matrix(matrix=cross_pose, frame_id='world', normalize_rotation=False) self.addOrUpdatePose("cross_pose", "/Calibration", cross_pose_worldview) hand_tooltip = np.matmul(inv(Hg), cross_pose) print("Tooltip (grippertip) pose in flange coordinates (with same rotation as flange):") print(hand_tooltip) print("\n") np.save(os.path.join(self.storage_folder, 'tooltip_pose_in_flange_coordinates.npy'), hand_tooltip) print("To relocate the TCP in the 3D View, add a \'tcp_link\' to the file \'robotModel/main.xacro\' of your project folder.") print("As \'origin xyz\' of your tcp_link choose the translation vector of your calculated tooltip pose in flange coordinates.") print("More precisely, you have to add the following lines inside the <robot> </robot> environment: \n") print('<link name=\"tcp_link\" />') print('<joint name=\"tcp_joint_F\" type=\"fixed\">') print(' <child link=\"tcp_link\" />') print(' <parent link=\"e.g. arm_left_link_tool0\" /> <- adapt this to your endeffector link name') print(' <origin xyz=\"{:f} {:f} {:f}\" rpy=\"0 0 0\" />'.format(hand_tooltip[0][3], hand_tooltip[1][3], hand_tooltip[2][3])) print('</joint>') print("\n") print("Now, compile the new main.xacro.") print("Then, in the \'Configuration View\' once press compile to make the \'tcp_link\' selectable.") print("In the \'Configuration View\' under \'MotionPlanning\'->\'MoveIt!\'->\'groups\'->\'<group_name>\' select the new \'tcp_link\' as \'Tip link\'.") print("Moreover, under \'MotionPlanning\'->\'MoveIt!\'->\'endEffectors\'->\'<end effector name>\' select the new \'tcp_link\' as \'Parent link\'.") print("After compiling the new robot configuration, starting ROS, changing into the 'World View' and choosing the corresponding move group and end effector, the interactive marker appears at the new tcp link.") return True
The distance between the poses in y-direction Returns ------ List[Pose] A list of generated poses """ # The point defines the rotation and is a corner of the grid rotation = pose.quaternion # Calculate the delta of each step in x- and y-direction # For that, we scale the unit vector pointing in x-direction/y-direction # and then rotate it by the rotation of the pose delta_x = rotation.rotate(np.array([xStepSize, 0.0, 0.0])) delta_y = rotation.rotate(np.array([0.0, yStepSize, 0.0])) poses = [] # type: List[Pose] for y_index in range(ySize): for x_index in range(ySize): translation = pose.translation + x_index * delta_x + y_index * delta_y poses.append(Pose(translation, rotation)) return poses if __name__ == '__main__': # Called when running this script standalone translation = [0.6089578, 0.3406782, 0.208865] rotation = Quaternion(w=0.231852, x=0.33222, y=0.746109, z=0.528387) pose = Pose(translation, rotation) poses = main(pose, 3, 3, 0.05, 0.05) print(poses)
def main(xSize: int, ySize: int, xStepSize: float , yStepSize: float): """ The parameter for this function describes the size of the grid. To manipulate orientation and position, one can alter the Pose named "GridPose" in the folder "example_02_palletizing" of the world view. Parameters ---------- xSize : int Number of elements of the grid in x-direction ySize : int Number of elements of the grid in y-direction xStepSize : float The distance between the poses in x-direction yStepSize : float The distance between the poses in y-direction """ world_view_folder = "example_02_palletizing" # Create move group instance targeting the right arm of the robot move_group = example_utils.get_move_group() # Get the gripper attached at the right arm wsg_gripper = example_utils.get_gripper(move_group) # Create a instance of WorldViewClient to get access to rosvita world view world_view_client = WorldViewClient() # Generate the folders used by this example in world vew generate_folders(world_view_client) # Get the pose of the position which defines the location and rotation of the grid pose = world_view_client.get_pose("GridPose", world_view_folder) jv_home = world_view_client.get_joint_values("Home", world_view_folder) # Get the target place poses poses = example_02_1_generate_grid.main(pose, xSize, ySize, xStepSize, yStepSize) rotation = pose.quaternion # Calculate the orthogonal vector of the plane we want to span # Since we use [1,0,0] and [0,1,0] vectors to span the grid relatively to the # pose orientation, [0,0,1] is orthogonal to the grid orthogonal = rotation.rotate(np.array([0,0,1])) # For visualization and possible collisions, add some boxes below the positions # we want to visit getBoxPose = lambda pose : Pose(pose.translation + (orthogonal * (0.02)), pose.quaternion) boxPoses = list(map(getBoxPose, poses)) boxes = example_02_2_create_collision_boxes.main(boxPoses, (xStepSize*0.9, yStepSize*0.9, 0.01)) world_view_client.add_collision_object("collision_matrix", "/{}/generated/collision_objects".format(world_view_folder), boxes) # Now calculate the pre place poses, which hover over the desired place poses # Since we want to access every pose in the grid "from above", we apply a # translation orthogonal to the place poses for the pre place poses #func = lambda pose : Pose(pose.translation + (orthogonal * (-0.1)), pose.quaternion) func = lambda pose : Pose(pose.translation + (pose.quaternion.rotate(np.array([0,0,1])) * (-0.1)), pose.quaternion) pre_place_poses = list(map( func , poses)) # Now that we have all the poses we want to visit, we should find the # corresponding joint values pre_place_jvs = calculate_pre_place_joint_values(pre_place_poses, jv_home, move_group, world_view_client, world_view_folder) example_02_4_pick_place_poses.main(poses, pre_place_jvs, jv_home, move_group) world_view_client.remove_element("generated", world_view_folder)
def runPatternCalibration(self): # determined by measuring using a ruler offset_to_edge = Pose([-0.0145, -0.0142, 0]) edge_to_screw_center_table = Pose([0.045, 0.045, 0.02333]) return offset_to_edge + edge_to_screw_center_table
# Read names too to associate the joint values to the poses names = list(map( lambda posix_path: posix_path.name, list(joint_values_dict.keys()))) # Create no names for joint values new_names = list(map( lambda name: "shifted_joint_values_of_{}".format(name), names)) # Read poses from world view reference_pose = world_view_client.get_pose("Reference", "/{}/poses".format(world_view_folder)) shift_pose = world_view_client.get_pose("Shift", "/{}/poses".format(world_view_folder)) # Create a pose defining the shift from the reference_poses view # First undo translation and rotation of reference pose # then translate and rotate by the shift pose diff_pose = Pose( -reference_pose.translation, reference_pose.quaternion.inverse)\ .translate(shift_pose.translation).rotate(shift_pose.quaternion) shifted_joint_values_list = main(joint_values_list, diff_pose) # Save the shifted joint values to world view print("Save the shifted joint values to world view") try: # delete folder if it already exists world_view_client.remove_element("generated", world_view_folder) except Exception as e: None world_view_client.add_folder("generated", world_view_folder) for i in range(len(shifted_joint_values_list)): joint_values = shifted_joint_values_list[i] # Test if not "None" if joint_values: name = new_names[i]
def main(): world_view_client = WorldViewClient() input_var = input("Please type in the torch filename (with path) of the sensorhead stereo camera calibration (e.g. \"calibration/torso_cameras/stereo_cams_<serial1>_<serial2>.t7\" ): ") cam_calib_fn = str(input_var) stereocalib_sensorhead_cams = torchfile.load(cam_calib_fn) end_of_right_serial = cam_calib_fn.rfind(".") start_of_right_serial = cam_calib_fn.rfind("_") right_serial = cam_calib_fn[start_of_right_serial+1:end_of_right_serial] end_of_left_serial = start_of_right_serial start_of_left_serial = cam_calib_fn.rfind("_", 0, end_of_left_serial) left_serial = cam_calib_fn[start_of_left_serial+1:end_of_left_serial] print("right_serial:") print(right_serial) print("left_serial:") print(left_serial) print("Please type in the exposure time of the cameras [in microseconds],") input_var = input("or press \'enter\' to use 120000ms:") if input_var == "" : exposure_time = 120000 else : exposure_time = int(input_var) print("exposure_time:") print(exposure_time) print(type(exposure_time)) move_group_names = MotionService.query_available_move_groups() move_group_name = move_group_names[2].name # left_arm_torso flange_link_name = move_group_names[2].end_effector_link_names[0] # arm_left_link_tool0 print("For which arm will the tooltip be calibrated? Please type l or r, then press \'Enter\'.") print("l: left arm") print("r: right arm") which = sys.stdin.read(1) if which == "r" : move_group_name = move_group_names[3].name # right_arm_torso flange_link_name = move_group_names[3].end_effector_link_names[0] # arm_right_link_tool0 print("move_group_name:") print(move_group_name) move_group = MoveGroup(move_group_name) print("flange_link_name") print(flange_link_name) torso_link_name = "torso_link_b1" print("torso_link_name:") print(torso_link_name) last_slash = cam_calib_fn.rfind("/") torso_to_cam_fn = cam_calib_fn[:last_slash] + "/LeftCam_torso_joint_b1.t7" print("Please type in the name of the torchfile (with path) containing the transformation between torso joint and torso cameras,") input_var = input("or press \'enter\' to use " + torso_to_cam_fn) if input_var != "" : torso_to_cam_fn = str(input_var) print("torso_to_cam_fn:") print(torso_to_cam_fn) torso_to_cam_t7 = torchfile.load(torso_to_cam_fn) torso_to_cam = Pose.from_transformation_matrix(matrix=torso_to_cam_t7, frame_id=torso_link_name, normalize_rotation=False) pattern_localizer = patternlocalisation.PatternLocalisation() pattern_localizer.circleFinderParams.minArea = 50 pattern_localizer.circleFinderParams.maxArea = 1000 pattern_localizer.setPatternData(8, 11, 0.003) pattern_localizer.setStereoCalibration(stereocalib_sensorhead_cams) #pattern_localizer.setFindCirclesGridFlag(cv.CALIB_CB_ASYMMETRIC_GRID) # Don't use "cv.CALIB_CB_CLUSTERING", because it's much more sensitive to background clutter. pattern_localizer.setFindCirclesGridFlag(cv.CALIB_CB_ASYMMETRIC_GRID + cv.CALIB_CB_CLUSTERING) world_view_folder = '/Calibration' storage_folder = '/tmp/calibration/storage_tooltipcalib' offline = False tooltip_calib = TooltipCalibration(pattern_localizer, stereocalib_sensorhead_cams, left_serial, right_serial, exposure_time, world_view_client, world_view_folder, move_group, torso_to_cam, storage_folder, torso_link_name, flange_link_name, offline) tooltip_calib.runTooltipCalib()
def main(): # create entities which should be added to and manipulated in world view joint_set1 = JointSet(['Joint1', 'Joint2', 'Joint3']) joint_values1 = JointValues(joint_set1, [1.0, 2.0, 3.0]) joint_set2 = JointSet(['Joint1', 'Joint2']) joint_values2 = JointValues(joint_set2, [19.0, 20.0]) joint_set3 = JointSet(['Joint1', 'Joint4']) joint_values3 = JointValues(joint_set3, [100.0, 30.0]) t1 = [0.502522, 0.2580, 0.3670] q1 = Quaternion(w=0.304389, x=0.5272, y=0.68704, z=0.39666) t2 = [0.23795, 0.46845, 0.44505] q2 = Quaternion(w=0.212097, x=0.470916, y=0.720915, z=0.462096) t3 = [0.6089578, 0.3406782, 0.208865] q3 = Quaternion(w=0.231852, x=0.33222, y=0.746109, z=0.528387) pose1 = Pose(t1, q1, 'world') pose2 = Pose(t2, q2, 'world') pose3 = Pose(t3, q3, 'world') cartesian_path1 = CartesianPath([pose1, pose2, pose3]) sphere = CollisionPrimitive.create_sphere(0.1, pose2) cylinder = CollisionPrimitive.create_cylinder(0.4, 0.05, pose2) box = CollisionPrimitive.create_box(0.2, 0.2, 0.1, pose1) plane = CollisionPrimitive.create_plane(1.0, 1.0, 1.0, 1.0, pose3) cone = CollisionPrimitive.create_cone(0.2, 0.2, pose3) collision_object1 = CollisionObject([box]) collision_object2 = CollisionObject([cylinder]) collision_object3 = CollisionObject([plane, cone]) # create a instance of WorldViewClient to get access to rosvita world view world_view_client = WorldViewClient() print('---------------- add folder --------------') # add folder test at WorldView root/test world_view_client.add_folder('test/joint_values') world_view_client.add_folder('test/poses') world_view_client.add_folder('test/cartesian_paths') world_view_client.add_folder('test/collision_objects') print('---------------- add joint values --------------') world_view_client.add_joint_values( 'test/joint_values/joint_values1', joint_values1 ) world_view_client.add_joint_values( 'test/joint_values/test', joint_values2 ) print('---------------- update joint values --------------') world_view_client.add_joint_values( 'test/joint_values/joint_values1', joint_values2 ) print('---------------- get joint values --------------') get_value = world_view_client.get_joint_values( 'test/joint_values/joint_values1' ) print('joint_values1 is: ' + str(get_value)) print('---------------- query joint values --------------') # query all values which start with t under test/joint_values queried_values = world_view_client.query_joint_values( 'test/joint_values', 't' ) for v in queried_values: print(str(v)) print('---------------- add poses --------------') world_view_client.add_pose( 'test/poses/pose1', pose1 ) world_view_client.add_pose( 'test/poses/test', pose3 ) print('---------------- update pose --------------') world_view_client.add_pose( 'test/poses/pose1', pose2 ) print('---------------- get pose --------------') get_value = world_view_client.get_pose( 'test/poses/pose1' ) print('pose1 is: ' + str(get_value)) print('---------------- query poses --------------') # query all poses which start with t under test/pose queried_values = world_view_client.query_poses('test/poses', 't') for v in queried_values: print(str(v)) print('---------------- add cartesian path --------------') world_view_client.add_cartesian_path( 'test/cartesian_paths/cartesian_path1', cartesian_path1 ) world_view_client.add_cartesian_path( 'test/cartesian_paths/test', cartesian_path1 ) print('---------------- update cartesian path --------------') world_view_client.add_cartesian_path( 'test/cartesian_paths/cartesian_path1', cartesian_path1 ) print('---------------- get cartesian path --------------') get_value = world_view_client.get_cartesian_path( 'test/cartesian_paths/cartesian_path1' ) print('cartesian_path1 is: ' + str(get_value)) print('---------------- query cartesian paths --------------') # query all cartesian_paths which start with t under test/cartesian_path queried_values = world_view_client.query_cartesian_paths( 'test/cartesian_paths', 't' ) for v in queried_values: print(str(v)) print('---------------- add collision object --------------') world_view_client.add_collision_object( 'test/collision_objects/collision_object1', collision_object1 ) world_view_client.add_collision_object( 'test/collision_objects/test', collision_object1 ) print('---------------- update collision object --------------') world_view_client.add_collision_object( 'test/collision_objects/collision_object1', collision_object2 ) print('---------------- get collision object --------------') get_value = world_view_client.get_collision_object( 'test/collision_objects/collision_object1' ) print('collision_object1 is: ' + str(get_value)) print('---------------- query collision object --------------') # query all collision_objects which start with t under test/collision_object queried_values = world_view_client.query_collision_objects( 'test/collision_objects', 't' ) for v in queried_values: print(str(v)) input('Press enter to clean up') print('----------------- remove folder test ---------------') world_view_client.remove_element('test', '')
def test_rotate(self): m = self.m3.copy() m[0:3, 0:3] = np.eye(3) p = Pose.from_transformation_matrix(m) r_pose = p.rotate(self.m3[0:3, 0:3]) assert r_pose.transformation_matrix() == pytest.approx(self.m3)